mBot Ranger 高速循線範例＿

/ c* T( f# p- w  P/ u

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   + w' M" ^' ]5 }: P5 L! X  n
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   4 b  z0 f' T+ y5 I& y
6. MeEncoderOnBoard Encoder_2(SLOT2);
   % X7 R! x1 s/ I7 W4 N, @7 U" i
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   & f7 s0 Q/ ]& W) |! a' B
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    " e6 b0 d+ I; l8 y
12. #define Error1 1
    
13. #define Error2 2
    4 ]. j) `( F& j+ x6 w5 B' Y
14. #define Error3 3
    
15. 
    & _* q) e; ?% Q" r
16. #define Kp 15
    & s4 b* p: N# ^: K: C
17. #define Ki 0.15
    # c7 l" A; }) L4 X
18. #define Kd 0.03
      N! O6 h8 i! l% A* J% c
19. 
    + ^9 W$ |0 J0 J
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    ; @  b8 _7 ]9 H" _6 B, G
23. uint8_t D3;
    # C& S% @- H, S  }7 `- Y) J
24. uint8_t D4;
    8 ^% j! y' J, e9 R# |* o
25. uint8_t D5;
    - n0 B+ W4 y/ R+ p# m0 i6 v% L
26. uint8_t D6;
    ! [( g7 t$ X  u$ ~  _$ m
27. 
    
28. float previous_error = 0;
    . Y2 t3 P9 Z3 z6 l& ~5 A3 t
29. float integral = 0;
    
30. float derivative = 0;
    ! r; F0 U/ E0 C$ L/ ]# `0 F
31. int  Speed  = 160;
    
32. float output;
    ( X0 i5 l" L9 n8 A0 f' ]6 t2 a# j
33. 
    8 j4 f- ^$ f. o8 a
34. byte Left;
    5 B# r: c, m& G6 F6 x) s+ x/ d
35. 
    + Q' [. D7 A" P: M
36. void setup()
    
37. {
    3 Z9 j) E- V1 x* q' n7 y" x
38. //Set PWM 8KHz
    # D6 ~2 y2 _. N. r
39.   TCCR1A = _BV(WGM10);
    ( r7 f* a; [3 c  n& v
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    + A- M4 b$ v; H9 L
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    
48. {
    3 P( o# c, Z. z% n: [
49.   while(!((lightsensor_1.read()) < (10)));
    , P$ L/ V9 n3 _! S) n1 V$ X5 [
50.   buzzer.tone(1047, 500);
    * E, l! C" y* c& H; B( R
51.   delay(500);
    
52.   do
    ; r8 r  S1 b+ |% R
53.   {
    
54.    int Err = getErr();
    4 L& L. R' @9 b% B' V+ I* C- T% w2 U+ o9 O
55.    if(D1 == 1)
    4 F! W8 ~$ M0 U
56.    {
      ^: |. X- d8 [
57.      Left = 1;
    1 C& m' h$ |% f
58.    }
    
59.    if(D6 == 1)
    
60.    {
    4 T9 @7 Z. Q  u- B; S6 g
61.      Left = 0;
    
62.    }
    ; ~! j  j5 c; t5 F& E" t2 o+ W  {
63.    if(Err == 99)
    
64.    {
    & ^2 ?+ E5 {+ n$ C; X
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    $ H* e# |% G2 x- i
68.        moto(0,Speed);
    " W( m7 R% H- L  }- W+ i' D
69.        do
    8 @  W  L$ I7 H3 P& [
70.        {
      q; C2 B2 \: p  t# i
71.          Err = getErr();
    8 ?; u: Q# N. H) a4 G# W; i$ ]
72.        }while((D1+D6) == 0);
    , v! k; V9 Z3 Q! r
73.      }
    2 x5 w3 q- P# y
74.      else
    
75.      {
    : d4 J- ?0 K& W. W  r
76.        Speed -= 5;
    , K& G+ g: H; o  L7 ^7 {" N
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
    $ Q( S0 u3 f- h/ e- F2 Q
81.        }while((D1+D6) == 0);
    8 W9 s0 m0 |8 R+ Q: u$ {
82.      }
    
83.    }
    
84.    else
    9 o" \& n" v! ~0 Q- J: V
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    * R4 w* E- B6 T  N, ~' b
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    - a$ _6 V6 U- c1 K( w
88.      integral = integral + Err;
    * h" d( _' R5 T  X
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    ( t3 p* y+ m7 F+ z& M4 c7 h. k
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    + B; ^2 ^. n/ G9 e- c* y, B
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    0 c8 A/ e, m+ m
96.   delay(500);
    ) p! K, f' U9 T3 o8 n
97.   buzzer.tone(262, 500);
    ) _/ B" k! H0 ~& R6 Q
98. }
    
99. 
    
100. int getErr()
     
101. {  
     $ X6 \1 [  j7 ~
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     2 \. b: F, i7 h, a
104.    D2 = ~(sData>>1)&1;
     3 |+ c1 k$ S* `5 f
105.    D3 = ~(sData>>2)&1;
     # o; a" r( b1 Z; u5 i
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     * K4 H' \8 k" v9 S( f
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)
     3 u& x7 ^+ A3 y8 P4 x) I
112.    {
     $ v# P$ b6 I  I; W1 w6 U6 }7 G1 [* f' T
113.      return 99;
     ' m' G  X2 @" l9 m) [0 M. u
114.    }
     
115.    else
     
116.    {
     " X7 Y3 A! O9 Y" p" T$ Q1 E
117.     return upD/downD;
     ; J) j- f) G4 A# T
118.    }
     
119. }
     
120. 
     4 j0 G: j& f% X, y, U( m3 R5 C
121. void moto(int Speed_L,int Speed_R)
     $ ]  H' U" r$ R) \
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     + r& P( D1 f1 P( Y  W- }
124.   Encoder_1.setMotorPwm(-Speed_R);
     3 b& w6 v! U& f$ {8 d9 K
125. }

複製代碼

, b0 M/ j( k( p; p1 f% L# a" _: pMeLineFollowerArray.cpp
6 z/ ?" U8 D4 N

1. #include "MeLineFollowerArray.h"
   3 G+ x$ P: m- O: @
2. 
   0 @: t4 `2 i: w, p: L
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   : o3 p! |8 ~% E3 L- G! N" e
5. {
   
6. 
   
7. }
   * J9 j& }0 d+ \5 v
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   , d! k% ~- |2 A& j
9. {
   8 r6 C$ q/ L* A1 U/ t
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    : d6 k5 x2 z  w
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    + @  y) G- L# F
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    " Q* K7 \7 ~7 H) {
16. {
    / Q9 r8 V/ F# u5 U4 t" Y
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    . K  m$ m+ E( x" O$ \( F
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    ! Q4 f* [2 L' W# r% a( a6 T  Q
22. 
    # l. P) L* R( _- J/ F3 g1 Z1 X
23. 
    " r" w! T4 ]( c7 \4 X
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    * o' o: F+ r6 H4 Z6 G
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    , I0 _# B( G  Y% n- x* H
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
      s( H. w" q1 w' \+ f6 o- C
33. }
    2 C  M( c7 e/ H) O% d: `
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    ' V, B( k4 i% |& E/ `3 }
38.     uint32_t HIGH_level_read_time;
      V( Z1 l2 N3 E9 e8 L
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    - `: t6 X* w2 {/ i7 s2 u
41.     static uint8_t old_data = 0xff;
    - E( D$ d7 v  l2 S
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    ) f0 M) T' h8 L, B
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    + O! g, L7 A# T$ `! ?
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    6 g$ Y7 x, q* x, f! q" z1 U, d" n
49.     delayMicroseconds(10);
    : T! _2 j+ F5 r* I" J9 o
50. 
    0 X: y8 M, c3 {5 J
51.     time_out_flag = millis();
    - ~. x7 e( F7 N, g9 m0 E$ u
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    3 C' ?; t$ Z( {; D  N( [
54.     LOW_level_read_time = micros();
    / X; o& _5 c. D6 I/ z: {
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    6 X, L8 x) }& M
56.     {
    
57.         return 0xff;
    / D. r* g2 X/ o) Y% W  K
58.     }
    
59. 
    7 O$ m0 N4 Z% Z# H) K' n, f
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ! ^6 G5 A9 a4 a! E5 U0 x+ {( H9 j
61. 
    " I! C, a; ]# A. o
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    + \( a/ k6 R0 e+ e) s6 S
64. 
    9 y2 C5 [: ]# @0 i/ o& Y4 Q
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    ) Q: n, x+ \1 W6 T) U
67.         return 0xff;
    
68.     }
    7 @4 s; Q, }) ], E
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    2 |( _* {4 r, B' V
71.     {
    
72.         return 0xff;
    
73.     }
    
74. 
    ( M3 v" [4 k: k. e) j* L
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    6 V# @7 M% n& O% q
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    . K( Z/ h' b) N: M3 }% i  i: F
81.         return 0xff;
    1 l; Z/ Y, M' [4 h
82.     }
    , x5 i; e, @! A5 v. m% `6 p
83. 
    ; J- C4 C# v. S9 i
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    1 p; r9 P7 ?" Z5 U' w1 W0 H
85.     {
    ) B: H5 h' T! V$ Q
86.         return 0xff;
    
87.     }
    5 J  ]7 T# }. r9 k; O
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    8 [+ c5 |3 j1 s/ s3 o7 H' K
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    # `2 q' q- y9 I6 z
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    3 k' s% y/ \2 k8 _% D* b* \' K
96.             HIGH_level_read_time = micros();
    6 ?+ H  l. z' Q( r7 S5 U
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     ) J4 w; a) i" R
101.                 return 0xff;
     : t- K, x) ~" g$ A; k  y% j% K" r
102.             }
     - {8 Q3 o( ^' z- x8 V5 [# j( a5 g
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     6 U, t9 x& v$ D
105.             LOW_level_read_time  = micros();
     # Z0 u( ]/ \: N
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     8 v. Z9 Y1 B5 E. k* @/ C- T0 i* v
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     # O9 W3 E2 H, Z* H( S7 k/ ?6 P
109.             {
     9 N. F9 ]1 g; B1 Z
110.                 Sensor_Data[k] |= (0x80 >> i);
     0 K+ G0 ^; [  K
111.             }
     & i+ j% w9 M& G. o3 f
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     " w, P: A  h; d1 Z
114.                 return 0xff;
     
115.             }
     ! J+ R) B- y$ L6 \8 U5 u
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     8 n4 S' ?9 ~4 q5 ^* X( u
118.             {
     . ~; z* e( B! S( w8 S( _
119.                 return 0xff;
     5 ^" [' |6 @2 |" ~& `
120.             }
     ; b: a% T& U. y) x8 ~6 p9 y
121.         }
     
122.     }
     1 l7 a) `. j$ B/ C
123. 
     9 |0 p  r9 b; H5 u% S4 G0 a
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     $ F& u3 |) ~0 s0 N! M
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     5 r" B0 U( N( d% W" x
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     + q  E! Z% x* f' c+ I2 l) \. K
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     ( d! ^3 g3 Z% Z: T& m
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     8 b5 {4 s! ^; k7 `- x) f
135. 
     + p( P/ p! W; c! M9 E7 E1 m
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     
138.         old_data = Sensor_Data[0];
     ( y  g; Q1 J7 m* s) G) u
139.         return Sensor_Data[0];
     $ \3 @* I0 G+ Y% G; ]' z* _- C
140.     }
     
141.     else
     
142.     {
     + ]  o6 H' A" @5 k2 z2 U* o' u- V
143.         return old_data;
     
144.     }
     & @* o% d2 S# A1 m$ `! T+ F9 [; l1 D
145. }
     : \; z9 x" R+ E( u; @" s6 A% B

複製代碼

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

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

3 x& T2 v0 q8 Z0 ]認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

* ~6 u# B; A. T5 g9 L/ u) E6 |MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手