mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
     ~, X' H  T  p& H; V8 ?! |+ S
3. #include "MeLineFollowerArray.h"
   3 d- S. z! H/ y6 P" Q- B4 I
4. 
   ; K* ~) I8 }- n: w
5. MeEncoderOnBoard Encoder_1(SLOT1);
   5 X! I/ A( W9 G+ m! j
6. MeEncoderOnBoard Encoder_2(SLOT2);
   0 a$ W6 f& `& ]0 ]8 u
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   . T) d! @1 W) r& ^: ]3 K
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    3 {$ o8 c8 b0 @3 z0 `6 Y
13. #define Error2 2
    % h9 w" \( R+ [! s( K5 F/ Z5 M2 i
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    # k: w2 d9 b  m6 W2 ^5 v
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    - l# \8 Y. m1 ^8 ]# {
29. float integral = 0;
    . W( M8 v4 I) s5 {$ D  i+ ?
30. float derivative = 0;
    ) ?1 h3 A3 L' e8 Q8 v: c0 E
31. int  Speed  = 160;
    
32. float output;
    ) L7 z' F5 y0 w+ i3 y# @
33. 
    , F0 D* a! h( s: C0 a3 D
34. byte Left;
    - D9 C, K, q! P; X. N
35. 
    4 X/ U5 i8 O1 Z7 C7 x9 Y+ j
36. void setup()
    : w5 Q9 \" ?! |8 x; l! f+ `
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    , J) w1 \" ]+ [" Z
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    , q  b6 V+ i% ]4 H/ Y# F% Y) S
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    8 q" w& X$ I/ Z# y
46. 
    
47. void loop()
    ) c8 ?" q5 z4 C
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    8 ^+ S6 l7 r5 a7 C: r, S$ o
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    / j1 U# M+ O: K2 w$ L2 w
56.    {
    . c$ y4 A) [6 z1 [8 E' P$ s# _
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    * B- S* Z1 @  `
60.    {
    
61.      Left = 0;
    6 E# E) d+ F5 F8 W: h8 G$ g$ n
62.    }
    1 ^7 Z4 D! T& B: f; j3 c% r% m
63.    if(Err == 99)
    + `/ E0 ~8 s3 G4 W. E, P( ~- _
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    : A$ g, y% |! |8 r: j7 [/ v
69.        do
    
70.        {
    # q- q# p- `1 E& o  X
71.          Err = getErr();
    ' O/ ^8 O  _4 h8 u' v
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    9 x9 I( n$ C' m- u; S
75.      {
    - W6 }6 Q+ {7 d0 I; J! n! }& T
76.        Speed -= 5;
    / }7 U* g( y. @7 h( K+ G  L* x7 H
77.        moto(Speed,0);
    0 f+ v) j: g0 b$ {0 D
78.        do
    / C, p+ U6 h& D& K2 I
79.        {
    ; D  X0 B# n2 d( c; X5 J+ J
80.          Err = getErr();
    7 |: v+ M* E# ]. s* Y
81.        }while((D1+D6) == 0);
    
82.      }
    3 H( U& k# ^; |+ Q4 ?8 f1 a- e
83.    }
    : e+ W1 p: d! H
84.    else
    $ D- }8 H7 N* d* D  b0 i, W% l
85.    {
    % U& d, R3 U6 t8 U' Y9 w
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;
    0 N( z, m- G7 X
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    ( E8 I$ u/ ~& w& K8 h8 M
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    , l8 ?/ C/ U& |+ S+ q; S
93.    }
    2 x6 R1 J( c; S1 M, U. M# D! O& Y- f
94.   }while(!((lightsensor_2.read()) < (10)));
    6 z  |# V6 z2 T( `& |
95.   moto(0,0);
    ) Q2 O: x# a7 c3 X
96.   delay(500);
    $ X6 c5 E# j' l% k2 P
97.   buzzer.tone(262, 500);
    
98. }
    - W+ F) F5 V+ z. f, F8 ]
99. 
    ! q' D: k1 a, b% y  b' H7 R
100. int getErr()
     
101. {  
     ) g* I0 {, p9 `2 t" f2 O8 e
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     8 S/ G3 ^7 f0 L. _& `9 f/ H  I
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     - r& f% j' T% X! o! C3 T# n' @
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     * j* \8 V% j. j; n) j
109.    int downD = D1+D2+D3+D4+D5+D6;
     " E. P* _' h7 |5 T, n% q+ k" m! f9 ?) P# J
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     $ _' E& ~9 C8 A: d
111.    if(downD == 0)
     ' K+ u* A4 U* u# I) i; H
112.    {
     2 Z& i9 _; r! C" n! E
113.      return 99;
     
114.    }
     
115.    else
     % b. i5 ]( Q% s# N1 E
116.    {
     
117.     return upD/downD;
     
118.    }
     ' p) b% t5 ]( K/ `! ~
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     7 g( S: u+ }* X7 D" X9 r- }) e0 R9 J( E
124.   Encoder_1.setMotorPwm(-Speed_R);
     + |/ t/ l6 {) X2 R
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   
6. 
   3 O* K  i0 P! S! y% ]1 Q0 n
7. }
   & {/ ?% q8 @- f8 H0 Z
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    ' q( F$ h( ^: n6 o" z# R/ R
11.     pinMode(_DataPin, OUTPUT);
    7 ~6 {6 _9 j! f% T3 T& l% Q+ m
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    2 \6 I" R$ v" `6 F) |! {
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
      _2 U$ V* a; z3 h( I) p; n# w  R  G
21. #endif // ME_PORT_DEFINED
    . n( {- d4 i/ K
22. 
    
23. 
    2 ]3 @9 O) @3 B
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    : |! h. r# J; h! r
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    9 a( n; c+ k9 G8 y" _3 R
30.     #ifdef ME_PORT_DEFINED
    9 H8 v# j- s: K/ J2 S
31.     s2 = pin;
    
32.     #endif
    - r6 @& v  V8 X# l& y, l
33. }
    
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    : V  W& ]/ y* ~: l3 {6 t$ l
40.     uint8_t Sensor_Data[3];
    . J4 e0 B. J; w
41.     static uint8_t old_data = 0xff;
    
42. 
    - [2 x+ w1 T0 I" n/ V9 ?
43.     pinMode(_DataPin, OUTPUT);
    $ v1 g+ y% r- b3 O+ t
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    / d' ~7 j. c, b8 t: I- M  l
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    ! `: c' T; N) ^0 Q* c
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    - g: a' I9 K1 a7 P& a2 i6 n' w
53. 
    
54.     LOW_level_read_time = micros();
    7 c9 p- s- S! {6 `- ]
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    2 F8 g  v3 y! `" _0 p
57.         return 0xff;
    0 \5 i0 @$ R: c3 W& G. c5 ?
58.     }
    9 t, Q! f/ C( Z$ m. P- y& C
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    & R4 ?& V0 H7 K& C. o- y* `& h
62.     HIGH_level_read_time = micros();
    , G( D6 ]9 b5 K8 l
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.     {
    9 x8 R9 {* w5 p4 M6 X& `* D
67.         return 0xff;
    ! H- y9 z* m8 j& V
68.     }
      y, U; M% h8 K  w) a7 x
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    " `( J0 X+ t2 d/ G' l
76.     LOW_level_read_time  = micros();
    " O+ B0 ?4 ^7 s, [8 v, t
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.     {
    $ T8 e$ ^# e, g- V3 K
81.         return 0xff;
    
82.     }
    
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.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    ( s: H" K4 R+ l( V' V
93.         for(uint8_t i=0;i<8;i++)
    + P6 Q  h8 I0 z' i9 g
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;
    1 y  z9 o7 [+ K+ w- \
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ; c7 Q$ _; l* B+ T4 F& L
100.             {
     3 ^! S. ]: H) q$ h9 S
101.                 return 0xff;
     ( n3 j4 U, H. E
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     - ?& {; Z- z$ B( n6 S
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
     7 S5 z5 H+ r) _1 ]4 {. Z
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     % |; `6 k8 Q+ q) d' f2 p5 j
112.             else if(HIGH_level_read_time >= 100)
     9 _# g# x6 L4 ]# S  i) |- e
113.             {
     " n- P: }+ a( z' R; b: X7 N+ @
114.                 return 0xff;
     ( O* p8 D! g- e& E
115.             }
     . A/ p7 Y% {1 N
116. 
     ' h! N; U$ d6 D- }" G6 l+ e
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     
120.             }
     ! W1 l! W( x' k3 d9 F
121.         }
     
122.     }
     
123. 
     8 w- k; b6 ]$ ~3 ~3 Z+ \2 Y
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     6 Z6 S$ o) \8 X" p* A
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     + Z( R# r4 i; W: K& e' ^
131.     }
     ! z# ^6 a- t' s) Z' F! }
132. 
     : \5 `% B/ v, w# I: ~- v) I8 j
133.     pinMode(_DataPin, OUTPUT);
     
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])))
     
137.     {
     , Q. p' L. S: y  ^! m% P2 y' S
138.         old_data = Sensor_Data[0];
     - I# D( t+ j5 E% R
139.         return Sensor_Data[0];
     
140.     }
     . {* k# M- t0 U
141.     else
     4 ~* Q8 T2 B, o8 `
142.     {
     
143.         return old_data;
     
144.     }
     
145. }
     

複製代碼

4 y$ e, ~) l. q. q2 A& m( h5 IMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

6 R# f" O; g9 R# o( g* W2 j

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
* D( J0 I! S4 b: b0 k! F能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手