mBot Ranger 高速循線範例＿

% _, d! f7 r. w% o4 o( o) o, i& w2 y

1. #include <Arduino.h>
   + R( m' k" u; {* l9 Q
2. #include <MeAuriga.h>
   & z' M  z# E5 O4 N$ S1 a; ~
3. #include "MeLineFollowerArray.h"
   $ t4 \* U& z6 T6 I# n# m
4. 
   3 y; j8 \# n3 H, i+ ^
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   . |& O1 q. E" \6 R* T; x1 V
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    - r7 @- ^# t8 C3 r! T& c
11. 
    0 x2 m% p( S3 Q; h
12. #define Error1 1
    4 K2 I5 c6 I1 E3 H6 R& ~  g, ~
13. #define Error2 2
    " v& z  E$ B1 A. _- d* Y5 Q) K4 E
14. #define Error3 3
    % c2 V1 M" T0 R! D: k
15. 
    
16. #define Kp 15
    % e/ l6 W0 v) t9 {
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    2 p- d2 {1 U: {8 K- f" h" ~7 c
22. uint8_t D2;
    0 @5 r( }  @. L& h
23. uint8_t D3;
      B) j( l9 o0 R5 Q1 i
24. uint8_t D4;
    - N2 L2 K1 h6 ]) G8 t# k* j/ h' y
25. uint8_t D5;
    $ ]. l* c, N2 _9 a: c. r2 ?
26. uint8_t D6;
    
27. 
    8 I, [% z. o& N9 L/ s6 [* x4 g
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    
33. 
    ' H- Z7 n# t, I% i' a! B
34. byte Left;
    ' t6 A2 b! ^2 k" J+ I
35. 
    ! _; C. M& I8 w$ r1 l4 v
36. void setup()
    
37. {
    ; J0 P- F& N( `* y' L$ f
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    + H/ O& T! J# V" S
45. }
    
46. 
    
47. void loop()
    
48. {
    - C- D: M& j: c% h% p4 u! m
49.   while(!((lightsensor_1.read()) < (10)));
    : c0 J" @* K, j9 E) j& O  S* s
50.   buzzer.tone(1047, 500);
    ( F4 ^1 d& O! T7 A8 Z( k
51.   delay(500);
    
52.   do
    " s$ y: x9 D! k" ~- F4 [
53.   {
    & v' F0 y4 `2 m4 A- p5 ~% M
54.    int Err = getErr();
    * W7 R/ `+ `! q  M- a3 r; M- ^
55.    if(D1 == 1)
    
56.    {
    # Y% M" D& r& _( ~7 ~
57.      Left = 1;
    ) @( |1 @' |1 N% K' m
58.    }
    ' C6 \& }  a0 ], C2 @6 }
59.    if(D6 == 1)
    # O2 Q+ e# E2 H' y( X
60.    {
    
61.      Left = 0;
    1 P  U/ m+ u' @7 e0 s5 p( q
62.    }
    + E: `' `, q! n5 h& Z1 G; A
63.    if(Err == 99)
    ' }) V4 A' |/ L- s1 e: [6 M6 b% c: K. f
64.    {
    : ?+ S6 [2 N2 F6 S' \4 e4 _9 I- w
65.      if(Left == 1)
    + W2 z: w/ G, N6 H' v4 `
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    : q3 G8 ~8 [+ o6 x! K2 {
70.        {
    ; O$ Q: O5 G) @9 M3 L5 q5 Y
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    4 q9 @' [7 E% ]3 ~% y" ~2 P3 Z
73.      }
    
74.      else
    * R* ~7 s; G) D
75.      {
    : k  A1 ], c, m* l
76.        Speed -= 5;
    
77.        moto(Speed,0);
    4 _4 j* l4 R. m" D7 g) B4 x4 D
78.        do
    9 \, R; ?- i$ P7 }
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    
84.    else
    / O3 z; _6 m% b
85.    {
    ! K5 S% r6 t" C2 T  l: X8 _
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    % T8 F7 a. a! r2 Q& S
88.      integral = integral + Err;
    6 q8 L, J. u: A6 r" \. N5 e
89.      derivative = Err - previous_error;
    1 o# M/ R5 h) x' _3 ~- i
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    " c4 C2 N+ |; _& s1 F
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    , E; c7 e, ?% K8 `4 Y% Q2 V
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    2 w3 }1 K. ~; _* E
97.   buzzer.tone(262, 500);
    
98. }
    4 B( H1 g3 }. N5 X  o2 c  v; X
99. 
    
100. int getErr()
     : F3 [% [7 f  i5 v/ |
101. {  
     
102.    sData = linefollower.getValue();
     # f6 w4 U" }+ O6 ^: j
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     # ~* J( h. d5 E# x  t6 G
107.    D5 = ~(sData>>4)&1;
     , A, w. k. i2 B! k; J$ p
108.    D6 = ~(sData>>5)&1;
     ' l- \) |, r  r8 b0 i$ w
109.    int downD = D1+D2+D3+D4+D5+D6;
     
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     & x4 z. R2 T3 h; c6 |7 s
113.      return 99;
     
114.    }
     # C( X5 P2 F' i$ C4 G6 `/ V
115.    else
       H8 @8 n0 t' |& `- o
116.    {
     
117.     return upD/downD;
     ) S6 n7 V  J' H( z
118.    }
     ) ~6 b6 j! M( W' \
119. }
     
120. 
       j9 f! m( I1 t& }
121. void moto(int Speed_L,int Speed_R)
       K0 B6 e& f2 O" V( R# ?  ]$ Y
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

7 W& o# E  E3 k* ^MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   7 X3 u8 k; u' |. R
2. 
   2 k3 \  B  x% G. `" _" R1 |2 s: W* p
3. #ifdef ME_PORT_DEFINED
   0 j) U! L0 e: E+ @: [- i" o) J
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   2 }& ^4 W, P9 u1 q/ H: M
5. {
   
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   ( A8 m. v( @7 ^
9. {
     N; a+ g; F0 u5 ?
10.     _DataPin = mePort[port].s2;
    ( \' Y1 x* y+ F' k0 B5 x% J
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    0 v( r  L' y& e
13. }
    + f& K% M- _$ Y9 ~
14. #else // ME_PORT_DEFINED
    : }- |; D5 h+ x& j6 o* I6 |+ X
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    : [5 {: y8 t! B# w. L8 `
16. {
    
17.     _DataPin = pin;
    : |1 \* ^% P( j1 m- ~- }8 x: x; m
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    ' ^! z9 F/ t, V6 J' P' n% }
21. #endif // ME_PORT_DEFINED
    
22. 
    ; \, V2 t. q0 ~/ [. {. {9 G
23. 
    , y2 }5 T' J! ~# t3 M# ?5 y
24. void MeLineFollowerArray::setpin(uint8_t pin)
    : B' D) u+ ]6 O; e$ D) H
25. {
    % _+ `0 K/ P6 [; k1 G2 N3 t
26.     _DataPin = pin;
    8 ?& b# `$ k( L& O1 F
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    0 P2 h  {1 N6 X2 @7 m
29. 
    # I1 e1 z: Y1 N8 m
30.     #ifdef ME_PORT_DEFINED
    6 f( [7 a( i2 _5 D7 r
31.     s2 = pin;
    ' X0 u( |4 g5 h, g* j
32.     #endif
    
33. }
    . }9 m% z3 R% s" \6 l
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    8 @9 o9 ]2 ]; I4 j: C0 ~
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    - X: F2 l% a8 m7 G" F
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    , j, v7 C( Y0 f3 j
43.     pinMode(_DataPin, OUTPUT);
    7 c* r  x, p. o0 ~; I2 Z% b& [
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    8 {6 u( v& W7 Y: h' Q" |; |  |
47. 
    , K* @0 H. s/ y+ X2 P1 s8 S  s
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    * W& `; z4 R! p: X  k' e* s  _
50. 
    / Q* ~: |7 q" o' L; y, p, u5 n: t
51.     time_out_flag = millis();
    ) {) Y7 @' V( Y3 W" u% u: V
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    6 C. B+ {9 t: K( o1 B! L
54.     LOW_level_read_time = micros();
    5 L- E% {5 m) z& _& t  X+ r3 Z" {( p
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    : Z6 `0 Z! Y+ e7 M$ i! h
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    / J/ r* T' W- A  h/ w
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    + I4 H5 @  A! h7 p& d& D0 L: U* W
67.         return 0xff;
    , A& h8 ^2 e3 s9 I, X6 L& M+ q% ^' A
68.     }
    * f: C# B3 H. T8 L8 G* b; x: b/ d
69. 
    5 P4 D8 ~' r$ P3 }$ I9 R8 i. [% S
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    ! d3 g6 E4 m! G
71.     {
    . s( L; N: p4 g' M( `8 G- {9 i6 e
72.         return 0xff;
    ( ~0 i2 t% A9 f+ U
73.     }
    
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. 
    6 F5 u$ b% u1 M' a8 Q" v
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    # E, [* x' f! d
80.     {
    
81.         return 0xff;
    ! s: j0 O# W- w, _2 K
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
      h3 a5 f+ I/ _0 a( p4 ]4 w
86.         return 0xff;
    , K4 L" H+ L* a1 A% s" ?9 g/ s
87.     }
    ! k% [' Q( ]$ z
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    , e5 p2 }: |6 [4 k+ b9 a/ ]  P0 R
91.         Sensor_Data[k] = 0x00;
    ' y7 u7 D  }2 b. X1 W4 E: @+ S
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    
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;
    
98. 
    & v; z$ Q2 z$ n8 r9 }. ]* Y# E; I( j/ e
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     
102.             }
     
103. 
     4 S' ^4 r# d' s/ t
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     5 X6 l: V" b3 z0 o4 Z9 H7 j
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     * Y8 B1 E4 d' |  t3 @) Q
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     3 L, p1 c+ g9 A) ^
110.                 Sensor_Data[k] |= (0x80 >> i);
     ; S( U* k2 i4 @. L& v  z
111.             }
     % S! b) S* i; J% d
112.             else if(HIGH_level_read_time >= 100)
     8 Q, y3 z/ D* P8 x8 M2 X
113.             {
     
114.                 return 0xff;
     
115.             }
     0 G2 q$ T( w8 T' I  J0 I
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     " W1 g& H5 A8 m4 B/ @  P1 P
120.             }
     
121.         }
     % x+ U) z( c! t" `' X& N
122.     }
     
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     ) m+ y5 v" K! y( @) ^! e3 h
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     ( f. V4 k5 g9 I4 I6 \' H0 p
127. 
     7 H: J/ x- ]. n6 [" q- I
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     7 i  T+ s" t: `( ^/ n
130.         return 0xff;
     - {8 a& r" y5 T1 k) |
131.     }
     9 g9 v+ P1 v/ v
132. 
     . A$ A4 n7 e3 Y2 D
133.     pinMode(_DataPin, OUTPUT);
     2 k3 K% ]9 ~, H1 G) Q1 D; v5 E/ N) v
134.     digitalWrite(_DataPin, HIGH);
     ) ]3 c. _1 y3 F* P1 A+ I
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
       b; T# U9 |* r
137.     {
     
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     ; n1 [3 Q. g$ t. ~0 {+ Q
140.     }
       {, s9 @- T: B% \; K  A' E
141.     else
     
142.     {
     ) N# w' {! g* B4 b: N& g& X
143.         return old_data;
     * ]6 m- Y) X2 \' o" q( m
144.     }
       }# J7 A4 o  M% n
145. }
     

複製代碼

MeLineFollowerArray.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
能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手