mBot Ranger 高速循線範例＿

7 {  e& T2 l' M- [6 ]

1. #include <Arduino.h>
   ) y; z2 l, _3 E; z3 \, ~1 L! m
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   7 N% j6 N( \9 ?" R) d
5. MeEncoderOnBoard Encoder_1(SLOT1);
   0 d, a) z0 g0 W% N
6. MeEncoderOnBoard Encoder_2(SLOT2);
   / g( `! ]% z' d8 Y3 b  h
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   ) F( b2 P+ q5 Y0 a4 [) V2 ^
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    ' A; L: r/ |+ t/ S' J! J0 I3 v* [* y
12. #define Error1 1
    9 [* s- {/ D/ a: k: C- I
13. #define Error2 2
    
14. #define Error3 3
    ' M! p6 f6 B, l! _+ B
15. 
    5 `$ k- \( X$ @
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    # `6 G$ \7 @2 I5 J- T0 a
19. 
    
20. uint8_t sData;
    * ~+ r- z% E( @. b, {. j
21. uint8_t D1;
    
22. uint8_t D2;
    9 J2 Y/ m4 `( J- M- I) A7 X1 X
23. uint8_t D3;
    
24. uint8_t D4;
    5 @6 H9 ?5 t4 l) W% F/ Y( u# Y" j
25. uint8_t D5;
    8 F6 Z! K4 D% q) o: c
26. uint8_t D6;
    $ l0 j+ _5 i) j
27. 
    ; n; o  @5 W- ^  ~$ y+ s4 P
28. float previous_error = 0;
    ' {0 ?" n9 \6 l8 G7 Y
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    # c" b( c2 L& B( h" M. U
32. float output;
    
33. 
    
34. byte Left;
    
35. 
    ; z! ~$ T& d* b* e$ p1 g2 g+ O0 U
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    ; B% T% F. y% E1 D5 p! G
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    + h  H6 v2 E# K
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    ! q8 {: v. D4 M7 v* f* d* f
43.   Serial.begin(9600);
    2 b7 f! c6 Y1 G' K7 [" N
44.   buzzer.setpin(45);
    4 Z* Q* Z& P" |" Y* a1 Z0 U6 k
45. }
    1 I; z; U0 p1 g1 r  c6 E6 L
46. 
    
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    # _% ^7 G2 Z) b( C* p' r8 E
51.   delay(500);
    
52.   do
    % B. ?/ b  Q9 }2 I
53.   {
    ! J/ T% Z6 C3 z" `
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    $ \6 _/ ^/ {, t8 Y* y7 l
59.    if(D6 == 1)
    
60.    {
    3 N( {3 f/ r- t2 i, K
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    ! V3 @0 ?5 n) m. R1 u3 A5 [
70.        {
    3 y& s& H6 i8 r: X% F2 x, ~; i/ f
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    - S8 u4 p0 d; J9 C5 b
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
      U) O) {7 G; g
81.        }while((D1+D6) == 0);
    - t( f! _- R, F5 E: B, I4 a
82.      }
    9 C& W9 K2 h. M# q+ o# x2 q1 S+ X
83.    }
    8 {" k+ |3 ~- p4 e( n/ |
84.    else
    1 H5 J6 r3 P* s4 ?. \1 {% P
85.    {
    
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;
    $ n7 V3 r' z' n! b
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    , O, V! x  A0 V8 m4 c' I  j: X
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    ; \3 ?$ O  a8 C1 Z1 u4 e7 J
93.    }
    ) O3 y$ N# f! d4 ~" P$ ^* e# g
94.   }while(!((lightsensor_2.read()) < (10)));
    1 T5 u5 ]3 S8 r# u& H5 w- X6 i, d
95.   moto(0,0);
    ; a2 o* N6 x3 k2 O' H
96.   delay(500);
    : A' C9 e9 B# {" c9 r8 w
97.   buzzer.tone(262, 500);
    # a5 g7 F- m" O
98. }
    ; h, h1 I' L- M8 I
99. 
    0 s2 g5 W. Z+ ~  {3 x
100. int getErr()
     / g4 u: |+ _( [: d$ ?
101. {  
     
102.    sData = linefollower.getValue();
     7 V# W( `8 \! _1 R+ T6 }
103.    D1 = ~(sData>>0)&1;
     , ~! b% o5 B- Y
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     4 r' @6 @9 q2 v5 M: ?3 I- `$ |0 M
108.    D6 = ~(sData>>5)&1;
     7 {! v; T; Q# ~/ s, h
109.    int downD = D1+D2+D3+D4+D5+D6;
     % M# v; i' V: R
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     * d: x4 Z3 W0 Y, C
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     
114.    }
     
115.    else
     . }4 R7 A( Y2 u& I+ L# ]8 a
116.    {
     
117.     return upD/downD;
     # g7 o% c4 p  B# o  W
118.    }
     1 P# U* P8 a8 {4 g: J; Z
119. }
     $ P  n9 Z" p( T4 ?
120. 
     , r& \6 _6 [6 Z  Y
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     ; p& u; V5 |6 b
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

) J, D" k) k. B* f7 k2 k. AMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   1 I# I) t* ?, V4 e+ @6 B$ H
2. 
   / O+ N# w- m$ o  f" G
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   . V/ w3 c+ J" B! `$ B% Y9 _
5. {
   
6. 
   ! ^' A$ X( Z( l+ M6 |
7. }
   ! o1 r4 T8 _! q5 \  g( W
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   ) d) I$ G5 J4 f0 z9 [
10.     _DataPin = mePort[port].s2;
    - c/ X) c( {& U
11.     pinMode(_DataPin, OUTPUT);
    9 u* B: {' _3 E) G
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    ' u# J0 f" S3 p- O8 B3 ^' y( Z
14. #else // ME_PORT_DEFINED
    $ X/ Z8 X) z) N% p* f, T' `
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    ; }! u7 z4 A0 D" u
16. {
    ; G. o2 M( i6 Z2 M
17.     _DataPin = pin;
    8 _8 c0 W: V8 ]. ?% K# g3 v
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    + e2 u. |' |  R9 V: ?# z. A1 Y' k; O5 I
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    $ @) b- K- \# h5 G- k! |
25. {
    
26.     _DataPin = pin;
    0 \" u; ~4 G! s# t9 b
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    ; p9 J8 S/ Y  e+ a* d
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    5 O  m: Y# O- j
32.     #endif
    
33. }
    . B, [$ h- @' |3 F
34. 
    # E7 S! L, m( N" b
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    6 Y1 B) U7 f4 Y1 L
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    # k0 f3 u6 U& X' u: Y8 h9 C, K
40.     uint8_t Sensor_Data[3];
    $ y% Q8 |) Z9 ~7 f! @
41.     static uint8_t old_data = 0xff;
    ' ]) }. ]% ~" @
42. 
    6 @% K  K9 X7 i. t2 V6 Y3 p
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    : }+ e' _1 M$ E; B; _2 c  |
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    ; F9 |+ ]5 F0 M3 _4 O
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    6 z) ^( H( H6 E9 z2 s
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    7 d' r4 P9 f, ^& Q- l9 a. M$ k
53. 
    * ]: _4 P. n' Y8 k
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    # E! A# o1 l: T, ?2 y
57.         return 0xff;
    
58.     }
    $ n& i3 O5 }+ N3 v, g" L  ?+ ^
59. 
    # l& A. [) s0 M5 h& u. J0 w
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    " M- [! \7 W  _* Z$ G
61. 
    % n! C; F" u& M. f3 S
62.     HIGH_level_read_time = micros();
    # Y  K4 Q/ F/ W$ R9 N* ~1 T0 H0 ^& n
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    / W; @1 ?2 d* p9 T4 B9 D* C4 [! h
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    0 v$ u* P. c; K
67.         return 0xff;
    
68.     }
    # \' R/ ^4 N& ?4 i
69. 
      J. t) i3 V8 W
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    6 j8 _& ]) f0 i1 K
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    : a7 V4 z, p& `" C, |9 f4 d
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    8 p% b5 A8 S. }7 c$ P0 K+ T
78. 
    / q7 u+ r+ ^' v6 V( g9 L% t
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    * U* Q+ U1 K. O& U- {6 q# T9 C
81.         return 0xff;
    
82.     }
    
83. 
    & h+ X# z: Z# Y% e& a
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    . h! P; l+ p  K. i1 C: H
85.     {
    % R2 I1 f9 s5 Q" z# K+ n
86.         return 0xff;
    , w! V6 e3 F8 \+ ~
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    / `: I3 G; T; Q' V0 u0 @3 l
90.     {
    / ~# D9 T3 g3 T3 C5 {
91.         Sensor_Data[k] = 0x00;
    $ s: A& P. o2 W, l1 c
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    ( z- P* z" Q2 s. L: S" j# b  D
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    6 q9 K  @8 z) X4 Y' M7 U1 G' ~7 `
96.             HIGH_level_read_time = micros();
    + L0 {+ v- A9 V1 Q& \- \' n
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    $ h* h' y9 c8 z1 V
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     # \/ |# C# b- r: m  G- s) v. E7 s; j; c
101.                 return 0xff;
     9 q5 e* ?# h2 Q
102.             }
     
103. 
     # `: Q+ t; W2 V9 Y1 i# _
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
     , ]4 d+ Y9 @7 n- `2 A+ U* Q3 V0 V& Z
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     # N( ]8 K3 M9 M) ~4 t5 I. S4 b& @0 W7 x2 P- ]
112.             else if(HIGH_level_read_time >= 100)
     9 h: l  k1 R$ \; D. s8 |
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     ; `* b4 M5 P; F/ v" J3 E( U
118.             {
     
119.                 return 0xff;
     
120.             }
     # w" G( H8 T. w, H7 [
121.         }
     
122.     }
     2 _0 C/ D* }$ G/ N' |! D
123. 
     1 k/ `# ^' B4 d8 _& z
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
       q" a" f1 e: L( O% k! D
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     # u( w( }  h$ e; S) n! |$ x
129.     {
     * m! @" s- N1 w) ?2 X
130.         return 0xff;
     - z. i4 p, A% b
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     , O2 }6 H9 p# I$ U
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     ' l0 t' X& O7 V% f6 L- p
137.     {
     " D7 R0 Y2 J4 i- c
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     . d9 \8 d) w. G
142.     {
     
143.         return old_data;
     8 Y& D: B; y8 z0 r# r  r* Y6 I
144.     }
     
145. }
     

複製代碼

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

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
) W# J' m% Z. p1 i3 j3 R: j, ?7 y哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
1 L% E$ B2 o4 k4 q) L能否用到mblock 5 上面呢？

& C6 z1 y4 M2 C4 {% I/ ~MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手