mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   2 i' I  Q! K- Z: q
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   3 y6 i9 Q; H; Q
6. MeEncoderOnBoard Encoder_2(SLOT2);
   2 o" d+ V5 i* G3 Y
7. MeLightSensor lightsensor_1(12);
   0 a- s2 ]  Q+ A
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
     j$ o* X" r/ r2 s# {9 o+ s. D
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    $ _3 i' |: c) U8 L( q" x
12. #define Error1 1
    9 \$ w* T- r/ v9 {
13. #define Error2 2
    % S. V% n6 J# S5 n4 Y  d7 d$ `9 z. d" z
14. #define Error3 3
    
15. 
    ) A9 h  `+ i2 w, A9 _8 k3 ~* l
16. #define Kp 15
    % A1 d6 x' u0 E7 Z/ R  K5 `
17. #define Ki 0.15
    . D8 U, T3 t1 p7 W  J2 a
18. #define Kd 0.03
    & c7 u+ ~: O3 H
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    2 E' ~; _$ i0 A; j
22. uint8_t D2;
    + z, q- @, V$ n! }- ^+ }7 s/ j
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    : x' e% T* M8 e
26. uint8_t D6;
      P2 E. G7 ~2 F8 a8 b- r2 `
27. 
    
28. float previous_error = 0;
    " D0 w7 s% I1 I4 ]  e/ ^
29. float integral = 0;
    ) k  ?) Y$ `" @, R. U* }% Z
30. float derivative = 0;
    
31. int  Speed  = 160;
    " G' f$ _! @' E% S5 j2 v
32. float output;
    5 x/ ?2 ?3 t) Z1 A  U- D& K. i
33. 
    " J% N6 p+ ^. s% \& _
34. byte Left;
    2 ~) P' v" O7 K! }8 K/ P
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    # M7 v' {) m* @  j, \8 l
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    0 G2 u1 H9 ~% x' z6 s
43.   Serial.begin(9600);
    * b% f9 V& F% ?% b' g
44.   buzzer.setpin(45);
    
45. }
    " i3 R' ?) L$ f, K( O" H0 y
46. 
    . w/ l4 W  f3 q
47. void loop()
    0 N; M# p% O) ~. S: X
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    " J/ G0 Z/ M+ a
54.    int Err = getErr();
    
55.    if(D1 == 1)
    ( t3 W, r9 {7 z+ k; F# t0 v' u
56.    {
    
57.      Left = 1;
    # |8 y; x1 X  O
58.    }
    0 e& u8 r4 j& r
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    8 P: Q* B  O6 |/ D
62.    }
    
63.    if(Err == 99)
    9 u! m3 A7 e$ Z& c" C1 w( E2 S
64.    {
    
65.      if(Left == 1)
    3 V9 r$ ?$ w; T8 J: o5 D2 F
66.      {
    
67.        Speed -= 5;
    . s& y1 X+ y9 x+ c6 g' I( t$ W
68.        moto(0,Speed);
    
69.        do
    
70.        {
    + X3 v7 r- a% c8 K' j$ T+ t
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    . ^; G- D, c3 Y) ?8 d
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
    & b2 s& f; I4 I9 C0 L" Z
80.          Err = getErr();
      o: ]/ u, X0 T% f8 G5 d
81.        }while((D1+D6) == 0);
    
82.      }
    3 G" T% W! O; m: s5 y. o! }3 I
83.    }
    $ Q3 x  r8 n$ Y0 t- P
84.    else
    
85.    {
    7 Y. E9 y$ ~: V" m) F& i
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;
    % [; V. O9 b+ \3 l7 V) J
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    & w/ c  @" U1 f2 X7 z+ L  t
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    & B, ?9 S$ \1 M1 g+ E* ^
98. }
    
99. 
    6 S. n  h+ s  Q2 i( r
100. int getErr()
     
101. {  
     " f9 c; B, K  @0 D3 O* v
102.    sData = linefollower.getValue();
     7 O+ r5 Z) x, M" n# R
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     0 b2 w( ]. V+ h- l6 ~/ }" f
109.    int downD = D1+D2+D3+D4+D5+D6;
     
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     3 Y1 ~0 [; X3 P' c! e1 I# J+ s
113.      return 99;
     
114.    }
     ( A' j7 x  l/ q
115.    else
     + j9 [! ~9 R" O9 V. d5 F* G" ^
116.    {
     & F, `1 K; {2 S1 X- m2 i" c
117.     return upD/downD;
     $ E2 j9 Y- T% F( G7 [
118.    }
     ; _+ ~& t: l! ^1 ^9 j1 k! m6 g
119. }
     ! G2 z- ^1 _- |; `3 q
120. 
     
121. void moto(int Speed_L,int Speed_R)
     , z5 ^8 G: D# e% i1 d
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     - V) W% h; R2 [2 d  n, a
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   ; k2 a) U0 ^" P4 K5 K0 @5 ^5 E. l2 i
5. {
   4 M! s/ h' x! M5 B
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   - F2 h) X7 j" Z5 x' ?+ Y
10.     _DataPin = mePort[port].s2;
    7 z5 `/ u  z5 m; u" y7 I
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    7 `: [, y: P& E+ O
13. }
    & k. B& a, ~* H( {7 F$ p
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    ( E2 ~5 s, ?, E5 {  i
16. {
    
17.     _DataPin = pin;
    3 m2 I' p% K: H5 j0 b, o  Y
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    / D/ R. m/ B  x. ^
22. 
    
23. 
    6 ~$ a2 n1 J1 d7 l! M* A
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    # Z% d, S1 [9 Z; N% m: y0 M
26.     _DataPin = pin;
    0 p% J$ S/ A6 w) i
27.     pinMode(_DataPin, OUTPUT);
    . h0 l* C; L) ]& [6 }5 v4 p
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    / H  N2 m0 f2 |
30.     #ifdef ME_PORT_DEFINED
    % V7 {  Q6 f* d% v$ x7 O- @5 h
31.     s2 = pin;
    ! W( w& A% P+ ^6 a+ H: Y( m
32.     #endif
    , ]- a# V' u( B6 ?
33. }
    
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    . X# @% r8 P6 [( N! h# \" S# Z
36. {
    ) }3 u4 x" r) O0 d
37.     uint32_t LOW_level_read_time;
    & Q1 ]8 r; Y* T/ K# M0 X
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    3 q# P4 i6 F/ c+ v1 p) O" l
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    ' T2 h/ {: N; b6 G( z( }$ f9 i6 f
44.     digitalWrite(_DataPin, LOW);
    ) _% Z$ W" D6 ?4 P- R3 m0 @4 j# V
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    ( ~9 P7 H; `" {' d! A! |
47. 
    3 C2 o) o2 z* o$ r* y
48.     pinMode(_DataPin, INPUT_PULLUP);
    ' q; R+ I3 Y7 g8 i
49.     delayMicroseconds(10);
    / o2 N7 x; E: @
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    7 b0 t* `2 n$ i
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    , N- B+ g( f  C( f" C
57.         return 0xff;
    
58.     }
    
59. 
    - p5 Y# w1 _% ]0 {$ J
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    . W, G0 l3 ?4 W% O4 @: Z, A
61. 
    ' Y. x9 I! e9 ^4 @* Q7 D' a
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    # k# S+ o. ~* t: h; v& k6 U' k/ l. |
67.         return 0xff;
    $ ]; r( Q+ o6 ?$ T! x5 G. H
68.     }
    % m' ]+ N2 H1 v+ h. f6 u+ k7 Q/ n6 p
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    $ ]& z/ j2 z3 h/ }3 @
71.     {
    
72.         return 0xff;
    
73.     }
    
74. 
      _+ \5 K* ~# n4 x0 P7 H
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    " X$ r( n1 t( b( \; i# U
76.     LOW_level_read_time  = micros();
    3 j, V) o8 M  J; D
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
    " h' T& T% M) S" @: ]/ w
80.     {
    ! o; l; L% W7 W8 Y  M2 e) S. S2 p
81.         return 0xff;
    6 U6 B0 X1 [7 Q$ v' }/ @8 q( R
82.     }
    3 z2 o( A( b, O" Q' q& I/ Q1 Z$ ?$ o
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    % f4 d. I" C. ~. v& p6 ^0 U
91.         Sensor_Data[k] = 0x00;
    + a6 [/ N: d; c  A
92. 
      J6 L; R' k. y. u3 x
93.         for(uint8_t i=0;i<8;i++)
    3 w# o. U, v2 Z+ H
94.         {
    , p, K' d' B' n0 Z( q
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    & E  i% d" m4 X7 H3 X
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    . _$ s  U* r! z3 F* R$ l
98. 
    + n. G% K) T! @! d6 x( g+ j
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     + j, d3 V2 k* T2 Z3 }
101.                 return 0xff;
     ! s! ]9 i$ z( L, ~% c3 I' W+ O
102.             }
     4 k1 B" g1 T2 x
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     8 U9 A. I" \* @, ?6 E
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     & p% v, x4 I9 J, D+ H
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     & v0 V9 |' s* R  Z9 S3 Z
115.             }
     
116. 
     ' d! ]) ]4 b* d
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     * m& i, _! O6 @* ~9 H) q8 ^1 b: `% l
120.             }
     
121.         }
     
122.     }
     
123. 
     9 t0 e) i8 @1 U- Q' N
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     * j7 ?& u; ^* C0 u" a
125.     HIGH_level_read_time = micros();
     ! A5 @9 A! h+ [- O1 Z( B" H. f
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     3 x5 p5 z8 E2 }2 Y. U( @, M8 a
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     7 x: A* e3 S. \. J
129.     {
     + d: y/ p  L7 Z% C; d8 Z: @
130.         return 0xff;
     8 B( X+ C$ M7 E7 B, ?8 M# M
131.     }
       T, l' S+ b% v1 O
132. 
     # o' g: Q9 d1 P/ C4 i
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     7 s$ B3 A  x8 x% A8 e9 F8 E
135. 
     4 H* [, A; j6 r  b" ^8 O
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     + D% \7 u2 t9 y6 k# O" {* c& @
137.     {
     
138.         old_data = Sensor_Data[0];
     ) l7 z9 o7 \8 }' @& z  z
139.         return Sensor_Data[0];
     
140.     }
     ; N/ F7 ^/ f' m8 X2 y; }
141.     else
     $ u" j% c- N3 S; Q' w  n
142.     {
     
143.         return old_data;
     
144.     }
     0 }. F4 d7 M  z' p% ^% V( P
145. }
     

複製代碼

: P" y0 g/ }/ d/ g% n9 U* o: YMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

; Z$ Y5 t9 m  M% C7 I2 K) {! m
0 ]# w; p. b+ W, a% Y+ p

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
# k7 v  @) C. @  Y哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
- N" R  l5 {. f- |8 }/ C能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手