mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   + b; C2 _  `4 l. K) Q( u8 ?! y+ S- A
5. MeEncoderOnBoard Encoder_1(SLOT1);
   7 \) h& D, D! t7 t' K% ?. a5 x
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ; L! J. I( Z; f6 ?- R/ j
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   9 n: F1 G+ }* o* ^2 f4 ^6 a6 z9 ^
9. MeBuzzer buzzer;
   0 ?8 ?4 p* q" d1 {9 H6 [- h9 C4 R
10. MeLineFollowerArray linefollower(PORT_6);
    / i0 E& V1 l$ |$ T! M3 T6 s( Q
11. 
    
12. #define Error1 1
    * G& f+ O+ A! K& p6 \
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    - t" g; E7 e/ U; K9 r: {
19. 
    ; H; S+ x! `2 X& h) r
20. uint8_t sData;
    0 Q+ \6 ?6 B1 c3 ]
21. uint8_t D1;
    3 G" z. A4 O5 R7 o# x
22. uint8_t D2;
    9 g  P4 ~5 n1 S' X, m( \
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    3 _7 S: S0 R* V
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    $ _- E3 M' Z) d
29. float integral = 0;
    ) a2 d- ]4 ?1 \" v, m0 c4 L
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    / b3 m% e, n0 Z3 r
33. 
    
34. byte Left;
    
35. 
    ) B& ^8 ^; K% M7 w
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    % x& ~0 k! {+ ^; [
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    1 C! r' d" k7 \( e- M
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    6 t4 ^; c& j2 ~  ]9 W& v9 A' T1 T' E
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    ' _9 h6 T7 {6 D" z* w! B! C
46. 
    
47. void loop()
    4 l% _4 }! m. r# p. r1 Q  D
48. {
    6 R- }% u4 p6 J* `9 y- ~
49.   while(!((lightsensor_1.read()) < (10)));
    * Z7 R  d/ j) J  M
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    ( [! |  x1 x5 d9 `& _
54.    int Err = getErr();
    
55.    if(D1 == 1)
    , U! f: ?9 f. Q" r( c. s
56.    {
    
57.      Left = 1;
    . W3 k: b5 T% b0 A
58.    }
    
59.    if(D6 == 1)
    ; v$ e0 l# |( C
60.    {
    
61.      Left = 0;
    6 \# O- s. q$ x
62.    }
    
63.    if(Err == 99)
    + R3 S. B  n1 n* b9 z1 J* Q
64.    {
    # _/ y5 H7 e6 V5 g9 I
65.      if(Left == 1)
    : H! N( B# x) ^2 n! G4 j/ B3 T
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    # J! ~, _+ x  t$ I
69.        do
    / l2 `+ D1 [. {% y
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    , L- c, R5 D7 ~! w7 ^
73.      }
    
74.      else
    
75.      {
    
76.        Speed -= 5;
    $ P& @- n% D# D, {0 A6 q
77.        moto(Speed,0);
    : M8 ?% z# u) v. o9 Y
78.        do
    9 _; G, ~8 v1 h# J# K
79.        {
      `. _! F" G- j- c/ Q3 ?  N
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    
84.    else
    
85.    {
    8 Y8 t# W% ]7 Y, H% q
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    : E0 i6 B" h2 {9 W, T- }
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    1 D# D0 h& ^. C
92.      previous_error = Err;
    
93.    }
    5 S" B" g; G9 k) m
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    # W* P+ m1 A9 V7 B. M
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    - B3 a5 {0 i, c  J, F% ]
99. 
    6 I, b/ A( {  I/ e: |4 ^
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     * A: P/ k1 ~( m0 n
103.    D1 = ~(sData>>0)&1;
     7 x3 \/ R9 d+ R6 b9 G! `5 N
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     # k3 F7 b) G) t
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     1 V: r0 f  v  d7 ~0 H% f
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     # y/ ]7 S6 v1 J8 Y- i9 t  F: T
113.      return 99;
     
114.    }
     
115.    else
     : c% x" Q. G5 t
116.    {
     
117.     return upD/downD;
     % j8 @" k( ?8 Y: I
118.    }
     
119. }
     
120. 
     8 {* f  C/ i6 W3 B/ }  D- m
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp
2 e# c5 {5 @3 o' _

1. #include "MeLineFollowerArray.h"
   
2. 
   8 W, `9 e; h  j! ~$ k
3. #ifdef ME_PORT_DEFINED
   ( m/ v4 w; x$ k3 {1 ]3 C
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   ! Z$ |: O4 Y( w7 r
5. {
   
6. 
   + {# R# [) ~! Z5 B9 m
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   : r: z, P! ]8 k
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    / a6 |3 ~% F; E3 d0 U3 o; ?
13. }
    . D. T6 E& |, {0 `8 o# t6 @
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    6 P5 U$ f' @- v
16. {
    
17.     _DataPin = pin;
    ; Y# H% r5 p/ n  n4 L6 r
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    : y) T) O. [  C1 M# v
20. }
    & g2 Q; q  @5 h
21. #endif // ME_PORT_DEFINED
    
22. 
    7 k! [! Q  K" s" j, ^+ ?
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    # |  V  D7 M% z4 I1 n
26.     _DataPin = pin;
    ! o$ i! {  d) x/ _& H3 p! v. ]
27.     pinMode(_DataPin, OUTPUT);
    ' h& E' Y4 \* d" g' {4 Y
28.     digitalWrite(_DataPin, HIGH);
    9 f/ E+ ?4 [" b& ]. Y2 U
29. 
    
30.     #ifdef ME_PORT_DEFINED
    * U5 L" }, n( A% F1 T5 `
31.     s2 = pin;
    
32.     #endif
    1 s5 V1 G6 N2 o+ V% W2 E
33. }
    7 L- v( K; e* A' I
34. 
    # H! R9 I: v: A) a! n
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    * ^3 d+ z( h6 K6 j
37.     uint32_t LOW_level_read_time;
    / J3 P% S0 {+ E8 q( u/ ?9 L
38.     uint32_t HIGH_level_read_time;
    * n, a5 z6 R2 d1 ~$ ]5 Q
39.     uint32_t time_out_flag;
    & _  w& ^) E. [- s2 Y- s
40.     uint8_t Sensor_Data[3];
    ( W4 x$ s/ p$ K2 U/ |
41.     static uint8_t old_data = 0xff;
    $ B) y! l+ M. \& L7 ]" ~
42. 
    5 n$ K( H, s* w
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    * T0 J2 B% @: N% n2 g' t3 D
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    9 x! O) |6 w7 ^! T# p) N
50. 
    5 [6 O  ?9 B% G+ w& ]
51.     time_out_flag = millis();
    1 ?; D  [: x; q
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    , k( h; f1 i$ j
54.     LOW_level_read_time = micros();
    8 e, }0 Q! _/ A( ^
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    8 J2 [* o; c1 {( E
56.     {
    % x+ b9 e: b8 N8 W/ h2 j6 G
57.         return 0xff;
    
58.     }
    - D1 j  z1 }# s' V- Z
59. 
    5 |. H9 N7 K8 d! F% w* B8 q
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    / j4 [) l# B, t1 y( q9 y
62.     HIGH_level_read_time = micros();
    ; @( ^. R8 S. b7 @
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.     {
    
67.         return 0xff;
    1 m' C3 e5 L# |7 y2 g+ l7 }( E
68.     }
    
69. 
    # j; j) v9 @  T
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    0 \! _5 e4 @" j- Y% m
71.     {
    ! N: e$ t4 d  d* l
72.         return 0xff;
    
73.     }
    
74. 
    ; t' B6 a1 a  B. T! ~, B' Z
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    / W' y  p' x0 q# l8 W
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
    0 w$ {% M; Q) x5 @& W8 ?3 ?
80.     {
    # ~2 S. S6 G7 ~# i
81.         return 0xff;
    
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    ( a# p: q4 P! T( v+ n+ u, @
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    2 V8 [! X& L9 Q$ y$ B/ m; N" k  C- x
91.         Sensor_Data[k] = 0x00;
    . b1 x  |" |7 }
92. 
    # d9 D: h4 Z/ c! ?. q# ?( u
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
    8 Z. _+ k  F7 g& K; k9 c4 [, l
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    : j2 i+ [* a6 s3 G/ a& n+ d
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     ) U; c" k+ I' p, [0 s! u, l
101.                 return 0xff;
     
102.             }
     5 F, J' M, C1 X+ U+ V: D  s* S
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     & a- M' O% e, I8 P5 T
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     : y/ H2 I0 j$ I; s: o" X2 X
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     ) G3 I$ T' ~" m: M& `4 X
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     + x9 o0 ^( l- C9 m7 U8 {6 B
116. 
     * r: d( {! ?9 ]# Z$ |
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     ' @- X* l+ r  R: v7 n
118.             {
     / @! P! w4 F. |, K" r: r2 R' k# z
119.                 return 0xff;
     
120.             }
     
121.         }
     
122.     }
     4 w" x* f/ t) Y7 E( J
123. 
       u" o: j# f. d
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     7 h4 E5 P' R# B% B  b
125.     HIGH_level_read_time = micros();
     1 y7 S5 O$ \. h6 C7 l: I9 H5 x
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     9 i: I$ I# d9 N- Q4 d
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     6 w4 @5 v$ C3 N4 L' @9 r" Y+ i
129.     {
     ) b" |: Z: k0 E+ C: D3 F1 n0 r1 g
130.         return 0xff;
     % D) z, }! a4 t5 Z. T8 R
131.     }
     & C/ e- ]- }' t9 N/ O
132. 
     5 Y$ \' m, a6 A! |+ I
133.     pinMode(_DataPin, OUTPUT);
     3 v% c. O& g% r
134.     digitalWrite(_DataPin, HIGH);
     ( E$ L' j8 x+ X4 @
135. 
     . f" h( p, n" B2 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.     {
     ' n/ J/ ]. V- e$ u; d$ F
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     $ N+ H% `9 D3 v4 y  r
143.         return old_data;
     
144.     }
     & d: n1 K% G! j$ ?
145. }
     % s4 m5 U: K. Z2 g2 E! _

複製代碼

" p6 W6 \# I5 y5 JMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

! K( X( n0 t" l% p$ u

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
( T% b& E% ?6 m# V1 m哇...要100 個銅錢

# k9 q1 \+ D8 j- p- i認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
8 ?1 f" h- x2 e3 E: ]% v能否用到mblock 5 上面呢？

- ^" v0 B& Y9 Z1 c& g5 ^. k! k7 WMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手