mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   ! }: b  Q  M/ ?. R# j4 O. O4 ?
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ; K8 n0 k4 i9 E' l8 ]
7. MeLightSensor lightsensor_1(12);
   1 a0 j+ ?3 e5 J9 s& L" c
8. MeLightSensor lightsensor_2(11);
   . f2 F# S$ N+ s
9. MeBuzzer buzzer;
   7 d: C* j. |. f* L: T! x
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    9 n  R3 j4 m" h1 a8 P
12. #define Error1 1
    
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    / {7 b" t9 \5 V) s; X  ^5 J
19. 
    
20. uint8_t sData;
    # ]9 w; u8 [' Z3 E. s0 ~0 G# e
21. uint8_t D1;
    
22. uint8_t D2;
    8 w6 w" h! C! x. V9 j
23. uint8_t D3;
    - f/ `+ M) e+ k
24. uint8_t D4;
    
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    % p6 g0 J: w0 c
29. float integral = 0;
    7 l. }# e% w& B( G% W% A0 D
30. float derivative = 0;
    
31. int  Speed  = 160;
    ! `/ F' i' X3 C$ n" @# y) i
32. float output;
      X2 {; s! D# R* ]7 b
33. 
    
34. byte Left;
    1 H, I* A8 Z! j( F
35. 
    
36. void setup()
    - v# b! ?  j- e# ?/ X' H
37. {
    
38. //Set PWM 8KHz
    " v1 s" ~) Z; ^8 P  X5 O
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    : i; p0 P4 |  v  R, n5 D* v. F" t. {
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    7 E/ ?6 f. F9 V- o( N! \* U
42.   TCCR2B = _BV(CS21);
    ) W" Q  V: w( b9 `& J7 W
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    5 t9 W: ~# ^) O1 M% B" b: H
46. 
    
47. void loop()
    " N9 k  B: b* |4 t/ s) n4 o# Z
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    7 y' R$ n) V1 q& C. C
50.   buzzer.tone(1047, 500);
    1 B, r0 V8 z, U, a
51.   delay(500);
    . Q6 w  {4 G5 b* [$ d5 e6 Y% S
52.   do
    
53.   {
    8 q- c9 H7 t9 x! J# b+ `
54.    int Err = getErr();
    4 l5 X1 p5 i. j) l5 K% v4 k' Q  p
55.    if(D1 == 1)
    & m5 ]; `! i: O* V$ T" ~! g1 }# T
56.    {
    0 L5 e& M- V7 f$ y5 I- x
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    3 D. d. A: t$ b+ K) h$ x6 p! V
62.    }
    
63.    if(Err == 99)
    $ u9 z8 ^1 X2 N
64.    {
    4 J0 i2 h5 A! \: o) p9 \3 K7 `
65.      if(Left == 1)
    
66.      {
    $ ^+ ~. A! U* l* ?$ k; J: M
67.        Speed -= 5;
    
68.        moto(0,Speed);
    7 N/ [7 S; U: B$ Z+ s# ], i9 h
69.        do
    - K/ b& F' u7 {
70.        {
    
71.          Err = getErr();
    $ I" a, l" w& `* {8 s& P7 n3 L- L
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    9 h! x8 c2 l8 n" s, q6 L. r
75.      {
    $ x' p2 e/ D0 p) B
76.        Speed -= 5;
    , {/ I4 e! A/ e1 q3 F) d3 S. Q
77.        moto(Speed,0);
    
78.        do
    - c( S( [% s$ @' l4 M
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    1 r; h5 U" `" B, @: D% t& h
84.    else
    
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    - W1 h' n: R' m: ?' R
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    * V: q3 p2 O  r4 u
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    1 _$ r. C" I* i) _1 e3 ?' q
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    : j. l5 D5 I4 ~* e/ d# G
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    % L7 J2 ?/ N. m6 f) `
98. }
    # Q! G0 c& c* `1 a. l# C6 t
99. 
    + e: c+ e9 U' A$ }0 I- Z
100. int getErr()
     6 @. W9 E" _4 ^- S
101. {  
     % l9 r$ Y: G. P9 Z6 N2 ?% {/ K
102.    sData = linefollower.getValue();
     * ?' f" w) A2 v6 M
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     / {+ y  c% Y0 D. s: l+ ]- T" t( K
106.    D4 = ~(sData>>3)&1;
     5 B) r* D. q. @
107.    D5 = ~(sData>>4)&1;
     , F2 O: ?  E; q' y, N
108.    D6 = ~(sData>>5)&1;
     , u: i  @: I- x9 t3 l  }
109.    int downD = D1+D2+D3+D4+D5+D6;
     ; p9 B0 ]% b4 H+ B( `
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     0 T3 k( ]9 i. ]
114.    }
     $ z8 F9 |2 t( _- G- ?: g8 B
115.    else
     
116.    {
     + \! [3 ]/ q+ Z& H/ E0 S
117.     return upD/downD;
     
118.    }
     
119. }
     # L( K$ J! ?5 b
120. 
     5 ]6 `, I/ z- H8 b- w2 N
121. void moto(int Speed_L,int Speed_R)
     
122. {
     . A% @* E4 n; ~+ N; Q/ k
123.   Encoder_2.setMotorPwm(Speed_L);
     6 b3 U: j$ N8 P# z& H- j
124.   Encoder_1.setMotorPwm(-Speed_R);
     " c, f# g- D, r( \  e  p! H
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   - T6 U! _, _* n1 v' ~2 D& ^! @
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   7 @; e' _" H5 z7 R9 ~
5. {
   
6. 
   : m' w! F/ [2 |- Z8 U$ X
7. }
   1 I/ l8 n& D8 o  y2 {
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   + ?* u+ b8 `1 Y4 |: c; m; g
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    ( ~: Z- ~- D" t6 t
13. }
    / y  O* \$ a/ s% [( w
14. #else // ME_PORT_DEFINED
    5 [- _4 V% x! i
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    / G7 S$ F* o" w2 A9 `6 ?
16. {
    7 Y! ^% p2 d- N& h! B
17.     _DataPin = pin;
    % C9 E& ]- [* N. @! b
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    # l7 p& D! ]0 I' B
20. }
    ) H/ e7 d8 ?3 d3 y0 h% Z
21. #endif // ME_PORT_DEFINED
    ! ~+ s$ L6 k8 ~, p
22. 
    
23. 
    * J3 Y0 z, Q3 M* D/ L1 L" x: d! K9 l
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    1 t/ C3 G) x4 U! j! E* d3 Y
26.     _DataPin = pin;
    8 e, H- K* k, w% z  z0 I8 a
27.     pinMode(_DataPin, OUTPUT);
    . V7 u( e7 J5 Z3 j3 J, q
28.     digitalWrite(_DataPin, HIGH);
    5 q- z  W3 w  q! T1 T8 Q1 T9 K4 K8 a
29. 
    & A9 j/ p/ Z/ T+ J2 v) ~3 I
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    ' P+ B3 @& u" `
32.     #endif
    
33. }
    " F3 Y% ~1 B0 g8 O
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    , Z' p, T; R+ O! T
36. {
    & z2 f, d. S2 v+ M
37.     uint32_t LOW_level_read_time;
    9 v4 ?' _* m/ ^2 b9 @  x
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;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    1 C3 h" Q6 W4 c
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    8 {7 ?5 X9 g+ @2 p
46.     digitalWrite(_DataPin, HIGH);
    4 r) I. K+ i2 i4 T
47. 
    & T+ o! J  m4 O& `5 @
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    - a/ D" v, I- _8 O0 s  F
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ; H" J6 r9 s2 G. H
53. 
    & C( R9 ^" P4 m$ j  Q/ b" Q
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    $ t; r/ a2 P5 t" w& ^6 {
57.         return 0xff;
    
58.     }
    : u5 K; j- _6 L4 V9 K1 x0 a
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ( W$ E( h: n, E2 {( E( _
61. 
    
62.     HIGH_level_read_time = micros();
    
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.     {
    
67.         return 0xff;
    
68.     }
    / X3 e* k, o- s4 G5 M6 v$ }$ N
69. 
    % M) A" \' ]% o( j( L9 G: A
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    ' b9 E6 o. [1 N
71.     {
    
72.         return 0xff;
    ' e' L" `1 s% u  t/ j
73.     }
    
74. 
    + B6 c: A# R- ~/ g$ G+ d
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    # H$ p+ }# i1 R) Z9 ?% A4 M9 P
76.     LOW_level_read_time  = micros();
    # ^; C/ j/ A8 o6 A
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    $ R  V% F' i, `* h+ D$ @3 S. U9 c
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
      @9 Z! V) s* Q# ?7 j  I1 ^- n
81.         return 0xff;
    
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    ! h" b4 K1 o1 f& l3 c3 t
85.     {
    % a9 M6 q9 P0 w( {
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    , d0 Q& w4 ]: w7 @
91.         Sensor_Data[k] = 0x00;
    6 k, k3 \9 F3 V) X
92. 
    . E! i# {. n: B; U
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    9 g* }, u, q; `! X7 B
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;
    8 V; L  ?/ t9 R2 q# T+ e
98. 
    . F8 M& ?7 \. f) V4 }" M  H
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     : [$ Q( r/ Y0 q8 v8 {* p5 h) ?4 a
102.             }
     % h  Z) g: K2 m0 D4 f
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
     + _- e# S& R( m: d5 _2 U5 g- \
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     , x& Q, O: H0 L( N$ o
109.             {
     9 c- G# Q7 Z1 @/ x/ N) }  [
110.                 Sensor_Data[k] |= (0x80 >> i);
     8 \, H8 ^9 C( v2 ^+ b# I! \6 n
111.             }
     2 h( ~: Z& v) `3 V
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     . o  [0 u4 @0 r% u' b& ]
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     % q  V. J1 i5 X( M6 R9 T# T" n2 d1 T+ p
118.             {
     
119.                 return 0xff;
     9 g- q% C- n" v( A) N
120.             }
     7 M& C# K7 G) p
121.         }
     # y9 k, |! W+ ~& E: ?* V
122.     }
     
123. 
     / e* `) h$ v: j* H* T7 u4 h9 L. w
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;
     
127. 
     7 ]0 g  h; h$ b
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     : _3 Q, X2 ]2 @$ _; u
129.     {
     
130.         return 0xff;
     
131.     }
     ; X2 Y3 ^6 l1 C6 ?) e
132. 
     - E* A. |+ q/ \0 B: T$ E- u4 G! u$ `: 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])))
     . t; S$ t/ X' c. W  X
137.     {
     
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     - |+ P7 t- a+ F+ ^$ U7 F; t2 ]
141.     else
     . B% E/ G5 J- s9 }& z6 y+ o
142.     {
     6 m. r# l; [4 d7 w
143.         return old_data;
     
144.     }
     
145. }
     

複製代碼

& V' `8 \0 C$ ~" EMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

3 Y" P2 m( a8 t3 a7 a7 w) r# P
/ z$ w6 K. q- \- [4 f" D/ o! a

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
1 k# k) K; B! e8 x( x, j能否用到mblock 5 上面呢？

* @% E9 Z# ]. i0 S( f( k! TMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手