mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   , J, m' o  \) Y* Y2 o  q
4. 
   ) v  V" w# }; i3 _, Z, g  O
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   6 S4 k/ a1 o2 @! A3 V
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    + r: }" u" _' ~. n+ l
13. #define Error2 2
    
14. #define Error3 3
    , u! `; L  W' m! _& y# S6 O
15. 
    & ^( v: Y9 J) i6 C( G4 s
16. #define Kp 15
    . }  s7 O" w; \4 X# ?2 j2 N
17. #define Ki 0.15
    3 ~2 v0 G$ E& I+ |6 q4 r
18. #define Kd 0.03
    
19. 
    0 H& x- `* I9 @4 j% `! k1 D+ u
20. uint8_t sData;
    7 Q. O9 h* g: B! o+ o1 ^: H% L
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    8 h+ w9 v. [8 ]6 C: f9 I
24. uint8_t D4;
    ) {7 N8 Q5 [! j# t) {( P
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    8 q! v0 L" `$ y: Y4 J6 S& t
31. int  Speed  = 160;
    
32. float output;
    . W  `( x  G! v9 F+ m' q- m
33. 
    
34. byte Left;
      Y& Z! X. E* y) c
35. 
    " O( U7 u% ?# r" S1 n0 Q$ W, A. v1 E) h: ?
36. void setup()
    3 [% j/ g, H9 A1 b
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    % x* ]8 a: e: D! q# @, U# Y
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    6 t0 Y) Y# c# q4 x
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    % f) o: D, j6 r, x9 E. Q* z
44.   buzzer.setpin(45);
    
45. }
    " V* d6 i" n& |6 S* `0 m6 J; s# f
46. 
    8 p3 P1 c. U9 N4 k
47. void loop()
    3 @. E3 Y5 C1 C0 t
48. {
    : Q4 F. \- ?& Q5 ^+ o3 \
49.   while(!((lightsensor_1.read()) < (10)));
    ! z( p* z* S9 E, B* O* C% u% Z
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
      l8 j/ \- B8 o5 j' W
52.   do
    
53.   {
    
54.    int Err = getErr();
    ( U# V4 C! W: M* ]* ~
55.    if(D1 == 1)
    " |( P" ^* V( z: `
56.    {
    " F# u" D8 P0 C' A  \. ^
57.      Left = 1;
    3 T# }/ m& `  e8 @$ }
58.    }
    
59.    if(D6 == 1)
    
60.    {
    3 v1 p. ~& F8 I! s( U) x) F6 t2 s
61.      Left = 0;
    # f+ ~3 v9 L- _
62.    }
    - ~, M# @& \! ~9 Y4 O2 Y3 P
63.    if(Err == 99)
    7 n* ^. g7 J! j/ j% q+ |! U
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    1 h1 [8 T* T& V$ j" E: p# b! v: H
68.        moto(0,Speed);
    
69.        do
    + |4 ^, O. U, n2 s5 u, a8 H
70.        {
    
71.          Err = getErr();
    , n5 J6 l5 [- `; q5 b
72.        }while((D1+D6) == 0);
    
73.      }
    9 ^( j% y6 N8 R5 A& s( t7 u4 L
74.      else
    " v3 A5 ~. H! f2 i5 @
75.      {
    $ Z- e5 Z* X1 v* K% m$ [) A' o
76.        Speed -= 5;
    
77.        moto(Speed,0);
    * t% H7 R& a8 G* b: C
78.        do
    ( `- ~) u+ Q1 T0 k" R7 D
79.        {
    
80.          Err = getErr();
    . B/ g) ]7 T% p# a( |! p- O
81.        }while((D1+D6) == 0);
    9 ?- Q+ c3 Q; g4 G  G6 _
82.      }
    . [7 X" ?* ?* p
83.    }
    
84.    else
    
85.    {
    % K6 w0 U! _0 w
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    : ^% Z; m9 m" |( m
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    # O  x& K! |# W) P( h5 i/ S
88.      integral = integral + Err;
    ) ?1 j+ D/ L& o2 Z% n7 R
89.      derivative = Err - previous_error;
    , j, v* ^1 d  p, [# Q
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    ; n! A1 K3 k- I' i5 }) C
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    & |2 d# p* B' b: G0 R
93.    }
    : M. Q# V  O. O# J+ ?# S1 s1 R7 W; g
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    ; x% g1 T5 G" _9 K6 g% x/ G4 g
99. 
    
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     9 f. p6 y" ]* V' @  L! X
106.    D4 = ~(sData>>3)&1;
     $ F4 Q' H9 q( ]
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     1 [: I9 r2 c& S
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     ' n+ Z8 P7 D# J0 ]
111.    if(downD == 0)
     1 y) b+ q2 `7 h% m5 Q
112.    {
     - a4 ]" c7 M, _5 R$ @! f2 b
113.      return 99;
     ; G6 \9 v# e4 U. X$ c7 g
114.    }
     
115.    else
     / n* U: u) E! @4 ~- N! g  Y" D
116.    {
     
117.     return upD/downD;
     & D6 v$ \0 F4 }0 s
118.    }
     
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     & t, }6 g7 D" u. n0 V9 d
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     7 f9 A3 Z0 L' V. U# \9 e
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   + r6 t4 `: ^' _
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
     U" T, U1 W% x
5. {
   - ?+ j, ?4 G+ }0 L
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   8 x3 M4 L) \. d. S& ?4 H
9. {
   # {2 f+ `1 ?) O; T
10.     _DataPin = mePort[port].s2;
      E7 t$ c, J) R1 _5 p
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    8 @1 r+ d/ v" h7 ?8 N# \
13. }
    + t3 I& ?6 A; L$ P- @& H
14. #else // ME_PORT_DEFINED
    : M' v# Y) P$ C3 q/ |8 f
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    1 d) j' e" i2 q# z2 |9 |( S- D
16. {
    
17.     _DataPin = pin;
    0 N9 X! x4 ~8 M! t
18.     pinMode(_DataPin, OUTPUT);
    9 `9 \4 B" _/ {% O! f) k
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    6 F  g2 H/ K( m
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    - `4 i1 Q' S- E( J2 U' Z- a  e
25. {
    1 p; [' H. ]6 _+ [* T9 N
26.     _DataPin = pin;
    4 W" s1 ~9 u+ K- A
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    2 Q( p8 w( \2 ]& O
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    6 y2 `+ t5 g; u# f. `2 O5 D
32.     #endif
    
33. }
    6 v) q1 x6 U: f5 {
34. 
    ; @( q4 r7 i# U: L* Q  @0 S
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    3 z8 I1 [9 p4 n
37.     uint32_t LOW_level_read_time;
    , P; X; E$ k7 d+ C4 n: U: C
38.     uint32_t HIGH_level_read_time;
    0 i  b  P6 r0 J' O
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    " }6 Y4 N3 ~( v' D. e1 [8 ]/ l% J
42. 
    2 ~! M! g! G/ ^* _% m6 E
43.     pinMode(_DataPin, OUTPUT);
    , b& X3 ?+ W- ^" h
44.     digitalWrite(_DataPin, LOW);
    2 T0 R, d2 O4 J/ n5 c
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    - A1 R+ s* L+ ~/ t; K/ Z/ {
48.     pinMode(_DataPin, INPUT_PULLUP);
    . Y4 B' G, W, U! n; y4 p
49.     delayMicroseconds(10);
    : D- u" {% [8 v* F+ v! ~
50. 
    3 F  r; T6 X$ q. E
51.     time_out_flag = millis();
    # {5 x' ~- w( A
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    / t5 z+ Y5 O2 o) {0 X7 a2 C
53. 
    8 Q! d  ?# L5 q" X
54.     LOW_level_read_time = micros();
    ) h) }+ {; I/ _( p
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    * Z6 h$ U( ~& E1 ~. a' U0 k5 c
57.         return 0xff;
    ) p8 x* O+ r9 L
58.     }
    9 a. a; b. H; B$ W+ g4 U4 R
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    % h; J- J: e" f0 C7 U
62.     HIGH_level_read_time = micros();
    " q& Q; x6 N4 d6 y! z# f9 U
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ( K$ J2 Z  g6 T5 B
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    5 o3 O, _- g+ Z7 N/ ~7 _6 E1 g% p
68.     }
    - M# L! d' Q' U" X6 }* M
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    % P( R% t7 e# Y. `/ Q
71.     {
    
72.         return 0xff;
    
73.     }
    ! J; p- D2 v8 m/ c2 e0 \9 ?1 g( ]4 d
74. 
    
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
    
78. 
    5 P0 z1 j) t/ A3 _5 M1 u/ C. C; u
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    : B* Z6 ^- @8 a3 v0 t) q
80.     {
    
81.         return 0xff;
    4 V0 x, }  C# g8 Q+ l8 Y% b
82.     }
    , L7 W) Y3 J4 d
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
      Q4 ^- [: l1 K+ v9 G
86.         return 0xff;
    
87.     }
    % e& d- G  ?5 K7 l8 e: w  b
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    1 }0 O; J5 e" q7 C5 i# \1 I% [
90.     {
    8 F/ o1 M. p  Y! D7 z+ n$ V
91.         Sensor_Data[k] = 0x00;
    
92. 
    1 e, ~: G, ]/ [
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    " V/ P0 p0 c. y" e! @3 m" S3 D
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    6 o' W" s8 v! C( x2 Z
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    6 j* U# Z* b( C7 ?$ T7 H
98. 
    7 d: \2 }3 \1 d8 F$ A
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    - |# C& `; |  d3 b4 K& h
100.             {
     % X6 z' k. L6 o0 n2 Q2 K* r
101.                 return 0xff;
     0 |% R& L: J1 p8 a8 f
102.             }
     
103. 
     ) p. B$ ?; l, k+ G* d0 w" A3 j2 @
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     4 n9 {4 ~( o! X7 W6 V
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     + n; X7 u+ w# c
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     + K/ F3 r* a5 K& n; |% d0 i
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     # C% Y% i' P, e' q
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     1 ?( t$ n( `) v, y& C" I
113.             {
     2 L3 a6 U* A  i  E; W" m7 I
114.                 return 0xff;
     
115.             }
     , U" {" Q! K! v/ V" n  s" \
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     8 m" T. ?" K. q" k, o
120.             }
     
121.         }
     
122.     }
     - e7 x0 U+ J2 O9 I! R
123. 
     ! L% j6 f) L( K
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     . v9 ]1 K/ Y2 n
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     1 d' S$ G0 }; B
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     1 I0 u' V& }& e/ O* u6 _: S5 @
129.     {
     
130.         return 0xff;
     
131.     }
     0 N* @5 S8 m* @  ^4 E6 ~0 H5 I
132. 
     & H/ w$ Z& g9 }/ y: l
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     ) {, p5 l6 Z( j* Q  C) W
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     / K1 X' f6 }, G1 e
137.     {
     
138.         old_data = Sensor_Data[0];
     ) E( {: @0 g+ d
139.         return Sensor_Data[0];
     # p4 l+ K0 ]! _2 j; ~
140.     }
     1 y7 v! _& b7 c
141.     else
     ! M( w" g4 j' D# s  g4 P
142.     {
     + ~5 M4 k3 F1 K; |
143.         return old_data;
     1 K+ x) O, q6 g$ Z. d! [4 C
144.     }
     
145. }
     / }* V% n0 |9 v: u

複製代碼

1 p* ^8 [! K/ T# z. L% n( H% S8 @% jMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

8 Z! p" D8 b  l認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手