mBot Ranger 高速循線範例＿

% }" x. ~0 f# S/ ]/ U8 f& u

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   7 y5 P. p' I# {
3. #include "MeLineFollowerArray.h"
   
4. 
   & J% C. P+ P) P  l
5. MeEncoderOnBoard Encoder_1(SLOT1);
   ( ~/ @7 s7 T% e9 N; H( N
6. MeEncoderOnBoard Encoder_2(SLOT2);
   : ?6 H6 h; v5 r7 R
7. MeLightSensor lightsensor_1(12);
   3 E+ |- I5 e. v
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    * F  {/ I: }% J
12. #define Error1 1
    ( i1 a# B1 X! L0 Q$ z. L: `7 y
13. #define Error2 2
    9 @: V6 \8 p. k. T" Y$ f
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    4 L8 ]3 x; I. ?  N
17. #define Ki 0.15
    
18. #define Kd 0.03
    , B0 N0 d: |8 z
19. 
    
20. uint8_t sData;
    2 e7 m( s) I- L) u8 Q7 A8 G# n
21. uint8_t D1;
    
22. uint8_t D2;
    1 A: v, Z3 I5 r: r0 y7 z# D
23. uint8_t D3;
    
24. uint8_t D4;
    ; c# D6 P% }  d5 N
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    " D# a- x& f# v. b
28. float previous_error = 0;
    6 q% f! w# E% V
29. float integral = 0;
    6 N( q4 U2 {4 I4 ]0 O9 w
30. float derivative = 0;
    
31. int  Speed  = 160;
    & p0 P# z! N" [
32. float output;
    & x9 Z2 I2 z& p& ]- ]
33. 
    1 g* j0 D9 ^3 U
34. byte Left;
    ( `3 ~! B0 o7 s# y- ]
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    # _* J) c' F0 u5 ]7 J
39.   TCCR1A = _BV(WGM10);
    2 h! a9 z9 o1 B/ V
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    : J# o6 Q  O/ Z
43.   Serial.begin(9600);
    . \7 V( A0 U5 Y# V/ T! m
44.   buzzer.setpin(45);
    / ~8 h3 h; o5 m# y
45. }
    * c1 s3 m* U: k  Q5 x
46. 
    / l4 ?! L2 G7 T5 ^/ F* ^
47. void loop()
    - U0 g) a; ]" z9 X+ O/ \$ M: K
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    ' C5 t; f/ D3 R" a
51.   delay(500);
    
52.   do
    
53.   {
    
54.    int Err = getErr();
    2 t# B+ h, j2 w3 Y  l/ r
55.    if(D1 == 1)
    6 M/ |3 ~( P: }! n
56.    {
    ' V% r+ B3 S& b4 B/ V6 C
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    5 H9 f; H" m- ?
62.    }
    2 r. q4 \' |: j' N2 D
63.    if(Err == 99)
    5 a# {7 G1 c# Q- i9 L
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    ! I! a" k' O: k) _& z4 i6 \2 m! a, }
68.        moto(0,Speed);
      R7 q+ X9 x  m3 Q7 j# `5 H; D0 P
69.        do
    % |+ G- U* q2 r; Q& f# A- W
70.        {
    
71.          Err = getErr();
    : _( l" ~& m- h0 `# h, }
72.        }while((D1+D6) == 0);
    . m* z# X! j) e' v
73.      }
    
74.      else
    
75.      {
    0 h! X; O5 `, A5 l& p$ w
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
    2 W  d4 H% \/ N1 H7 x9 l! V6 e5 I
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    ' E2 h) @7 L, r; [5 e* o; @
84.    else
    - \: Z8 C! y3 Z7 q+ a& m  _3 w' x
85.    {
    , d; T8 b# \- J) ~
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    1 t1 y5 [% f4 r; |3 D
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    & ^' e! K4 Y7 L* B9 E# |( V& _1 W" U
88.      integral = integral + Err;
    & y5 p$ X& H& q  C) v; n& C2 h: Q
89.      derivative = Err - previous_error;
    7 F7 i3 M2 f0 {$ o" U. @( b# G
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    2 k" h9 q5 i$ s2 s# M7 I  t" z
93.    }
    " x8 O. N5 n. q: k. b0 `& M+ m
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    0 w& J( m6 ~; s0 N" m+ O! q
98. }
    
99. 
    
100. int getErr()
     2 q9 G3 z" }2 P4 \. E8 C8 T
101. {  
     9 F7 Z' ]5 y9 d- j( B& ^6 [
102.    sData = linefollower.getValue();
       Q/ a3 d; T! J; [$ b  ^7 g7 E
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     " q5 C$ N( \% W) D
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     : |! B; a( _' l7 }) Q
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
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.    {
     
113.      return 99;
     
114.    }
     
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     0 i8 B  [9 v/ D
119. }
     
120. 
     4 Z1 |( |7 O3 Z8 k4 r% a
121. void moto(int Speed_L,int Speed_R)
     / B) W+ C  Y8 Y1 z- ]* F6 D" I" C% n
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     * J6 ^: V5 v8 y
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   - _6 m& ?3 g1 H$ z, Z
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
     Y' ]2 ], r' y. ?9 R0 j0 r0 j
5. {
   & m6 n. r, u- |& G. w' P& k4 |# f
6. 
     z# ], G" z6 S5 [- T
7. }
   ; f0 D$ c/ s& d  D2 F
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   ; s0 T1 w4 w5 m& t
9. {
   
10.     _DataPin = mePort[port].s2;
    6 A' h3 D- e, s1 \  q" B
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    ) v2 w' V' h( ]
13. }
    $ B' P8 i9 l! Z3 V+ p/ R! G  C
14. #else // ME_PORT_DEFINED
    0 V& Q# [2 z/ l: f7 b1 m% T
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    1 V, Z# b3 r' o! @" c4 P
16. {
    # N; r7 ?1 v, T0 X% r
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    % T5 K& Y1 B! U# H, F
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    ; U  x- j: @& ?  @% S$ v; k
27.     pinMode(_DataPin, OUTPUT);
    , S5 n# L; G/ v( B) [' ^
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    8 t' H% Z; J2 _" N+ Y1 |
32.     #endif
    & D; v3 `8 x+ u/ ~5 u4 O
33. }
    
34. 
    " @. b% o. S* O" P# ?4 x1 r0 q7 z- E
35. uint8_t MeLineFollowerArray::getValue()
    9 h7 |( a8 Y  [& E( G9 t- A$ }
36. {
    7 f2 z! J# y9 z6 Q" }+ u
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    2 ]0 ^7 g/ R' ^8 w  S. w
39.     uint32_t time_out_flag;
    ( ]/ v' i* r4 j
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    7 X3 H" q4 m7 e1 Y4 X$ B
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    - m  \0 z4 Z, a; f  H' A- a1 l# D2 B
49.     delayMicroseconds(10);
    $ ?8 a: Q, F  T! s
50. 
    ! ?6 g4 i. x7 f0 @6 s
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    : w/ O( \7 q/ Z0 u
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ) r6 E. t, q  ^' Z2 d
56.     {
    
57.         return 0xff;
    $ f( u6 c% {* ]
58.     }
    
59. 
    7 b6 B5 M& L! a' n3 T
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    ; I5 v' o9 E- V7 H. r* H
62.     HIGH_level_read_time = micros();
    ( T% g/ \' H5 a7 |
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    : d0 d. v, w% p) p( A- o, q
64. 
    4 W2 z& J# B, j3 J' ?/ ~
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    * i) T9 U) t6 ^+ k3 I
66.     {
    
67.         return 0xff;
    
68.     }
    " z1 t* ]3 H* L4 B* w
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    7 _3 H6 m& v2 k1 L2 A7 l% Q! B
71.     {
    
72.         return 0xff;
    
73.     }
    / m2 U0 m# S1 P# z; H
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    % }5 {8 {, i6 ?; n
76.     LOW_level_read_time  = micros();
    5 i1 j" q8 [( r6 \9 V
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    - g3 ?# B: \% ~4 t
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    2 a! i$ l; U/ @  J/ q
81.         return 0xff;
    
82.     }
    
83. 
      g; ^" d. a0 q8 g& W4 I1 X# |1 V
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    / n* Z9 Z1 y' \
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    6 B4 n0 ~2 T0 y8 r
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    % x& x- e# d; V' B/ M4 t& V
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    # L6 f" P1 c" E  Z& m$ F
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    ) s6 i. X8 f  S+ D" |7 \3 S
98. 
    . p5 E9 k7 o6 X+ R' }" W! h7 v
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     ( \2 [$ |* @" U& E* w
101.                 return 0xff;
     8 ?4 ?# q! I- `2 O
102.             }
     * i7 E$ D5 {+ b( q, b/ D
103. 
     
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
     
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     - n$ c' m9 X& e
109.             {
     6 \* X: I% k5 I4 t% J- o) ^. r; G
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     0 W3 g+ }# O( G6 p" \& n
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     6 Z2 z) g8 m5 Q5 I0 I1 n
114.                 return 0xff;
     
115.             }
     
116. 
     8 I8 K; M; q+ _; ]8 P
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     
120.             }
     / t# B, N: W- i) a. C
121.         }
     
122.     }
     ( _+ ]4 L3 {; F( j
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     / ?: t# d" O, z: a* h( @, Q+ E- A
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     1 N/ E2 M: C0 v' U
127. 
     + L5 l7 A1 B5 G3 Q0 N
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     % _+ C9 a" \" A: f7 f
129.     {
     4 _( w3 B. a$ l4 p. g9 O. A
130.         return 0xff;
     
131.     }
     
132. 
     % M. T$ r* z8 b
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     ; ?5 \7 {+ }" f+ E( z' W2 B
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     5 T6 Y; l7 i- H7 m, f
137.     {
       W# g: r, h+ s6 O, A4 O8 k* [+ f2 P1 P
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     5 k$ A  H% b% z4 _  t) d, Q4 n
140.     }
     ! W7 f& d$ ?9 y% H
141.     else
     
142.     {
     # E# v5 s( W5 y
143.         return old_data;
     
144.     }
     1 w9 H/ `) t4 w, k2 E+ U  K. T. h
145. }
     

複製代碼

MeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

. Y7 e5 I3 }, J

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
哇...要100 個銅錢

; ]8 P6 e% |) w5 p認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
& x* }. a: K1 M  e  Q0 F能否用到mblock 5 上面呢？

+ |, e: S( i  ?' W: fMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手