mBot Ranger 高速循線範例＿

( E- E! @! }& B& _# I2 Y5 u

1. #include <Arduino.h>
   ) W9 u$ ~' I' C. |( a" ^# Q7 Y/ {
2. #include <MeAuriga.h>
     e" E6 \3 c; T; k7 \( i. s
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   * @+ U" ~. k, a! h: S
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   : r3 X9 X: W2 G; ^6 _' x( ~) z
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   : P" S4 G+ m! a7 c* q
10. MeLineFollowerArray linefollower(PORT_6);
    1 i8 {5 ~$ Q8 E/ |" M5 \$ {/ l
11. 
    ) M9 a4 ~( S2 ~- `; P* ^
12. #define Error1 1
    
13. #define Error2 2
    * |# C. S& ?- t& g4 l" [
14. #define Error3 3
      z# G0 T( b5 O2 C  R. S6 M" X1 L; z
15. 
    7 A  M5 i6 e# ]) M( r/ C  E& [* V
16. #define Kp 15
    0 [$ G6 D+ @/ o+ j
17. #define Ki 0.15
    5 Y9 p; D* r; }0 @8 u# F$ P
18. #define Kd 0.03
    0 h6 z8 P6 y* A$ f' J6 k% k) }
19. 
    ; l- K( E2 W# G, d
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    & _4 t* B: i! j% O0 U/ X: ?/ ?
24. uint8_t D4;
    ' _, l; e. |  w2 B8 d
25. uint8_t D5;
    2 i3 w3 `. B& P5 N# x
26. uint8_t D6;
    + p, K) p) ?( E" I2 Z1 W
27. 
    
28. float previous_error = 0;
    0 u' m0 @7 k) R* {: E3 H% m
29. float integral = 0;
    
30. float derivative = 0;
    1 E% f, |* j1 ?  y& y
31. int  Speed  = 160;
    # j5 F/ K! A" W, M$ j, K7 z5 U
32. float output;
    
33. 
    4 M% b3 i: B* k" {* E$ {
34. byte Left;
    
35. 
    ; ~5 f) r1 T: x; f& D6 h; f
36. void setup()
    2 j( y9 f/ p. J( T4 @' w7 {* S
37. {
    4 u/ w! E& |" p4 M  S) h; i
38. //Set PWM 8KHz
    . h9 N) a' w( H( M
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    1 l& d" A# [& r  F6 h
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    3 @3 Z! d% ?. X
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    , R: f3 c$ `- F" w3 r5 D; a
44.   buzzer.setpin(45);
    9 J- e( D4 f3 h5 o
45. }
    
46. 
    $ Z6 F1 e: i% u. j  M: w
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    7 a& I& p* c9 i4 I, |$ w: H9 l. \
50.   buzzer.tone(1047, 500);
    8 z# u# q$ K' L& V* L2 }! W$ Z
51.   delay(500);
    / I/ V9 R+ x$ A
52.   do
    % C+ O# \6 y- r" t0 E
53.   {
    ) v$ q5 e0 ~- ~: ~' u
54.    int Err = getErr();
    ) g2 {8 Z; M5 I3 h: A  ~* P
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    " d; v, D0 ^' ^
59.    if(D6 == 1)
    - s: ~/ D- z$ _: _/ p0 G
60.    {
    & j# w( n  @4 `2 ]
61.      Left = 0;
    9 y* J0 _% O/ N) ~( A" Q+ w* K
62.    }
    
63.    if(Err == 99)
    ; W$ ^9 B, V/ E* L! c- E4 r
64.    {
    + o" y/ F" O- o/ S. \$ @3 ]' P  r1 S
65.      if(Left == 1)
    
66.      {
    , D# s3 a4 o; P$ G' v
67.        Speed -= 5;
    1 z$ ?+ L6 [. U* R
68.        moto(0,Speed);
    ' p" U( x% r2 f2 r: p5 p, ^% m
69.        do
    3 X, J1 ~6 j5 {' k, r  f
70.        {
    
71.          Err = getErr();
    6 z6 U. z. l1 _+ a6 o5 T' W. G7 A
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    + S& N' H/ Y! g) G5 ^2 A( Y$ N" A
75.      {
    ) l3 v2 S6 A0 T0 t' m
76.        Speed -= 5;
    
77.        moto(Speed,0);
    ' \  X/ h2 l2 a3 m1 B4 O2 j6 r* p
78.        do
    5 V; |' ^1 ~0 R. C
79.        {
    
80.          Err = getErr();
    6 q+ i1 O0 Y& \/ P* E3 p
81.        }while((D1+D6) == 0);
    
82.      }
    8 K$ w8 i& \& ]  s  Z7 n# n! k
83.    }
    7 ~' D5 D) Z% {  C5 ^% b. U8 C1 A
84.    else
    
85.    {
    " C  K/ f8 B2 Y2 e$ M" B
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    6 j, w( L4 o) V8 Y
88.      integral = integral + Err;
      m+ R9 m% N2 L! c4 \
89.      derivative = Err - previous_error;
    9 S1 ~; t4 T8 k) I, W
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    - U" q% L, F6 s9 C
91.      moto(int(Speed-output),int(Speed+output));
    * Z( G0 H" e3 j/ A
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
      `- K& x! R$ Q: i
98. }
    
99. 
    3 s  B( v( T' p! i: [- c
100. int getErr()
     ; X; y: s0 w! _7 D% G7 V3 D
101. {  
     " D! T& e, u3 ^4 |$ L0 d; }: C! d
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     9 w& O  R  \* `! {+ k" {$ g
104.    D2 = ~(sData>>1)&1;
       r' |5 J  M2 ^, F" J  j
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     8 y+ ?0 Z0 c7 h5 o- k' k( i
107.    D5 = ~(sData>>4)&1;
     % w* N& r  M3 R
108.    D6 = ~(sData>>5)&1;
     + T5 v/ o! x8 x
109.    int downD = D1+D2+D3+D4+D5+D6;
     5 l* I% e0 B9 x" k9 p) s0 B
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     4 F1 k) c& n( u
112.    {
     / p# v0 D* z# u. J
113.      return 99;
     
114.    }
     
115.    else
     
116.    {
     
117.     return upD/downD;
     6 @# f8 S- q% D/ x/ {9 ^9 m
118.    }
     - H/ R0 p' o: Q& D$ V: K8 [  u' n, n
119. }
     - O( g( n, [5 F% P% X  s4 ]0 n* m
120. 
     5 P+ k! d7 p8 C  p* v5 e5 `
121. void moto(int Speed_L,int Speed_R)
     6 @; q0 n" C! `5 [" \4 ?
122. {
     1 e/ D- w  _7 F  F* q3 v
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

$ `* N/ o, K4 D' Q& L9 Z2 aMeLineFollowerArray.cpp
. J8 {' f0 m: g+ N1 f

1. #include "MeLineFollowerArray.h"
   # i0 ~8 i1 l. k1 ?
2. 
   % x$ A, R/ ?7 M2 |
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   - C; J, B) y! ?9 g5 l
5. {
   
6. 
   - ^# z+ s8 p# f& \* }) j
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   6 o$ a* {+ x7 _9 |4 R: ]- N. m. [: M
9. {
   
10.     _DataPin = mePort[port].s2;
    4 w( u* d  m1 |) H6 ~
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    9 Z& \1 Y+ E. r: V0 @
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    . O7 y0 ]8 X2 j7 V$ E
16. {
    . l: a4 K0 M" t* a2 Y
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    - i7 P" N. P2 |/ i  _% j: D1 P
21. #endif // ME_PORT_DEFINED
    ; B  K3 k$ Z, w' \& [: T
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    $ V' u9 p+ D2 j5 k1 L) L
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    $ K" Q1 y5 {( v0 E
28.     digitalWrite(_DataPin, HIGH);
    + J( |; f3 F- a5 Y
29. 
    " V' ]7 P- s: s( u! R. G
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    
34. 
    9 @% |, \; s) Q: s' s
35. uint8_t MeLineFollowerArray::getValue()
    8 w$ Y9 d* v/ \8 M
36. {
    % f) v* i9 t5 q- n+ g: o
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    % z7 |- E6 J8 d0 O7 ?" Q
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    $ [6 f0 z/ j9 u$ j, X  }
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    7 z$ ~) G7 X  G! w) x* o
45.     delayMicroseconds(980);
    1 W7 K! N3 h+ j# [* \4 n
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    , x3 f; v7 }( L* i% R6 ^0 w( @
48.     pinMode(_DataPin, INPUT_PULLUP);
    3 {3 a( N# k5 \0 K
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ( V4 {! Q& P1 h% S8 @" Z4 T$ h
53. 
    
54.     LOW_level_read_time = micros();
    . ]! h2 G4 c7 @: K# b
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    . q7 C, a  r/ f% i8 H0 d8 y! |
56.     {
    
57.         return 0xff;
    
58.     }
    9 W/ g+ v& f+ s" F; S
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    8 B" b) `  O" X2 E: U. e
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ; W6 y! d" A4 G/ Z" T" \, m2 V  Q
64. 
    5 L5 n* x( ?/ h" l
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    
68.     }
    
69. 
    - r1 U* H9 C( u) P- Y. }/ V
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    0 g- M/ k6 p, e& E
72.         return 0xff;
    
73.     }
    
74. 
    # x7 G3 ?% I" m7 C1 W0 w! X3 ?; Y
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ! l* Z' G2 ?% N/ Z/ M% s9 V
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    * {$ Z7 v" p6 R& S8 ?
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    % g* ]3 m: Y5 W& B( W! |) ^" m4 y
83. 
    9 x+ ~, ?- U% l* `" i+ V
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    7 ]! D0 j/ ?$ E
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    : Z# b5 N' I) b* G5 `4 v4 a
89.     for(uint8_t k=0; k<3; k++)
    , Y1 ]( {2 I- G4 z
90.     {
    0 i7 p/ _( q5 u
91.         Sensor_Data[k] = 0x00;
    ( X4 C2 q; x& c! o# Q3 K3 ]) M
92. 
    ' B$ g( Y# e8 p0 M
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();
    ( s# v/ q# }" z/ t7 `" }
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    8 z. ^9 ~/ u: n2 G$ v
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    7 h2 g1 T7 f+ s: F4 q$ L
100.             {
     9 }" @6 S: M+ K# Z4 W9 @
101.                 return 0xff;
     
102.             }
     6 f& X1 n; i/ u
103. 
     : |; C6 M: Z! E, H: y' R! {
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
       a; r% n( K/ G! L& y+ S
107. 
     - @+ w6 ^9 X$ U2 H6 p
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     1 t" f* E; M; Z3 q- K5 e4 m; C
109.             {
     % b" ~6 }. d  i' Z( x* N& t- z( \3 m
110.                 Sensor_Data[k] |= (0x80 >> i);
     * ]" e( J$ h5 _$ I5 j5 e. T) a
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     3 ^7 `7 A" l8 @1 h# R
118.             {
     ( O0 M6 T  K1 S: _9 D0 m7 q. K0 S# K
119.                 return 0xff;
     2 O2 ^4 }6 }! ?, [0 a& y7 |3 u: [
120.             }
     , C" t" W" j' h4 `  U, x
121.         }
     ; G) K( @/ z( F9 T) E. N
122.     }
     / q) C& v  `$ Q; U$ `7 h
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     3 Q/ C: l8 ?' V2 ]8 [! u
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     ! G0 s( ?6 e2 S
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     6 o: d0 ?+ q* O9 _% z+ @6 a/ _
129.     {
     
130.         return 0xff;
     
131.     }
     8 r- w* p4 s$ Y. s7 @! S4 j
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     . f0 \, g4 \6 g; ^- K
134.     digitalWrite(_DataPin, HIGH);
     1 }, U3 {. y6 g
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     " `& j: G3 B, o
137.     {
     6 a  _6 ?( q  S: i, R1 n! h9 m# [
138.         old_data = Sensor_Data[0];
     3 A' u, H! k8 e4 r4 w( j
139.         return Sensor_Data[0];
     
140.     }
     % V4 g1 b, v8 m, g2 y1 o
141.     else
     
142.     {
     . B. U$ Z. I3 P! U) e
143.         return old_data;
     5 ~& R% h: N) c9 j. G6 C9 z1 I+ N5 w
144.     }
     
145. }
     

複製代碼

8 I+ i! g1 R2 oMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

+ v9 Z+ {+ n" Q: y) ~; j

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
' w( Y- L9 K9 a1 g5 x% L哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

9 c* O) I0 ^; Z9 Y8 T/ w- q5 ?- IMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手