mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   , r5 u. D8 Y( V  H( \8 Y& P
4. 
   1 b; W& \/ L8 F3 M/ j% O$ D
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   8 M& y/ \  z2 p" @
7. MeLightSensor lightsensor_1(12);
   ' |& A" ]! h) z9 S, p* k
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   ; w% B; F' d: @3 _) f! z% o
10. MeLineFollowerArray linefollower(PORT_6);
    ) [" D. p5 U, a! t' t
11. 
    
12. #define Error1 1
    
13. #define Error2 2
    # q8 _3 t. z8 U+ T' {4 q& {" x& h
14. #define Error3 3
    / V1 c4 _/ M" ^5 g5 V
15. 
    + s; Q# L# J# ^0 \8 U
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    3 C1 h; G4 S5 s4 J: e( U0 X
20. uint8_t sData;
    4 x& c; P( G, ?2 b3 z9 j5 T
21. uint8_t D1;
    ! w6 M7 _3 O$ D/ ^  ^$ n
22. uint8_t D2;
    
23. uint8_t D3;
    9 N- `7 i( g& m+ N- y
24. uint8_t D4;
    * q' H3 Z7 M# |7 I( d
25. uint8_t D5;
    
26. uint8_t D6;
    # p1 i" O3 d2 o3 K" ~" z4 E
27. 
    1 {2 N7 m% v2 B5 ~+ r: ^( S" y( e
28. float previous_error = 0;
    - R# M" m( A/ H$ R8 ]: c( s/ d
29. float integral = 0;
    
30. float derivative = 0;
    . }' _, ~) ?$ _1 T, F7 T8 z  E
31. int  Speed  = 160;
    
32. float output;
    * {  F9 }4 d% {
33. 
    
34. byte Left;
    - g! \. ?, T8 L8 t& A
35. 
    
36. void setup()
      _% R  k0 s" r# v# w5 n* S& {) O: _
37. {
    ) Z1 w6 s8 N8 x  [0 c" L' N
38. //Set PWM 8KHz
    / h& A% w/ @- B( ?( l$ S
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);
    # I' B* a; @5 {* S. s5 o
45. }
    & E6 }% M# T( m4 x
46. 
    $ M/ u* p$ H' I, W: k
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    / B3 r- l7 ~/ u1 Z/ V% Q. p  C
51.   delay(500);
    * r! ?3 k: `/ b# }7 ~# }9 n) Y* j
52.   do
    6 S6 q1 O4 ^% H7 o$ `! d, @
53.   {
    
54.    int Err = getErr();
    1 `' I2 ]) y+ H% P: ~. S6 x+ X
55.    if(D1 == 1)
    
56.    {
    * a1 f% f0 U  T6 h; q
57.      Left = 1;
    
58.    }
    1 a6 O; v- g! f& F
59.    if(D6 == 1)
    
60.    {
    / o0 D, N& x5 }. Y. h# S
61.      Left = 0;
    
62.    }
      e0 [+ R. I$ u: p
63.    if(Err == 99)
    ; e! @/ A/ d3 |! z; I/ @% q
64.    {
    + D$ a6 V) C+ B( \, ~. N: `
65.      if(Left == 1)
    5 _/ l4 E; a7 U2 T6 G2 R
66.      {
    ; F( W& U4 r5 a1 `
67.        Speed -= 5;
    
68.        moto(0,Speed);
    + S5 Q9 n  W6 d
69.        do
    5 e  P) O* ?' q# ~4 P; k, S
70.        {
    0 \) W5 I' \# P% Z) w
71.          Err = getErr();
    1 S5 h7 x9 h4 I! P
72.        }while((D1+D6) == 0);
    , T# @% o6 Z6 R, p, E, p7 d
73.      }
    : k' J9 O. m" c7 n" o  \
74.      else
    
75.      {
      M3 f' X. g9 g7 z  C& l
76.        Speed -= 5;
    / [/ X' @$ f: I) l% X4 ^: G
77.        moto(Speed,0);
    
78.        do
    
79.        {
    ( x& m! O9 M' C$ n& |8 a* m+ c/ j
80.          Err = getErr();
    # d! ~4 _1 z. I) e
81.        }while((D1+D6) == 0);
    ) O. [4 [4 h6 O& V
82.      }
    9 j$ X' ~% W4 Q7 l1 d# Q
83.    }
    $ T5 Q8 l! s3 O( f0 V
84.    else
    ! a2 p9 x/ F* ]2 ~1 ]
85.    {
    / g/ t7 U& W0 E6 N* @
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    4 h+ }! ^2 ]! {" v! u
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    & D1 X3 E. R( x: A. d3 h
93.    }
    " f/ _" C  C; i8 f; b' Y
94.   }while(!((lightsensor_2.read()) < (10)));
    ( M; g9 h% O6 d& j2 w! T
95.   moto(0,0);
    " b$ V. v3 ?$ R, v/ d9 k4 _8 W4 i9 l
96.   delay(500);
    / M+ o3 g+ x  g. T
97.   buzzer.tone(262, 500);
    
98. }
    8 r. B  b5 w  N5 G1 J7 R
99. 
    
100. int getErr()
     6 J2 b  [, ~# m9 r8 ~
101. {  
     
102.    sData = linefollower.getValue();
     9 v1 w5 I& [6 X8 ~2 T
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     % w8 v& `  h* v% w( @0 Z) ?
106.    D4 = ~(sData>>3)&1;
       B; _7 h3 i( A* k
107.    D5 = ~(sData>>4)&1;
     # f8 q2 ]1 t1 v1 @
108.    D6 = ~(sData>>5)&1;
     ( i+ D* k9 o# k8 Z- ?4 k
109.    int downD = D1+D2+D3+D4+D5+D6;
     + }+ X. y5 R1 E9 f( W7 ]
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     ! |$ N. j/ P. N" p; h' ~" i, @
113.      return 99;
     
114.    }
     
115.    else
     6 m. Q+ e% j0 L3 @
116.    {
     
117.     return upD/downD;
       z5 \9 k! j' q1 P! S4 b4 E
118.    }
     
119. }
       B. |1 ?! _) |$ e
120. 
     1 |4 E6 ]4 L0 r. f8 t
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     " h2 g& m8 p" a! q
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   % }8 z, U& j6 F, d5 i8 X/ L! T( |
2. 
   
3. #ifdef ME_PORT_DEFINED
   5 P* _0 s+ _0 {+ g
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   ) a; f: Y: z9 `# }8 H3 J; ]% n- D
6. 
   " |; U+ x% Y, ]: O5 ^9 c+ Z
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   + z3 T0 L2 u1 W3 P
9. {
   " C) |/ p. g0 {" y
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    ) \4 r, I  }, @% T& u: ^
16. {
    
17.     _DataPin = pin;
    9 q- B9 g- H+ `! e: ?
18.     pinMode(_DataPin, OUTPUT);
    - }0 B2 M8 u3 U8 ~* A/ A& W0 z
19.     digitalWrite(_DataPin, HIGH);
    7 C2 e8 w# A6 I/ n! a
20. }
    0 e, x- b, ^' }3 _: R) a5 s6 ^& \
21. #endif // ME_PORT_DEFINED
    6 i$ p% }0 n0 M2 f( ^3 `$ O% M( q
22. 
    
23. 
    ' {  o6 ~$ _# t; R9 g2 D
24. void MeLineFollowerArray::setpin(uint8_t pin)
    9 l: s$ A/ u5 E9 d
25. {
    8 F& d5 M& {' q' Q% {* c
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    $ T8 `/ X/ D2 P0 N2 w
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    8 c8 l' `" G- p' _; c8 I2 [
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
      V6 b! r  d& `: s/ z6 A1 Z9 j2 k! L
37.     uint32_t LOW_level_read_time;
    
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;
    ; r5 W/ R, e) b  u* v5 N+ C
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    9 H0 a! D& H$ r7 j% ?. S( p& K  b
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    3 Y0 }( W1 c* }+ D6 H
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    ' ?) s+ W& u% C1 N
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    3 i5 R; B  p. o
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ! t) \" l+ s4 r2 S7 {* ?" @( O
56.     {
    : P5 p0 C. O. f$ g
57.         return 0xff;
    : P' A$ a2 b4 R- O; j
58.     }
    ) F: }* L* w! D) n3 p% Z3 ]6 y3 D
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    1 R4 Y) D1 `4 H- {7 ^" l
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    2 ]5 r" N. m  t
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    9 p1 y+ m7 J, c- [
66.     {
    - @! m* X7 q% T' M1 \, ]* m! Z, d
67.         return 0xff;
    
68.     }
    ! p0 b/ i* b* J9 U& H: w- r
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    2 |6 b6 A- {8 }# v/ Z
71.     {
    
72.         return 0xff;
    6 e# @5 F  L8 w3 Y2 J5 w0 \7 A
73.     }
    1 `7 I9 ~$ V$ i% ^( B: M- p6 S
74. 
    % S: b& }  A$ L  Y* N) n
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    5 x2 Y' R* p  A( P5 |) j; {
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.     {
    8 }' \; _8 r  r$ M" [2 {7 [
81.         return 0xff;
    
82.     }
    - W' W2 G/ o' Q% {4 }
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    ; |1 ^. |5 c+ ?5 p5 f! Z( w9 |
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    8 m2 k0 l0 h% o( ?/ S
91.         Sensor_Data[k] = 0x00;
    * M. C9 R$ \( f: M
92. 
    - e/ i7 \- Y0 I' p1 l
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    $ E' v/ ?) F/ V
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    0 m$ K( g8 ^' F3 Z! `
96.             HIGH_level_read_time = micros();
    % b3 o  q  i$ C% n- J5 f3 ?
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    3 @8 g* ~5 i$ T
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     ' t! T8 J0 ?! g1 i7 ~& M6 Y; l' F
101.                 return 0xff;
     
102.             }
     
103. 
     9 _( A: M+ J2 P' z
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     5 x/ |# Z& Y4 q& {; y5 T
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     3 ~2 \$ [2 u) f
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     0 W- F+ b1 E$ I& v4 \- L- Y
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     ' L( @& _& u1 a- }; Q% c2 G
113.             {
     ' z( b) }8 U% ^
114.                 return 0xff;
     
115.             }
     
116. 
     7 o- Q( ^9 L+ e, L
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     ; o  E  ?% Q- V8 S0 i* I+ k( x
118.             {
     ; V* J) X* R8 U0 y( A# {
119.                 return 0xff;
     6 K9 H6 |& u; R
120.             }
     
121.         }
     
122.     }
       ?+ g1 Z$ a) d% a6 C, }+ Q
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     , O7 q1 O6 i" z1 a: e  G# n" c& O" b6 r
125.     HIGH_level_read_time = micros();
     & f/ ]0 ?% `: i
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     / {0 d4 c8 z0 E/ p  }
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     " `0 |# m, D" G# t1 Q+ Y
129.     {
     
130.         return 0xff;
     ; P0 U! s+ `, x3 a9 x
131.     }
     . [+ [6 V; G9 M5 \, `
132. 
     : _1 b. E* d# q. P1 j6 w& w
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     $ i; C7 t: L' `& a- l1 B* G7 E
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     ; X; Q8 A. {( c
137.     {
     4 |2 |0 O" ^2 n$ n4 v; O$ P
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     ! S; n' M, y9 `! c9 Z) F: X3 v
141.     else
     
142.     {
     
143.         return old_data;
     
144.     }
     
145. }
     

複製代碼

1 u/ X0 \2 x$ K2 {; QMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

  z4 ^. M8 Q: Y3 A" J4 D2 g; K* r
6 @2 b1 I0 D7 H  ~; @* L

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
+ n3 z  Q% o, G- {; Q哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手