mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   % H/ N" e$ N% c7 r4 `* m4 k
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   + W& K0 y4 q# Y  w2 c
5. MeEncoderOnBoard Encoder_1(SLOT1);
   9 t" I0 {2 B% q
6. MeEncoderOnBoard Encoder_2(SLOT2);
   - H+ p! P) S9 S! X/ ?( R8 j7 O4 a0 g8 D
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   3 n/ j: d  z7 \4 s# G
9. MeBuzzer buzzer;
   " B& R; `' T9 Q( m) W6 a! s7 O
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    9 e1 [8 v/ M! e2 o$ ]0 M+ N) X
13. #define Error2 2
    
14. #define Error3 3
    - A5 f# ~! Z2 t/ y3 |( h
15. 
    * D4 [+ x$ C0 x) P6 D$ z
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    8 E+ ^! F/ s1 D& t; \0 d; r
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    ( ~' ?7 y& x( u4 `" w% _* }
22. uint8_t D2;
    
23. uint8_t D3;
    - l( |0 `& ]; ]8 C
24. uint8_t D4;
    
25. uint8_t D5;
    
26. uint8_t D6;
    $ G* v3 u3 e6 m! D4 A9 x. g, v
27. 
    0 |. r" M4 c1 M! h+ q, W( Z' m- E* J
28. float previous_error = 0;
    1 h; y: Q" U* v) l. _! n1 d# k
29. float integral = 0;
    - k2 R9 ~' O& `5 M5 v- I9 @
30. float derivative = 0;
    3 }# y! q2 F& D* K/ Z; k
31. int  Speed  = 160;
    % j1 c! n+ O' S) K! a& @+ f+ f
32. float output;
    4 K0 A7 ]" x) v* w
33. 
    
34. byte Left;
    
35. 
    
36. void setup()
    - z* c5 g+ A( I& i
37. {
    
38. //Set PWM 8KHz
    % ^" b) p4 I6 o! Z% D! Q' E( K7 L
39.   TCCR1A = _BV(WGM10);
    # A. i, U- g$ @6 B7 m: C! V
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    . u4 ^/ z2 K/ p3 i2 x, I8 s. d
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    2 c) t0 a) d+ x+ S1 p
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    2 [5 q; e5 y# K9 G: s" ?- w! B, O
45. }
    0 |+ \' l* L# J* }
46. 
    ( f9 G6 O7 G: W$ P" p" Z
47. void loop()
    
48. {
    / K! U+ W: I# h8 ^5 ?) L
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    , i& k: t) k% h" R0 U
53.   {
    
54.    int Err = getErr();
    4 {- j( x' d+ D0 O0 M: r( \. x
55.    if(D1 == 1)
    5 X) F9 Y- t/ T/ P% Z! ^, C, k- k
56.    {
    : M+ P4 Z: e9 n
57.      Left = 1;
    
58.    }
    ) I1 J' F7 C# Y# S0 R7 G: j) n
59.    if(D6 == 1)
    
60.    {
    1 g. B2 {5 \) Q& p' b% `
61.      Left = 0;
    7 J/ o; g; ^6 v+ g' U/ {
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    3 y8 ?4 Y! i+ Z
66.      {
    
67.        Speed -= 5;
    ' l; R" F1 s1 \2 g: B8 Z
68.        moto(0,Speed);
    ( A, c( b, e( j
69.        do
    & g; [  O) U4 [$ r6 ?* {  f
70.        {
    2 M# O8 l' n+ U6 ?4 T/ e
71.          Err = getErr();
    + e. \1 S# |; F
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    / j4 ?" h5 \- g8 Z
79.        {
    . q) J$ E8 }: e: j9 Q
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    ; M4 F! }/ E) G) y. \
83.    }
    
84.    else
    % r* L2 {7 o6 F: J* U, x
85.    {
    6 f- F( p+ `8 s% @
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    ) ]4 S0 {! U% J. S6 b5 b
88.      integral = integral + Err;
    / P& C0 B  Q! J8 N" w
89.      derivative = Err - previous_error;
    - N5 z! ^" a7 T9 U! D
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    $ A# \4 _; I, J
94.   }while(!((lightsensor_2.read()) < (10)));
    8 O1 F, g, M( p
95.   moto(0,0);
    5 g9 p/ C, K4 H+ H! H
96.   delay(500);
    " x  p8 F+ ]1 ?
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    
100. int getErr()
     6 q2 s, l7 A) R: c' B5 K
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     0 C% O. m& y3 |3 N$ b
105.    D3 = ~(sData>>2)&1;
     % q1 a# Y+ k8 Z$ p7 B4 u
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     4 }+ |+ {+ o) I
109.    int downD = D1+D2+D3+D4+D5+D6;
     0 }- I2 H& ~, ^* ~. ?& w2 U
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     # a3 G3 J4 O( d3 l$ Z3 n, h
112.    {
     
113.      return 99;
     
114.    }
     2 J" J) E0 F: o& u
115.    else
     5 {" b8 G7 s- i' J
116.    {
     
117.     return upD/downD;
     
118.    }
     / }9 N4 D: m/ D  A( Q% D+ ?
119. }
     # B8 h' x! F$ E/ }
120. 
     $ t" r* \& K# ~( u* ^" G: L0 e$ r
121. void moto(int Speed_L,int Speed_R)
     8 S3 w+ `+ C( l0 I: Y" I4 N5 I0 T
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     ' A2 f  T/ [6 o
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

9 U5 O' v' m8 S0 [7 gMeLineFollowerArray.cpp
/ b2 w4 c' Q+ ^0 Y

1. #include "MeLineFollowerArray.h"
   
2. 
   7 @0 A+ h' X3 V
3. #ifdef ME_PORT_DEFINED
   1 @( \+ e& Y6 n# E3 C; i1 \
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   ; d1 R) S. X* ?) S6 _* X) b& r
5. {
   : W9 H: K1 Q0 ]3 R: v
6. 
   
7. }
   4 j8 B& \" m, R% ]4 m. ~" I
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   + k, y6 [& @) Q- t6 u, t$ b4 l1 J
9. {
   
10.     _DataPin = mePort[port].s2;
    ' u1 ?  I% U, r0 R5 \( Y* Q$ k4 f
11.     pinMode(_DataPin, OUTPUT);
    8 P8 S5 `' P& {3 `. q8 ^
12.     digitalWrite(_DataPin, HIGH);
    4 w# Z: O+ i1 h
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    1 b: y8 X" ~" [+ V* n4 B
18.     pinMode(_DataPin, OUTPUT);
    8 s! Z' U7 \; U
19.     digitalWrite(_DataPin, HIGH);
    ( \3 |6 H. w! `& k
20. }
    : a$ F2 y0 j4 k3 t: J7 e$ ?
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    7 ~% s7 `0 z% N
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    ' ~% K$ e! a3 S& v# o
29. 
    1 \. G" V8 m' _# W9 E
30.     #ifdef ME_PORT_DEFINED
    - D: X7 E8 I# N: L
31.     s2 = pin;
    6 M& X; O& a/ e" R
32.     #endif
    - l* J8 J0 @+ z3 T3 j: E
33. }
    ; T8 s0 t6 Y" x4 u- S
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    * V! Y/ m2 [( l7 a* W' a& N4 W
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    " z- p7 D8 x; o: Q  _4 ?1 h1 n9 O
39.     uint32_t time_out_flag;
      \) j: v/ x" m7 Y& d! M% l
40.     uint8_t Sensor_Data[3];
    : K. C' y3 k0 y0 b0 _! S* y  l# s8 n( F
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    0 q8 y3 b& [' J3 |: B5 P
44.     digitalWrite(_DataPin, LOW);
    - M2 X! ]7 w7 c
45.     delayMicroseconds(980);
    8 v' E7 X4 K/ L+ c  R
46.     digitalWrite(_DataPin, HIGH);
    % e9 X' b( F; O6 l) J+ a3 C- `; Q, b
47. 
    1 Z* [. {* v: B
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    - k6 n3 C1 L. Q, k4 L+ V3 `
51.     time_out_flag = millis();
    * P0 o8 y7 t/ y$ M, p8 k
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    9 X. ?' h5 I3 P4 O: F% e( f4 u
57.         return 0xff;
    . ^+ F4 r; r1 H  ~
58.     }
    / a1 b7 @0 @6 C+ |0 P
59. 
    & o# s% R, }  Q8 R4 g. [, m! s: A# K
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ) f. s$ ^- |3 b7 h7 b' ~
61. 
    0 R  `8 j% i- t( y  v- H4 x* _
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    4 a- `$ K6 o* W; A
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    0 I+ u0 Y1 p" @3 m0 s) j' ^
68.     }
    + Q. o6 `% u( [5 U  R
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    ! U+ H) S. k- T/ T( p' F
73.     }
    
74. 
    % @5 m* i& D' m! B$ L
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    + u# r$ L' {5 o
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    . A4 R- B( h0 t2 {* I, }5 C0 O- p6 a" P
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    8 U1 A. F: }; N, b& I( U
80.     {
    6 r# ?. ?# H: u, J* ]+ y& M
81.         return 0xff;
    8 I6 G6 }! S& q3 T/ {
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    ) U1 \* l2 g7 `
86.         return 0xff;
    
87.     }
    ( ^) K' g; ~& }8 ^6 x! ?; z9 r
88. 
    * [+ L  B& j, B8 p( _
89.     for(uint8_t k=0; k<3; k++)
    5 j/ m2 g" r7 I& F# Y# {$ ^
90.     {
    
91.         Sensor_Data[k] = 0x00;
    , z2 }& V% S& C" d0 G* ]
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    , c# W. N  E: X7 B
94.         {
    - M. G" O! p* c0 u
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    5 L, D7 S" {# K# R. w. V  D
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    & j# @# k) W5 Z& H9 b( Y
100.             {
     2 v7 b2 N2 l% P( ?# c! ?; S
101.                 return 0xff;
     7 A0 s! {6 v( M. _% j/ S. t: R8 S
102.             }
     
103. 
     ' g7 [$ `4 W5 ^# w- U
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     2 _' t  B3 q2 B* w9 x2 @; X# J
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     7 y% ]3 {9 h& I5 n! n/ {5 F$ I
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     # ?: A$ z/ \" B" M- N
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     $ z+ b: ^0 M" \4 e$ X7 Y5 m& {! M
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     " X. f" S9 R& O, S* b1 Q# J" y
114.                 return 0xff;
     - i5 |( A' ^5 w! P. E
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     & d  W% q0 P5 E& J  n
118.             {
     
119.                 return 0xff;
     
120.             }
     : f* f" F5 m. \" v7 Q# G. L# M) i" J
121.         }
     & L2 ~- _$ _9 u8 |
122.     }
     - q5 U! |6 W2 }! N& d$ b) [
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     & {1 u8 t* s& T( d0 J
125.     HIGH_level_read_time = micros();
     4 g9 \, l: i/ w+ x, d; ~2 X4 _- Z, d
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     % h0 o* V. b3 i: f
127. 
     / r/ h) R2 J( i( w: ^' u
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     ' y2 T- l$ P: d. Y6 G) O; `9 @8 A6 {0 n
133.     pinMode(_DataPin, OUTPUT);
     # x! J" a' j0 I% M% R  p
134.     digitalWrite(_DataPin, HIGH);
     1 Z7 d) p9 {$ p0 S5 U3 L
135. 
     0 u/ L. k# A! r1 ]! N/ c, L3 |
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     - b9 q% `# ?! w
140.     }
     
141.     else
     . u9 N) Q3 d# S8 [# A8 l. ]0 ^
142.     {
     : }, k$ k& [8 S% {: Q
143.         return old_data;
     
144.     }
     ' m+ M! [: ^( M
145. }
     " s: [6 S! T4 A+ s8 C

複製代碼

$ _/ X/ z$ x* B/ b7 j9 i) F& l/ }" tMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
0 ~. l' A% {7 m0 \( _! Y2 s( v  x能否用到mblock 5 上面呢？

- |  k9 m9 h/ T# X3 a1 W# z% V. L0 B3 PMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手