mBot Ranger 高速循線範例＿

" b% J4 m: C) ~: d0 b2 A& w

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   # i' B* R8 ]4 S8 }5 M
3. #include "MeLineFollowerArray.h"
   
4. 
   & `. y$ A) q' N
5. MeEncoderOnBoard Encoder_1(SLOT1);
   " f. Q, i/ e6 b
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   - z  s4 Y' H$ [/ G* z+ m9 {
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    ! g! M3 ^) k" s' _) f8 }
11. 
    
12. #define Error1 1
    . O( i: V0 r$ B: z' \$ @) b  K
13. #define Error2 2
    
14. #define Error3 3
    $ H. H5 P+ s- J% e6 @1 A; ~
15. 
    $ L7 G- n% m+ Y2 ~# ^' M: D
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
      V) a& a( V9 _- e% P2 c) Q0 ]7 _* H
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    6 X* P( c  l5 Q; x6 R
23. uint8_t D3;
    " [% f* O+ b1 ~3 E# a, V
24. uint8_t D4;
    
25. uint8_t D5;
    " q; V$ U7 B( F
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    + r* q% D& K; O; B5 K6 K, p
29. float integral = 0;
    
30. float derivative = 0;
    6 g  l& i, `0 M. d7 t' \
31. int  Speed  = 160;
    . i# U1 Y" v  T# G
32. float output;
    ( d- e6 R7 G1 D+ O# Y! H
33. 
    
34. byte Left;
    1 z/ e# p# Q$ ^" _! X: X5 {# T
35. 
    
36. void setup()
    
37. {
    0 r: z5 u( n9 @7 S
38. //Set PWM 8KHz
    6 S/ s$ W" O) k5 t
39.   TCCR1A = _BV(WGM10);
    * y4 w( b; [; T/ l/ Z3 X6 F
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    , I& K8 v4 ?  Y* M4 u/ t2 P: p
43.   Serial.begin(9600);
    ' k+ v! \: k" S4 ?/ i- [
44.   buzzer.setpin(45);
    * }: `" S8 }$ a& i" U
45. }
    + ]# S/ S/ r; B( c" Y) `7 g* J
46. 
    1 C6 m( v1 r5 }! q4 n; F4 J! j% C4 }
47. void loop()
    7 `6 X: O* d" u! i0 t
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    ) e' l! n9 L  j' a+ ~! y
53.   {
    $ J+ l( s% C  R' f; q
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    : @+ |8 Y1 a! j8 t0 ^, i
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    
62.    }
    $ i7 b  Y& x/ m: H0 x
63.    if(Err == 99)
    4 D& |  k* w, ]+ \5 _
64.    {
    
65.      if(Left == 1)
    " r% ~4 W5 g3 h* _
66.      {
    - l9 j! b, o0 J9 C$ |7 |
67.        Speed -= 5;
    ' S2 s  h/ D8 l
68.        moto(0,Speed);
    
69.        do
    4 G& B8 x+ R7 H2 W7 s( u8 v
70.        {
    - g2 `: z5 c9 h; G2 v% A5 d  c6 M2 [1 X1 g
71.          Err = getErr();
    # b/ ^" V7 X# g. \1 a2 {& _% F
72.        }while((D1+D6) == 0);
    
73.      }
    6 h' V/ J6 ]! a3 c: F; |
74.      else
    + ?9 `" n+ V$ p9 ?
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    2 G( t0 @6 N: ]5 M
78.        do
    9 |- @+ S- }2 `8 L9 d
79.        {
    
80.          Err = getErr();
    " {7 [" K+ h7 t; B# k
81.        }while((D1+D6) == 0);
    " D8 p" Z6 _$ K2 @
82.      }
    ( B0 S) c! \' N1 g
83.    }
    
84.    else
    
85.    {
    6 \! ~( n% q) Z( [- R9 V
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    & Q1 |# _1 l! u: Z. s$ @6 y. }; Q
88.      integral = integral + Err;
    , t' b* F2 Q$ l) [1 s
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    ! h/ k  T4 {7 G# d& N' u
98. }
    ; ?4 `' D7 N% {! @  @
99. 
    6 f! p2 y  t. B, S, q
100. int getErr()
     6 y+ P; r$ a- }/ r9 Q6 V. ?2 j4 _
101. {  
     
102.    sData = linefollower.getValue();
     , L2 x2 X% r7 u: H
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     0 l) |$ E* c6 _, Q- V: ~
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     4 S* U0 P( ^# D2 b8 H
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     . |* x2 B6 L8 {; I5 z5 j
111.    if(downD == 0)
     * y: _$ |1 g) R4 f9 t8 O. h& A
112.    {
     2 X8 G: _! d: a9 G
113.      return 99;
     " B  U# G$ W+ y
114.    }
     - t9 h, r3 y7 x: p3 a9 N
115.    else
     
116.    {
     3 D8 y; M9 f( {& v8 n3 r0 l
117.     return upD/downD;
     
118.    }
     ; C: v4 W+ g2 t4 ~3 U8 O
119. }
     
120. 
     ! V0 A# w9 `. e/ e
121. void moto(int Speed_L,int Speed_R)
     4 ^/ F+ X7 T9 f4 D: u: o: e
122. {
       l: ^; V3 W7 e2 r; x7 z
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     # j. G1 Z( N6 x! {6 z% I
125. }

複製代碼

MeLineFollowerArray.cpp
1 }+ K, j7 J& D) _5 }( q6 ^$ g

1. #include "MeLineFollowerArray.h"
   2 d! v" V+ O+ n3 u' Q: I3 j
2. 
   
3. #ifdef ME_PORT_DEFINED
   . |* R7 ~+ b& |$ H" d
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   
6. 
   
7. }
   * [. v/ F9 i) ]9 x7 @: ]
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   5 o8 ~) p1 l7 d: E0 D8 f: M
9. {
   
10.     _DataPin = mePort[port].s2;
    ( j2 ]4 m$ o7 X+ R- f; ~7 P% @
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    8 G3 }+ y' _5 i5 |1 O7 J" E; M
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    9 \; C1 l& t" p! j8 ^( e
16. {
    : x+ V7 l/ M0 t/ L$ _  C0 }# A: `7 i
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    8 y& V- @0 ]! f. V( [. M; X# B: r
19.     digitalWrite(_DataPin, HIGH);
    * U* B6 D* |3 ]( J, U
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    # X+ n$ R8 i, w, F
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    7 l( C' K3 {2 X1 h* [% x
25. {
    $ b( v$ X/ h6 c6 G9 G) f
26.     _DataPin = pin;
    4 d: w& W$ }  o, a
27.     pinMode(_DataPin, OUTPUT);
    , Y1 P7 g4 L2 ?4 r6 v/ Y' F
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    $ l4 S' l3 [7 x
31.     s2 = pin;
    9 c/ @* x# M4 A: ~) }
32.     #endif
    
33. }
    5 S3 k% Z/ b* v; l; E
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    6 s4 [. f/ x% E# J; n7 P/ p
36. {
    , k0 }, J* G1 V# ~
37.     uint32_t LOW_level_read_time;
    # Y. C% k. [4 S) G' m7 y
38.     uint32_t HIGH_level_read_time;
    8 z. Z/ ^% u* }5 z
39.     uint32_t time_out_flag;
    / o& w; `7 [: p# z8 ~
40.     uint8_t Sensor_Data[3];
    " o4 w. R! I4 L2 A5 s
41.     static uint8_t old_data = 0xff;
    - e1 F1 D* H1 `6 o! `- M
42. 
    ; _: F8 h8 o# g9 T5 h
43.     pinMode(_DataPin, OUTPUT);
    " e. X4 U* o2 ~' K# J/ O
44.     digitalWrite(_DataPin, LOW);
    # W+ F# h% z2 D  _  J8 w2 W
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    " Z6 K" P/ {' U, e
49.     delayMicroseconds(10);
    
50. 
    * T# l  J9 M, P5 c7 I
51.     time_out_flag = millis();
    ! ]  y* [4 G8 A: L
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    ' T7 r' O( i6 r9 f
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    # x' Q: f) S% ~4 [8 P# b. h
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    4 u( R( j% B4 K4 Y7 C6 t, G. b3 K
61. 
    " O+ G; s; h$ F) P; s  C0 @
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    - [) @8 g# ?* i* n9 d8 l
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    * P4 l) F: X+ F
66.     {
    + L, q8 Y. y0 K
67.         return 0xff;
    
68.     }
    / T% ?) B, p' X7 w3 {
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    2 s3 H+ ]" H& s. l" m# g
71.     {
    
72.         return 0xff;
    
73.     }
    7 ]" n1 _) {6 v. b" R; @. G
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
    9 g/ p, D, N8 G1 p- X/ D. U. W
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    ! ?1 C+ A  q. @. l  J! H9 v" p+ j
81.         return 0xff;
    7 L" I" L' L% T9 C0 h; [
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    , ?2 p% S# Y% {8 |
86.         return 0xff;
    ) E2 R: p9 C5 O: o+ p
87.     }
    8 Z: g  @# r: d. n* x
88. 
    , G* U" q! b# S+ E' e( g
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    8 t" G( a; C9 k$ L+ k" D
91.         Sensor_Data[k] = 0x00;
    # p$ L. i  ^% r( I1 h
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;
    6 N2 ?  l3 S1 O! ?. Q
98. 
    ( V; B# k3 V% Q3 l1 x! u
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    3 H2 S; [7 K$ m
100.             {
     
101.                 return 0xff;
     
102.             }
     
103. 
     # @6 |! i4 N, j& H* }4 B5 I
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     + G: ^" W9 T% L% \; A  A
105.             LOW_level_read_time  = micros();
     1 _& i' m" g# r+ Q% b! `* K
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     % e3 F: ]3 E( f7 n4 q# b
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     # V* N0 e7 C5 y9 r
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     0 a1 X% ]" R7 E7 D7 ~2 A- I
111.             }
     " I* m6 R% F0 N. _) y2 h& `
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     8 {: t7 S6 T3 D. w( Z3 w
114.                 return 0xff;
     " N) P! @0 k* E: w; z7 d) m9 Z
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     
120.             }
     
121.         }
     
122.     }
     
123. 
     ' \" ]# [! q( C4 U3 t1 d  a  A
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     , s6 z9 T! w$ N5 A0 R# o! _) \
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     " D1 I. c8 `" V2 {( A# z
127. 
     8 x, m  A; C' B- o- y4 m
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     : S; j' q& t  b
130.         return 0xff;
     # F3 T) t' l, x- I- @/ Y3 {" D; ^" g
131.     }
     2 W" B& [" T, ~  _# T' L2 G$ [2 h1 U
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     ; a4 q8 F% I" B
134.     digitalWrite(_DataPin, HIGH);
     ) ^" F# G  k; T- \8 Z/ ?
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     5 m4 t' ?9 F' \& o7 o+ ?1 o
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     # R! i+ d% r9 A& o
141.     else
     3 r6 N  ]& r; N2 V3 O
142.     {
     
143.         return old_data;
     
144.     }
     / l6 ?9 `0 d9 y- V6 p9 M. t# ^4 m
145. }
     9 E! G% _0 w8 J

複製代碼

3 v/ \0 S- M) S* q2 dMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

- R/ Q" X0 ~! ?& N6 k) b( ~) e) C

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

4 j5 h. o3 |) p2 P- N4 D3 `0 WMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手