mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   : p) b& r! u  K2 u; z/ C
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   ; g4 }2 F! A; P( a* [* [4 j0 U+ B
5. MeEncoderOnBoard Encoder_1(SLOT1);
   " A- ~! s  \6 t) y/ m9 U. J- U
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   ' j9 K4 B+ F  o1 u! Q
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    + l* c! a" N, E
12. #define Error1 1
    $ q2 D- j' E  J; G! ]6 K1 i# }$ M: p
13. #define Error2 2
    
14. #define Error3 3
    , w$ u/ T1 X8 W% e/ D/ s7 _
15. 
      H7 c8 Y. T. |% [' w
16. #define Kp 15
    ) i% A5 U9 Z1 `" K# i7 p$ A* z
17. #define Ki 0.15
    $ W$ D6 Y3 \( [5 Q7 w
18. #define Kd 0.03
    9 @3 l+ i4 e2 n) {$ Y" I- C3 k: _$ T
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    / ]4 F1 |; I% D( L5 W  c7 y. R; O0 b
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    : F4 I, X6 B$ G4 ]6 |) o: {: P
26. uint8_t D6;
    , c) B/ g( V- z, g% u
27. 
    7 A4 w; Y; M) N/ q# o* y/ [% }
28. float previous_error = 0;
    
29. float integral = 0;
    3 j, l! e  i9 s$ b; f1 @; e% R! ]
30. float derivative = 0;
    : G% k* a3 P0 d6 p
31. int  Speed  = 160;
    
32. float output;
    : e1 _2 h+ x  P/ v: U
33. 
    6 n) _( Q" r' \; T6 ]. b) z
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
      F. m/ f* |- h
42.   TCCR2B = _BV(CS21);
    0 Z  \6 B3 n. ]3 B: m0 s
43.   Serial.begin(9600);
    1 H( X. b: |' d, n# j; `! N8 k2 m
44.   buzzer.setpin(45);
    7 F8 t! ?$ K2 ]6 A: l' Q. {3 u
45. }
    7 Z- y7 P" a; X/ E
46. 
    7 @7 ?0 u, V9 g3 K/ W
47. void loop()
    3 ]* S3 B0 O- c! o/ f
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    . Y+ W6 ~1 _8 G4 h
50.   buzzer.tone(1047, 500);
    6 ]" x; G; D4 E( e) F
51.   delay(500);
    6 M1 q2 b3 Z7 a6 g
52.   do
      p9 t; j3 l* D2 m
53.   {
    2 }- w5 r! ^1 _4 h" t( x
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    % |8 H5 ?& T, D$ H  L% |
57.      Left = 1;
    8 m$ [; M6 k* f! V# x2 ^
58.    }
    
59.    if(D6 == 1)
    ) x  T" n5 U4 e# Y- t; ?0 g
60.    {
    
61.      Left = 0;
    / u. _$ d& u7 ~" _8 R
62.    }
    7 x- Z% P2 E2 c- z- h
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    # Y+ x" |+ N8 F' Y
66.      {
    / e; P% t' x! R9 a$ q  E
67.        Speed -= 5;
    " q( y% f" ~& U$ m# @+ R7 d
68.        moto(0,Speed);
    
69.        do
    
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    ; M( a* R, [1 \. M. E
73.      }
    
74.      else
    
75.      {
    + N" Y0 N% k( H* c4 w+ y
76.        Speed -= 5;
    4 {, L. M+ r* i4 s- G: N# u% n9 p
77.        moto(Speed,0);
    
78.        do
    & [6 Z! K/ D$ h3 u$ `! N$ @3 v" _
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    + _- D2 l7 v/ c' n" X4 Y# u
82.      }
    
83.    }
    
84.    else
    # V4 R' E( e! N' l% Q" S1 E5 B
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    8 \6 T) j$ n4 Q4 ~( @% D
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    9 a9 R" P! @; Q. m0 @
91.      moto(int(Speed-output),int(Speed+output));
    ( l- k/ H, ^* a0 c
92.      previous_error = Err;
    * H. L/ j: j1 q2 R! u/ K) A
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    9 _- Y2 i% ?! i, q, l6 J7 l
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    * |# `$ Z( J' o* Z& B
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     * C/ P# k7 m0 u+ T: i
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     ' E1 K! D4 ]! n* H1 `$ G
109.    int downD = D1+D2+D3+D4+D5+D6;
     . w8 [$ N4 F2 y- A1 z
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     - w' Q" {0 j& s" t# D$ X3 R+ {
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     
114.    }
     
115.    else
     
116.    {
     4 ^- i3 i+ ?2 v" k3 i' {, ^
117.     return upD/downD;
     
118.    }
     , k( J, Q" C: i+ ~" J
119. }
     " E* Q6 U0 d" W4 e" v2 j4 R
120. 
     # R+ C( F+ w2 s8 v2 y) [
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     : `& J3 m7 j5 S8 q- R
125. }

複製代碼

. J3 y" l- h1 ^/ _% v. hMeLineFollowerArray.cpp
, @9 H8 s& ?) X$ I3 X) |

1. #include "MeLineFollowerArray.h"
   , l) |& I0 ^2 S2 E
2. 
   3 a  R4 _+ n9 u& C- v
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   / i0 s: [- U4 ?: M$ I1 q$ V
5. {
   * Q% K/ s& _! h( @$ Z
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   . U# J4 }6 U& L/ Z1 K5 @6 I
9. {
   . _* ^8 {( p- \- P
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    3 J. ^; A. V( T5 n" ]
13. }
    2 P+ t* F- g5 ?
14. #else // ME_PORT_DEFINED
    0 m, o; g: q( T7 x2 p2 H. c
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    . Z; \6 q# d+ v: {5 F
17.     _DataPin = pin;
    % h3 U& X- S5 {
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    1 }2 D2 s2 d5 X! i
21. #endif // ME_PORT_DEFINED
    
22. 
    ) Y4 s* `0 C# K' K  e
23. 
    : D/ h2 w: `# L" G
24. void MeLineFollowerArray::setpin(uint8_t pin)
    * A! p! L/ I" E, A9 R# `: T
25. {
    0 E3 a. l: |9 |0 [& r
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    4 O( I* e/ w3 W) ]* P  \+ G
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    ' {7 F8 ^$ ^( }8 W
30.     #ifdef ME_PORT_DEFINED
    - n) k, I! O  t  Y( K
31.     s2 = pin;
    
32.     #endif
    ) [* N1 h' p& V4 }# G: v+ u
33. }
    ( V  S9 C8 K- B, P
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    5 j6 U3 a4 y  c- G7 y6 m9 F
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    & O( _7 {: i; M' b  o
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    ( l" y, h2 s: T* o2 Q
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    ( A+ w, m! U7 P9 k
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    6 H: D( h. T/ e& B* }
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    6 r/ U) j6 ~% C3 j0 t1 }9 d' D
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    ) c$ i% f- ?2 e: l: O% o: C6 ]$ q
57.         return 0xff;
    
58.     }
    # j4 `) g6 L) b' i2 \: o' ]8 v* K
59. 
    / b# R1 t' G2 r3 @5 V+ l9 G, f  j$ l
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    . ]6 L; l8 ~. f: n+ X& q+ V
61. 
    
62.     HIGH_level_read_time = micros();
    3 [: ]  _5 K; `% |( F& C- _
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ' R3 C& s# T) _
64. 
    " M2 V: I, U8 W, j
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    1 s7 w2 v2 ]; Y( T$ g) ^1 C
66.     {
    
67.         return 0xff;
    " u/ J- O, ~. ^/ m) `, W
68.     }
    * h3 B. b( j5 J, `) ?8 g& }
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    7 H2 \- d  d& K0 Y7 |3 Z
72.         return 0xff;
    # t+ J" T! x4 J& d7 J5 p
73.     }
    
74. 
    2 u- k2 P! i+ b' n
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. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    . x8 T) |' w% i1 ~4 K
81.         return 0xff;
    1 Z* G6 Z% ^: A9 g8 Z
82.     }
      U. W* \7 P- w% E$ H+ f, ?9 X$ o
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    * o8 A" W1 D- V
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    ( E) t: {' @# f4 k
91.         Sensor_Data[k] = 0x00;
    ) U# K. o8 I2 K5 E1 j/ W
92. 
    # O1 \; f; ?  T) N& L
93.         for(uint8_t i=0;i<8;i++)
    9 S0 D# A* o5 l$ V
94.         {
    4 N6 u/ U2 `9 ~
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    3 S5 P$ [* Y2 \9 w  n
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    " V7 M, L2 k3 p5 _/ \2 I# I. q& V
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     + [4 c- E/ f' l7 x4 R' M, e
101.                 return 0xff;
     ! `5 o2 H& d& e+ N4 d; {& W7 X2 T
102.             }
     8 h; s4 ^' R6 J+ v0 X6 R5 X
103. 
     $ ]- k# y& G' x
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     + R: v% }- Q% g( w% L
105.             LOW_level_read_time  = micros();
     7 z3 K, r, x) v3 O5 Y# `! c
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     $ j  B/ f% e  v$ z  V2 K
107. 
     : G/ n' W7 V' k
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     $ }# ^4 u- z7 y# T9 D7 U
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     / \. h3 u/ |. I3 c; b
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     - B+ s; z7 X' p+ @& d- j0 Y, b
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     9 k7 h. e8 u+ m0 n' v
119.                 return 0xff;
     
120.             }
     ( a2 G7 A% X$ U0 B
121.         }
     ' h/ n, d# }# M0 B" U* K8 A
122.     }
     
123. 
     & n; }! @% z5 O5 v) l9 |
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     ! J7 g/ i( L3 A$ }5 c
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     0 ^/ z- u( F, N2 }6 K5 V
129.     {
     3 M7 o5 H/ j4 b- K5 y0 [
130.         return 0xff;
     - P/ X8 _( h7 F1 \. r3 M
131.     }
     
132. 
     ' ^% H5 R2 K4 [, r
133.     pinMode(_DataPin, OUTPUT);
     : f: h# j" |8 S# U
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     & f7 P, z, L, D8 z
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     % C: h+ ^* d- }1 Q, A8 ]
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     6 \4 B3 C9 @9 j2 Y4 n! {% N
140.     }
     / J/ H) h9 {& X! ]
141.     else
     
142.     {
     
143.         return old_data;
     
144.     }
     
145. }
     + g4 E2 V% a' F8 N

複製代碼

' ]3 C/ O% c8 _. JMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

7 }. Y3 \: x% L0 T) `! B: H3 Z

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
) @; ]) z; s+ I) L7 y( ?% w哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
5 l" d% \8 z& S  B& G% A0 L! Z) I能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手