mBot Ranger 高速循線範例＿

: k3 B/ m+ e5 ~/ H, H) V5 T# H

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   5 N$ o! x( P2 K, m- q
3. #include "MeLineFollowerArray.h"
   
4. 
   - J8 s7 b% N- z
5. MeEncoderOnBoard Encoder_1(SLOT1);
   . s. \. r9 W3 u( k
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   . E  s7 X7 z% i$ o! U; n) Y
10. MeLineFollowerArray linefollower(PORT_6);
    9 X/ h6 s& t' x% \( c8 ~
11. 
    % c6 j# f/ @- i6 l! e
12. #define Error1 1
      T* {9 P5 k7 m4 c% C+ a: W7 z; O
13. #define Error2 2
    * @0 I: B8 `, {
14. #define Error3 3
    7 ~( N# @- N$ g& A. {" k
15. 
    9 E3 ]( ^5 d* f; K
16. #define Kp 15
    + R6 J( ^3 A3 A) z( `
17. #define Ki 0.15
    2 S' Y: P. r- L8 N. X, I
18. #define Kd 0.03
    : c% X# ^7 \7 R4 F  T% A
19. 
    1 v/ a5 V# |+ L% I: g
20. uint8_t sData;
    , d$ x' v/ z: Y
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    5 d! Z( V7 P. |8 i; h
25. uint8_t D5;
    
26. uint8_t D6;
    / x, V# A2 H* e: h6 `% s
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    1 ^- M1 X& E0 d/ y! g0 |
31. int  Speed  = 160;
    % }$ s2 n7 V. _8 K1 V
32. float output;
    , V, q6 B3 L, t
33. 
    
34. byte Left;
    0 k! y- j& S/ D5 t8 z& e" ]3 e: i
35. 
    4 g2 T7 p5 {1 y; t7 o4 u$ p
36. void setup()
    5 }( |1 J7 p( {; ]: \$ B  a7 j
37. {
    
38. //Set PWM 8KHz
    - t+ G- O+ L: T* J- L) h$ J. @
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    : ^; Z) C6 c. ?. P: H
43.   Serial.begin(9600);
    - D$ X; \8 z: D7 _( M1 @
44.   buzzer.setpin(45);
    ! `" t& t! `0 M0 ~& [; k. k
45. }
    5 {$ Z  P  S/ ]8 a
46. 
    
47. void loop()
    
48. {
    ) P. b% d# v% n# k7 T
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    9 t  J/ I7 A8 T+ Y3 r- Y4 i
51.   delay(500);
    
52.   do
    ' W/ O1 Y  E0 u7 T( G! f# H; d
53.   {
    
54.    int Err = getErr();
    ! n  X9 B) l0 l! _
55.    if(D1 == 1)
    
56.    {
    . q1 x7 ?& _: ?( L0 ?
57.      Left = 1;
    . M" X6 E$ O: c9 v  H  C, \, S3 }
58.    }
    
59.    if(D6 == 1)
    & X$ j4 J3 \3 @4 w
60.    {
    $ I3 ^  b+ H3 |. v1 y- j+ K& h3 Q
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    
64.    {
    & m5 [+ N. k# c. Q$ c6 B! R5 e! {2 a
65.      if(Left == 1)
      k0 T- g$ M. e1 X
66.      {
    $ g7 i3 b0 s1 m, p8 a6 v
67.        Speed -= 5;
    
68.        moto(0,Speed);
    ; Y% q; I1 I9 h
69.        do
    
70.        {
    $ f: A3 S" `* G$ Q. K
71.          Err = getErr();
    : S5 l. F6 N+ c
72.        }while((D1+D6) == 0);
    4 U% j2 y1 S" D$ L8 Z. |9 y
73.      }
    / k* I3 {8 V4 O) ]1 J# T$ q" L
74.      else
    ) ?, J# _& x/ C/ q
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    6 i9 ^$ ]. v5 H5 m
78.        do
    ( v  z% {$ ]- w2 ?
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    6 C2 Z8 ~* O1 w3 v7 p0 j
84.    else
    
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    / k" F" g1 S# z: o: @
88.      integral = integral + Err;
    ; e8 k# a/ ]9 |5 b  h
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;
    ; B8 A& Q$ {+ ^0 r* V; n* j$ M( l
93.    }
    2 |' [# v* N+ w  n0 ^  s* c
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    , C7 ?3 H) `; h' y
97.   buzzer.tone(262, 500);
    ' J- ~; n! f  H7 r
98. }
    1 @7 E) G+ v- k8 v% q3 W. G
99. 
    % n+ A3 V+ w3 L1 a! I
100. int getErr()
     
101. {  
     & Z5 v3 N0 E, ~% D0 W( N
102.    sData = linefollower.getValue();
     . y! k& U& ~  i
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     1 f7 B* {9 k  \
105.    D3 = ~(sData>>2)&1;
     4 h# P6 n  r) j- A% B6 Z
106.    D4 = ~(sData>>3)&1;
     ' V+ ?& C3 k; E1 h) K
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     0 S, {1 o/ J( |3 o& a; N' f5 [3 e- d
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     & o' Q0 R% E9 D" `) r. F* O% |+ q
111.    if(downD == 0)
     % p5 ~! g0 ?2 z* G* r5 d
112.    {
     
113.      return 99;
     
114.    }
     
115.    else
     ; T" w4 u  N$ I" ?+ x# x
116.    {
     
117.     return upD/downD;
     . v, t7 p. o& \, B3 v4 s1 B9 ?
118.    }
     - c/ d; t9 N+ n( [3 E9 U
119. }
     : _% q& W0 s2 @  i! I4 Q4 F% C
120. 
     
121. void moto(int Speed_L,int Speed_R)
     0 d, }8 b! G0 D) \. b1 l
122. {
     5 j6 M/ r3 C- O/ X# x
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     8 d7 n5 f6 M6 e( T8 B* L) _
125. }

複製代碼

MeLineFollowerArray.cpp
" W: T9 h' j7 F$ F& o% E3 k0 w

1. #include "MeLineFollowerArray.h"
   
2. 
   3 p5 d+ e5 k2 Y. X" }2 [( O
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   % g* \) @- T7 t& |/ }' s
5. {
   & G& P) {! J0 d$ M9 i  R
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
     V+ U6 F6 J( G1 s3 q
9. {
   7 Z7 f6 X" I, x* u, E; P4 H; R
10.     _DataPin = mePort[port].s2;
    5 S# x3 ^6 |6 h4 ?  R
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    , x5 i% A, R1 Y8 V. Q; Q
13. }
    
14. #else // ME_PORT_DEFINED
    / \0 x0 \4 y5 T, n+ [* K! R
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    6 P2 a  `5 g0 N& ]2 ~4 d8 g
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
      X4 ]6 ]7 d7 V
20. }
    
21. #endif // ME_PORT_DEFINED
    " i: s$ v/ {( s! Q! X
22. 
    . y+ \: V, V, a4 N( k, O' L4 m3 J
23. 
    " D' I' E8 N/ E3 c9 _; Y/ q
24. void MeLineFollowerArray::setpin(uint8_t pin)
    " G4 V% |! V- l+ J! D* ~
25. {
    ) g4 s; ?+ y0 A; L/ c# u
26.     _DataPin = pin;
    0 y, x5 W5 {+ u% J
27.     pinMode(_DataPin, OUTPUT);
    - H3 [) t" P- m  Y2 K# T2 n1 K
28.     digitalWrite(_DataPin, HIGH);
    & H0 V5 C: k9 m, G6 a1 C- R
29. 
    9 W& b; J! y4 \2 [
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    1 @- h* E3 q+ \+ S% }- {: {
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    3 G& z; k  O- M/ W8 N
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    & u5 d; W1 P5 q
44.     digitalWrite(_DataPin, LOW);
    * j! t: ?: i9 b8 N5 f/ A" v/ O% F
45.     delayMicroseconds(980);
    ! ^  ?2 G. o8 {6 U
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    # k& z: O4 h6 K  J) a! N2 {- s
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ( B! @) k$ A. l; a7 \& E. p
53. 
      L7 @" i- O6 C
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    ) y0 r& F: n1 Y" V5 i. i& `+ a
57.         return 0xff;
    
58.     }
    
59. 
    2 w4 D% h+ @( v- y
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    # c# X5 b& Q* t( U( D& s
61. 
    ) W- G/ ^4 ^! P& X) s6 Y, h% k
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    / A4 [1 M: f1 f. x
64. 
    * f& D4 |$ K* M2 V" u# t
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    ) G/ {3 x! A8 W. ?
67.         return 0xff;
    * Q. x' y, S9 }! y3 ]
68.     }
    
69. 
    5 {$ O: c: q/ C  h+ X8 M# W
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    / _0 m# X. C3 d! V% @, l' Y# Q1 X
72.         return 0xff;
    % [6 M0 {7 Z9 F: W$ I
73.     }
    ! P) u6 D0 v2 Q; o% F* d4 N
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    2 R; a0 O8 @# f5 }( B
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    0 D/ |/ X+ z  H$ N7 |
80.     {
    7 W9 c5 G0 {9 x
81.         return 0xff;
    
82.     }
    
83. 
    # T2 z6 |  E; t6 p7 L# C
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    ) _' U1 l. v- S( m/ K% D1 N1 v3 j
85.     {
    
86.         return 0xff;
    5 e% a2 T2 G; N7 G3 j# W2 M
87.     }
    
88. 
    $ X( t. V  A0 j
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    ) `0 _$ Y6 o1 p  [8 K3 C# @3 H
91.         Sensor_Data[k] = 0x00;
    7 k7 C6 s" I! G& ~1 L7 x& A9 L+ o
92. 
    ) _7 [7 [  P- P
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. 
    0 H2 o- Y, M0 ]- `0 M/ n
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     
102.             }
     5 P- t7 b: k9 s7 o4 f) `
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     1 o4 O6 x5 i8 {' H" `
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     ; Q2 Z. O- j7 X- p: l
107. 
     * \! F0 d, l* c+ |  K) t% ?1 T
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     ) t- P3 Z% Z: j/ `9 ]
109.             {
     " j! j: c) `& t9 J' W: i: _
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     8 `( @1 R- y; L6 V$ j2 b% Y  e$ d( [
114.                 return 0xff;
     ) m/ u7 z# x9 ^6 q. [* ^
115.             }
     / B0 N6 j2 v$ R/ _
116. 
     ) p0 Y- U# s0 k
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     / x5 h# p( c  ~5 p
118.             {
     
119.                 return 0xff;
     . E3 P1 @. _7 J: K  c% G, k) [0 \5 k
120.             }
     ! A* J+ P# ~8 E$ @+ d' ?3 ]" ~
121.         }
     - `$ m4 y) s$ u* p2 K/ e
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();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     $ T2 ?& Q3 a, \- L. i7 E
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     % ]: A: u, l( I% J& f) k
130.         return 0xff;
     ( f. P; _! S  U, Z1 ?. I
131.     }
     8 P- s2 C$ N! g8 U
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     0 N8 ]- `& K/ `3 [: S" {" `8 F
134.     digitalWrite(_DataPin, HIGH);
     + F/ i1 M) R6 L$ \, V) \# n
135. 
     & `* z) K- z: G. ~  m9 z
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     9 ?& |. U! ]: s. P1 e3 \
137.     {
     
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     ! g& o. I9 I' P4 R* w/ p6 u
140.     }
     
141.     else
     % \# G$ k: @: B* l1 c% x4 B0 t( w
142.     {
     ) F/ f# |0 J5 Q. i
143.         return old_data;
     
144.     }
     
145. }
     

複製代碼

7 s* [3 o" M1 R+ ]) X/ a) \MeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

' o' U0 g0 |, z: z/ S

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
' b' U, x) F: |0 M8 n7 t4 j4 W哇...要100 個銅錢

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

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

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
1 d  }  i" |8 l0 Q- K7 h7 V1 J2 Q能否用到mblock 5 上面呢？

; a2 ?+ G8 o/ l# o/ k4 eMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手