mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   / b1 E2 ]; y9 C, ^
3. #include "MeLineFollowerArray.h"
   ' H% K4 u6 S/ j+ X4 j! O, S
4. 
   $ V8 l- V1 q! n% C( P( D
5. MeEncoderOnBoard Encoder_1(SLOT1);
   , ?: u7 L! ]4 C, A  q7 S. B6 r$ P% Q
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
     {; \/ k1 H0 `; C3 n! `9 D$ ]
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    0 s- b' f7 i* E5 J
11. 
    
12. #define Error1 1
    ' y/ r( f! K1 d, e6 B1 z7 o
13. #define Error2 2
    / R: H- s5 [7 U; K9 N+ d7 _
14. #define Error3 3
      K( v. Q) Q0 }
15. 
    6 E; k3 E" n) X" _! M4 C8 i- S& f
16. #define Kp 15
      ]  n  b- Z! _* o' _
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    2 h9 G. W+ b2 X+ l+ p5 a/ L
22. uint8_t D2;
    
23. uint8_t D3;
    5 c  H2 P6 n8 X2 K
24. uint8_t D4;
    
25. uint8_t D5;
    5 R, [. n4 r; @; o3 }! r  N6 E3 E9 Z
26. uint8_t D6;
    $ n1 ]) n7 K* I% U
27. 
    # K2 {1 ~0 U5 l: y0 Q& q2 u) r
28. float previous_error = 0;
    + o, b* i: U% N5 z5 X! v0 {; }
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    
33. 
    * D; x4 K( p' \; c
34. byte Left;
    ! g, e* V, f2 t* t- P$ u
35. 
    + a" H3 i; l: Y. u
36. void setup()
    
37. {
    3 i1 W- g5 X3 b% X# M) Q# |# u
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    $ z4 x$ T$ Z+ m! c' b) w
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    " S8 |, [+ g  d, M+ S; n6 I
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    
46. 
    $ _3 B  z5 Q2 n- c+ j, f
47. void loop()
    
48. {
    . X+ J- \" D1 R0 m0 K0 R* z
49.   while(!((lightsensor_1.read()) < (10)));
    # h: X* ~6 y5 ?7 E
50.   buzzer.tone(1047, 500);
    # n0 Z2 U* l( s/ [$ _
51.   delay(500);
    
52.   do
    
53.   {
    * q% m# v, C: P
54.    int Err = getErr();
    
55.    if(D1 == 1)
    " `+ j9 A3 l2 {, T# S2 }
56.    {
    
57.      Left = 1;
    3 X  m- D1 r1 x# J! {4 L
58.    }
    
59.    if(D6 == 1)
      o9 t0 f, D! o* g* j
60.    {
    2 W2 B6 X3 k! X3 J/ u. s/ n
61.      Left = 0;
    9 ~" p) b  E3 T" L' V
62.    }
    ' r+ I# V, Y, L8 g# G) h, Z
63.    if(Err == 99)
    . d  {, d0 M, a* ^% E
64.    {
    9 Z% D% o( j; z, J7 I9 ]
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    4 x- Q8 Y  B# U7 e3 P
68.        moto(0,Speed);
    
69.        do
    ' G+ m5 h" l, k! h) i% n
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
    6 M: {$ T9 Q! s# R( w8 ?
74.      else
    % U( [* }2 c: Q" i5 \$ N
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
    " `. O, i4 M% a
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    9 K* k5 l* T5 |' W2 |2 c
82.      }
    
83.    }
    
84.    else
    2 T' D" r8 m( U& u8 u
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    & Q( K% z/ M/ @8 @  W5 ~6 w, i
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    ! B: S2 y9 r4 t- O9 c' a6 C6 p
88.      integral = integral + Err;
    2 W, Z5 }- h1 `1 f- e
89.      derivative = Err - previous_error;
    & B2 z& i9 M8 i! y7 _
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    ( z, S/ j8 W/ y' ]
93.    }
    ! D5 J4 W: |+ _) u9 B$ I
94.   }while(!((lightsensor_2.read()) < (10)));
    3 h: a" B+ [: z! [4 |$ C1 t
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    2 b0 l! r( S0 c8 `1 E& h8 R% V0 M
98. }
    * V' T0 o: \. I* }: l/ W# x+ t
99. 
    0 h( O* r$ w( U1 z! ]# L. @- @- N+ s
100. int getErr()
     * V% S+ ~! y9 L# P5 q, O5 ^# r  ^/ v3 m
101. {  
     . U6 o, l" V9 \% I! w- y
102.    sData = linefollower.getValue();
     * _5 X5 y+ z7 g5 J
103.    D1 = ~(sData>>0)&1;
     $ f; Q" R, c8 Y
104.    D2 = ~(sData>>1)&1;
     & E: \; L# M$ m/ `
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     . p& [9 @! r7 x% P4 a( w& H9 {4 X2 x8 A
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);
     ) q3 n: K% f' D! [9 m
111.    if(downD == 0)
     
112.    {
     0 d3 {, ~: e) Y2 L- h' D
113.      return 99;
     
114.    }
     2 b1 V7 f* Z+ \2 N1 V' k# Y
115.    else
     # M6 Q! M8 u# J9 a/ b, T8 T0 z
116.    {
     ! w3 Y, D) y' h" B
117.     return upD/downD;
     
118.    }
     
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     : |! x& T% C# j/ ~1 [4 {
122. {
     - I% q* j0 [5 [6 V5 w0 N# T
123.   Encoder_2.setMotorPwm(Speed_L);
     2 f& h% {- r6 g& p1 }6 `
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

" J& a* U' m5 W% z4 f$ ]MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   3 x3 q# A8 \" o6 C
2. 
   2 y0 K9 {# @) p3 w
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   - r' z1 L- D% Z/ J" M, Y
5. {
   : C& b5 u: ~9 C. A, a: ~2 j
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    . \( y2 @  U! L
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    . ~6 ~/ E5 B0 l. z2 u8 Q8 s
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    . n* k7 x" O* e
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
      \- j4 \5 ^4 p1 F( _8 R- \
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    6 S, O/ @2 v8 t" ]
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    : }4 t7 J& l6 h
25. {
    
26.     _DataPin = pin;
    ) p1 e8 b5 U7 J$ u6 x" |& c
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    ! c, O3 {8 H4 U) Z
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    
34. 
    * o1 E. A6 {/ v7 C' s6 v
35. uint8_t MeLineFollowerArray::getValue()
    & m# R: i, N  Z% @' p) r
36. {
    " D0 Z- W/ Z. |) ?
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;
    7 ~; U6 a# a+ S0 ?$ |) j6 Y
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    * q. B8 y% R1 r: K
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
      {& m! Y' U  z# V) A) n+ x
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    1 d0 k* ~5 l! G. N
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    6 O6 t/ F) g6 y% q0 ^+ S# X/ i( P8 w
51.     time_out_flag = millis();
    , q5 v# @5 L2 s; C
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    * {! |$ ]/ R, z' E; Q
53. 
    ) Z; [/ N, H$ Z3 o7 a- a
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    8 t% s( h4 `: L5 f% \# h" F7 P
57.         return 0xff;
    2 V# ^0 }% n' A( S; I
58.     }
    ! \* a2 e' V; W- G. _# H
59. 
    : q; u+ W! i+ C1 O4 _0 `. F9 J  o8 _
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    3 G2 O5 D/ ?( V  {- f  P; c
62.     HIGH_level_read_time = micros();
    4 L: ^0 E2 o. T' L
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    9 M% V; S% f6 `$ p. ?( u; y
64. 
    4 {" i# `6 S# a: j
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    : |4 d/ a( `( b! [( Z2 J. v
67.         return 0xff;
    
68.     }
    # S6 E  \# O0 y; |
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    0 m% v- N5 K/ J8 ~9 O9 c! N: D! |
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    ) a1 Y) c/ d2 _$ C% I: n6 m
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.     {
    
81.         return 0xff;
    
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    . _' L! p0 F/ M- z5 v
86.         return 0xff;
    
87.     }
    " n0 `0 B) h- \  t4 {/ M
88. 
    . b' [% F; s8 D3 u
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    
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
    
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    % i! ~0 r8 r6 x- Q
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    6 j3 z% V' x/ H  X
100.             {
     
101.                 return 0xff;
     $ n  b) b: j3 ?; z
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     8 I% ?; o; L. q  t
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     8 F2 S! n; u- j% C" r2 _
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     ( U8 G9 }' \% ^
112.             else if(HIGH_level_read_time >= 100)
     7 z( ]$ ?" W  H/ ^( u1 g
113.             {
     % V! h" W4 ?; |+ g
114.                 return 0xff;
     
115.             }
     7 Y( `; f: q* J# g0 @' h- h3 l
116. 
     ! A" D3 f$ m$ o' T1 Z
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     ) I. h/ P! G5 }& B4 I! o9 c
119.                 return 0xff;
     
120.             }
     
121.         }
     
122.     }
     ! D( x6 \2 N* _+ C# w9 I6 Q- @. N
123. 
     " _6 `5 s. O5 J
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;
     + J/ B. R6 `, V0 Q8 t! t: u
127. 
     5 U# K+ N% B4 R) G& I
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
       F. E$ q' D8 J- U+ A8 z( _
129.     {
     / ^9 Z3 |5 j5 V. t
130.         return 0xff;
     , m# i  p! s( }: n5 b( `0 ]+ j
131.     }
     * K( g, ]7 B4 F( C% T
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     ' ^+ Y9 q& `9 P
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     : N$ {. Q) Y; c: ~! L+ B8 W! U9 E
137.     {
     
138.         old_data = Sensor_Data[0];
     5 y2 K1 `' f9 T- m3 F& p, g
139.         return Sensor_Data[0];
     1 Z* u* _+ M0 o) n% i
140.     }
     
141.     else
     
142.     {
     
143.         return old_data;
     
144.     }
     7 H; U" l  m8 q& ?# l  {
145. }
     8 B9 [) v: y" A5 y" _

複製代碼

8 L3 ?6 u, M/ N2 z$ L& C1 pMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

- R- B4 L5 ]$ P& D7 ]# j

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
0 [$ j/ N' i( }' y" g; U哇...要100 個銅錢

) I6 F1 K3 c. Y$ X/ A4 o2 [認真的回饋本站一些內容, 很容易達成的!

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

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手