mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   $ D% J( _! g' K4 @9 U* g8 I
2. #include <MeAuriga.h>
   & o! ?3 |' X1 @# N) F4 T
3. #include "MeLineFollowerArray.h"
   
4. 
   0 {4 }/ Y& ~- U! X
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   , ]/ V1 R% @* U5 t1 d" e: |- z* e" I
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    7 }/ K/ R3 o6 F$ g& W) ~! N" O- w
12. #define Error1 1
    
13. #define Error2 2
    : u' c; \" t1 l: E' h! i
14. #define Error3 3
    
15. 
    ( t% d) Z) R4 }3 P5 C& v; Q
16. #define Kp 15
    / U) n. G( b) m" ^8 V
17. #define Ki 0.15
    
18. #define Kd 0.03
    - h; N* Q% x" x# l% J7 h2 V- I6 i
19. 
    
20. uint8_t sData;
    * z+ q9 Z" P$ B( A' U2 H- d
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    " z6 U2 s* o' [0 m7 X9 F1 m0 X  Y
26. uint8_t D6;
    : y* d4 X6 s7 Q& S
27. 
    5 L' w) B8 H% R
28. float previous_error = 0;
    ( Y2 O" o1 E0 N4 g
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    9 ]) |, F; S5 b5 G% x* l
32. float output;
    
33. 
    
34. byte Left;
    
35. 
    ; \2 o! R8 F7 m" j9 Y7 B
36. void setup()
    
37. {
    3 A2 Y$ }! e5 L' ^, y8 E
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    5 ^" b) D  W4 s( x* y2 |
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    % L. x! n5 \& d" ~4 E  e! z
46. 
    0 ~9 H7 b, M' F2 [& ]
47. void loop()
    0 K; |% x2 ]' a0 I' R7 P
48. {
    0 g* e) ]  `9 j
49.   while(!((lightsensor_1.read()) < (10)));
    & o+ S$ X) ?- X, J/ u7 D8 L
50.   buzzer.tone(1047, 500);
    ! v9 x7 h0 Z- ~1 Q; V
51.   delay(500);
    $ D2 c2 A$ N1 y" n8 [) k/ `; J
52.   do
    7 \6 X0 c/ a0 \2 b1 p* e) ]( V; Y
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    , B3 J" {2 b5 u% C6 m* {2 g3 r9 u
56.    {
    9 H! G, o, C- m  y
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    " R9 s0 w& l4 T3 d: ~9 S+ M
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    3 w0 u( _1 J$ u% R1 W/ m
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    1 _" L1 \+ I$ C9 o
73.      }
    
74.      else
    - M% m' g+ a  J+ V* i& r+ m9 L
75.      {
    + O7 S  Q& i# h& J. n
76.        Speed -= 5;
    
77.        moto(Speed,0);
    3 t  |: \+ R2 L) B& U+ Y- T# ^# [  P- @
78.        do
    . {) p+ l, s: j
79.        {
    / v$ R' e( g* K5 V- H7 P) v
80.          Err = getErr();
    5 {' v+ N3 k1 P* ~) g  _
81.        }while((D1+D6) == 0);
    
82.      }
    ( T5 ?4 C9 {, o7 V) `
83.    }
    
84.    else
    
85.    {
    1 ~6 q& m- p/ v) z9 k7 {, `
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    * s+ I' c! }; K% [7 \* h
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    $ _2 B. v9 j& M- ^; x8 o
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    + O( A1 y6 c; A  t8 q3 i9 ^! V
92.      previous_error = Err;
    
93.    }
    ( ?  d* d0 Q; e3 o
94.   }while(!((lightsensor_2.read()) < (10)));
    " F6 p+ c4 x: \' I& [# n3 a3 ~2 \6 _
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    ) m; m5 Z4 a; K* J$ Y. d# \' a, k0 Q1 K
98. }
    1 O" B% |/ {* f- b8 X7 y8 v* w
99. 
    
100. int getErr()
     ' g5 r0 X' ~) |, i3 r  R2 N
101. {  
     
102.    sData = linefollower.getValue();
     8 I3 ~& [; l- }
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     3 h4 d5 R7 d, o. `0 `
106.    D4 = ~(sData>>3)&1;
     1 T( q$ Q- i; P1 V
107.    D5 = ~(sData>>4)&1;
     ( h4 O6 p1 M" r4 ]. w8 |
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     ) D2 ~8 L) f+ C& ?  Y( ?! W5 s
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     3 Q9 h1 d9 A! G* k
112.    {
     + w) ?: l' i4 \/ b& m& ~
113.      return 99;
     * ?8 h' ?! u  S( K9 E' }) x
114.    }
     
115.    else
     2 M- m4 _" J2 {* a5 {
116.    {
     
117.     return upD/downD;
     
118.    }
     0 _  |+ g/ M3 k0 H- T
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     , d( u( @( o1 E5 p2 I/ c
122. {
     " a8 W) L. C- H/ v
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

7 G5 Z9 ?8 C0 d  G/ r& R! l4 cMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   : g0 ~  j% a0 a" k* g" L
2. 
   2 S' `4 A2 L. ~) ~3 V
3. #ifdef ME_PORT_DEFINED
   0 r7 Z' P* y6 G6 V9 _7 I2 c3 `& y
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   3 w! T- \# v. M' c+ U' ~
5. {
   
6. 
   ' R8 \) Z9 |) R; f3 z2 a, @
7. }
   ' z/ Z- ?3 L) {6 R% _
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   , d0 L6 ?% C1 h" k' d
9. {
   ' M' a7 {$ h# a
10.     _DataPin = mePort[port].s2;
    # {4 ]0 Z- f( L- [* L5 X2 j
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    . m1 O, ?, u4 t/ D3 G: _& @. K
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    / E# K# {* J. I
16. {
    
17.     _DataPin = pin;
    . o( Z* s# N9 H
18.     pinMode(_DataPin, OUTPUT);
    7 b8 |- u5 b  H; d6 b- C; V- m
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    # d" C, q+ z% W7 B5 B
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    / _( q$ Z/ z+ f& z5 b: j
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    # u% d* T, C! T, D. B. w
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    - Y  E9 B" z. ^2 N
32.     #endif
    
33. }
    
34. 
    " O' K. L4 d7 ~7 h  s5 c( L
35. uint8_t MeLineFollowerArray::getValue()
    # D2 w5 v) [# j8 j( d
36. {
    
37.     uint32_t LOW_level_read_time;
    : n6 {: I2 D$ g6 j7 S+ `
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    $ C) M- p. N# `# k" P5 m1 P! G
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    , d( L9 A9 ]8 o7 I+ M
42. 
    3 R8 K4 B" |: \7 z9 Z( T+ j' }( B
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    - z0 R  }) l- f, s
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    7 c0 V0 ~7 O$ ]: i! E' s! r
49.     delayMicroseconds(10);
    
50. 
    ; f( m4 _0 M4 e2 i
51.     time_out_flag = millis();
    . b1 w: f! p( U, d
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    1 G& C# @, h, @) T) k
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    % `& X. ?8 ?" {7 ]& c9 f
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    * r' v, ~2 }5 ~7 Q1 a/ D4 P
61. 
    1 W% ~) ^9 r. l8 H% f$ j
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    , i6 C, V: X6 E) k7 U+ |& _5 X
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ! ~; V# U8 }. G* B/ ^' @2 {- [
66.     {
    7 g2 ^" E& w: Q6 Y+ ]8 m
67.         return 0xff;
    9 B3 L8 b0 \0 g- c9 B9 M4 p
68.     }
    1 o' F" g: O. c1 b$ l0 s6 _! f
69. 
    1 A/ p/ M8 W! ]4 B1 r
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    ; A% _' a8 K2 A' k
71.     {
    
72.         return 0xff;
    6 D2 S/ ], l' H+ R- v
73.     }
    
74. 
    ( m  z$ Y3 d. r, W( Y' [% }8 T
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    - g: x* L2 Z4 X
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    ; R+ C* P% o/ ^& j) z( f
81.         return 0xff;
    
82.     }
    ) B& Z) V  g6 s) j) R7 Q# E4 o
83. 
    : O2 u: v; u9 j/ |
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    ' l) z0 W$ ?4 a1 Y4 S
85.     {
    
86.         return 0xff;
    ; H* j2 U4 k# J7 b0 ~' P* N( |& F
87.     }
    
88. 
    1 I8 A% m' |! q% G8 N# z1 s; Y1 n
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    7 i! ~! g1 s5 w: `3 C
91.         Sensor_Data[k] = 0x00;
    
92. 
    $ h( G# w8 O2 V- d0 L& ~# X
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    & u, f, G# X3 k9 E! r, |6 B
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    ; Y. r/ A" f& h) R
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    9 g( m' k( J) x
98. 
    & i  c6 L% L$ R
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    # C. t+ s5 y) \1 E& s# r. p
100.             {
     & [% L: _  P( ^& p) Z% z
101.                 return 0xff;
     
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     9 g% I: I& B, E/ U, {7 u& f# t. _) ?
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     7 `) c! t/ z* B/ g3 S9 _
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     , E- n4 {# M3 y6 t" \$ P- C
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     5 A' m1 U0 ^0 N/ }0 J4 }
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     . {. n9 q4 y/ y8 e5 r0 N) Y
115.             }
     4 |: y& F! W# ~& i0 \* Q
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     ( r/ m  W* O, ]  x$ R. \
119.                 return 0xff;
     8 n# ^2 d7 D2 n8 O# X( M& W/ z2 X( A
120.             }
     
121.         }
     
122.     }
     ! C2 X; ]# q/ t7 A
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     # v$ i) q: }( ^1 }7 S2 q& }6 N. S
125.     HIGH_level_read_time = micros();
     / a+ C3 W3 ?7 F* a
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     1 s; Q7 g6 a/ T. g( L6 c3 O" d. p
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     2 Q8 D/ y. _) v* f& ]0 Z% N
129.     {
     
130.         return 0xff;
     ; V* C8 q4 Z, h5 V5 L/ c- z
131.     }
     ( X2 L8 j' f( L5 U" I1 C
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     * _: g; D; L# V7 a& \$ a
134.     digitalWrite(_DataPin, HIGH);
     . e; F7 g# {& C7 J" N
135. 
     ! s# f/ ^  u4 B$ ~, F0 a6 O+ L0 ~
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     . Z, d5 H* I+ \" [5 u3 r
137.     {
     " J) P6 o7 a$ [% q* u& S) o
138.         old_data = Sensor_Data[0];
       e3 ~, X) f/ U$ t  \6 H
139.         return Sensor_Data[0];
     
140.     }
     , T7 G4 t: L; F$ I- D
141.     else
     
142.     {
     ! }# y* m) q: \  x
143.         return old_data;
     
144.     }
     6 J9 N) T( N( b, q9 {0 q% C4 g9 Y9 ]3 K
145. }
     

複製代碼

MeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

點擊文件名下載附件
閱讀權限: 20

2 R! \! W! @! w+ [
$ R& N9 d6 A* V9 {, g" k7 h

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
哇...要100 個銅錢

2 ]" l) H3 a' Y/ d  l認真的回饋本站一些內容, 很容易達成的!

感謝分享，還不夠權限下載

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
能否用到mblock 5 上面呢？

3 c6 J" N- O, {MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手