mBot Ranger 高速循線範例＿

4 W& B$ `' L/ o9 i

1. #include <Arduino.h>
   # \7 [  ?5 N, I, O( B  n
2. #include <MeAuriga.h>
   6 ^$ i+ ]' ]* n& a
3. #include "MeLineFollowerArray.h"
   # c& g9 r/ M9 Y  i" ^; u
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ) C0 r% n: @6 E' Q" _: D
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   7 I  s/ l$ @. K' m/ O" l8 P' R
9. MeBuzzer buzzer;
   0 _8 N7 Q/ W# L* ?; z8 d
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    7 q8 e7 U5 f4 a/ P
13. #define Error2 2
    
14. #define Error3 3
    " r" a- M! {$ |4 }8 @5 g6 `3 I' o
15. 
    : Y( x$ R) m0 y7 |+ m
16. #define Kp 15
    * g; J5 m0 ?/ `
17. #define Ki 0.15
    
18. #define Kd 0.03
    4 j: s6 O- x6 w3 _
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    ( P0 b+ r) Y( S4 n7 y/ C
22. uint8_t D2;
    
23. uint8_t D3;
    % Y6 D; ~$ W* I. C
24. uint8_t D4;
    
25. uint8_t D5;
    6 c, P4 h. m6 s9 Z& ?
26. uint8_t D6;
      F1 V$ k5 ?! s$ Z# W8 _1 ?
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    ; L1 L9 @" U9 @" s& G5 b
32. float output;
    7 D% i0 i  V  s8 F$ ~
33. 
    
34. byte Left;
    + D+ [( V) T+ V: H9 \9 `( C
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    9 D9 E6 s1 W5 Z( H" O& b. }
39.   TCCR1A = _BV(WGM10);
      X% E/ y; {' E4 G! b
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    0 u* m( j9 i& w6 {
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    9 f* T- L! Z0 d; |' o0 v6 X
52.   do
    
53.   {
    . n8 d6 S9 k) J% U4 L/ w
54.    int Err = getErr();
    9 f+ z. x+ K. X7 }4 H4 f$ E7 U
55.    if(D1 == 1)
    # w1 L2 v# ^' `5 N" L: `) ?/ M
56.    {
    
57.      Left = 1;
    
58.    }
    9 q  x" n3 h0 d" c
59.    if(D6 == 1)
    $ H7 Z% C) }( S& x* A. G7 P$ l
60.    {
    - t6 O5 C8 l1 c6 ~2 s
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    - J9 A, M& o* H
64.    {
      _' e4 s& C  z1 a
65.      if(Left == 1)
    7 d2 }( F# @! f+ A
66.      {
    5 S- i) K$ x; y3 k- ]
67.        Speed -= 5;
    
68.        moto(0,Speed);
    2 A& Z* ]5 \6 y: q5 R0 j
69.        do
    ; \7 g' p* C1 d# \" l4 Z8 F9 t
70.        {
      ^! K' X; M( Q( r" w  c& q
71.          Err = getErr();
    6 |8 `+ p8 j. `0 z+ A
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    " ~* n6 f. Z7 Y; P& G7 P' H) O5 n
75.      {
    
76.        Speed -= 5;
    4 |+ p: _2 H$ X
77.        moto(Speed,0);
    1 F: A7 _! z* i
78.        do
    / Y  l$ ?* R& A8 P% _5 i/ I) L
79.        {
    
80.          Err = getErr();
    7 t& z* c: e! r
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    & e8 d% B! k- k2 I% `
84.    else
      @# J* W2 g  \( s( i9 B9 s- q2 v
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    4 K1 W, H8 d% |* d
89.      derivative = Err - previous_error;
    1 A1 [6 k' v( Z2 Q* R8 A
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    7 ]; r0 k4 ^. ]: n1 b6 I
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    9 b9 _6 k1 E7 T" {* Z. D6 n. w
93.    }
    6 E' f" V( y: a7 e6 m  r$ J
94.   }while(!((lightsensor_2.read()) < (10)));
    5 p5 C! Q& [, j4 F' k
95.   moto(0,0);
    
96.   delay(500);
    ; n8 }$ {; e5 [  O0 z) ]
97.   buzzer.tone(262, 500);
    + I% G" O* c& J( O( ]2 V
98. }
    ( G& [% F. y( q$ J7 [/ @) a
99. 
    
100. int getErr()
     
101. {  
     1 \+ x" `  k$ ~
102.    sData = linefollower.getValue();
       E: ?2 n- O8 e' @
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     * x) s$ y9 f+ H# {4 t% j
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     4 U; ]* ~+ \. a$ y" U9 [7 v
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);
     
111.    if(downD == 0)
     4 R0 g# n0 @& C4 ^6 c
112.    {
     % {) J6 g6 d5 g, _2 n( z
113.      return 99;
     * r( v& f6 ]/ d( G- d, P" e" o
114.    }
     / H. Z# g0 ]6 ~6 S/ f; w
115.    else
     
116.    {
     8 @3 m! s& X0 v; C* o3 K" J3 g
117.     return upD/downD;
     4 J/ D9 d0 J6 {* ]: a$ k& u' y
118.    }
     : b( Z( Q* f3 i. A# l8 N% @
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

% F; X1 R" D$ @3 _  m2 t& J& ^MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   ) Q" S) I# Y) Z: _2 D% y! a, m
2. 
   
3. #ifdef ME_PORT_DEFINED
   1 C/ C" n: J. r) n
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   ; y, u1 ]8 P' h5 X0 n2 w
5. {
   
6. 
   ) F# b$ L5 b4 T- X+ k( Q& |$ c
7. }
   " ?( l9 E1 S3 d% O
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   ) C9 I5 k8 e+ b- D  H( G1 ]
9. {
   $ E+ N! V' P, H3 k
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    # Y5 s- L4 c3 w7 U/ d- s
12.     digitalWrite(_DataPin, HIGH);
    * b$ c( ?. q9 r$ ?+ o/ ^$ {' `, d
13. }
    : b( N" m1 o. t  v
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    2 c, T5 A9 d3 |: u3 U2 B
16. {
    
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    + ]8 o7 |$ i7 j1 a
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    # J$ h+ C, i9 @( N* S
23. 
    9 e/ d, ?8 k0 j5 v
24. void MeLineFollowerArray::setpin(uint8_t pin)
    ; s; J1 ^2 q& _+ l  Z
25. {
    0 D8 r- |9 M' r2 g! B
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    ) A/ f2 l6 X0 z5 q, t& h5 ?
28.     digitalWrite(_DataPin, HIGH);
      R; v- g& ?( K1 M) W7 o
29. 
    8 \" F: f$ P$ ?
30.     #ifdef ME_PORT_DEFINED
    + P! p9 y1 H5 X$ p+ t' Q
31.     s2 = pin;
    5 B8 _0 j) b; C9 b: F9 b' L
32.     #endif
    , p* i( M; f9 U: {
33. }
    * N4 a% X4 N- @3 c/ [( e. v" g
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    ; G- U7 u, F4 V& m8 r, z! B
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;
    3 X1 v2 p9 L" W
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    * _+ ]: K2 i' p, ^4 s" f
44.     digitalWrite(_DataPin, LOW);
    7 m# [* \1 U5 \
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    % f1 d0 }$ @( ?2 N
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    $ v" g3 ~. {6 @/ T3 e& m0 r# g
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    & n$ `, x0 b0 o& k7 i: [
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
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
    6 A& }# [/ x% B  R0 A( \& v( G7 ~
66.     {
    
67.         return 0xff;
    . l2 x5 K+ p5 L0 s' f
68.     }
    7 r9 S; ]6 N; w7 m
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    # Z1 i6 H7 _" I3 c# j
71.     {
    
72.         return 0xff;
    8 C' W) I) A1 s% U
73.     }
    9 L1 z; c! s3 v# U7 G. S! x
74. 
    " i2 z! _: q: r  s% ~. @
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    . J' Y/ O0 q8 V/ a6 J# ~
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    7 l/ _7 z5 y6 `0 m4 i
78. 
    ; d. f$ m7 z' ]9 u8 Y9 M4 m
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    , b2 P3 Y$ q; E: N+ E. z6 j
80.     {
    
81.         return 0xff;
    ( d! ?7 G/ G1 K
82.     }
    0 m7 y. W* v% v8 z- H1 b& Y' v
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    5 @$ R$ v$ ]0 w' [; a) A& k  V. X4 y
85.     {
    2 R8 B9 @$ ?* J& L$ o* [+ n
86.         return 0xff;
    * Y  S/ z7 R" \4 @! J
87.     }
    
88. 
    # X% H. }3 I9 L; _8 J
89.     for(uint8_t k=0; k<3; k++)
    % \) t- M! I0 K6 D) r: @0 U5 O
90.     {
    / W5 N5 b( W0 q2 l' J  f: q
91.         Sensor_Data[k] = 0x00;
    . u6 {' Q6 j, C( a5 o  Q, |
92. 
    & G  S! t6 A4 _3 h4 `
93.         for(uint8_t i=0;i<8;i++)
    & ^, t2 F0 u  y
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;
    ' U5 E2 Y: e9 p. y
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     ! `& K( e% k3 ~( d7 d. y) B( W
102.             }
     - n6 {2 e9 f& X3 C" b
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     . I$ U$ S- {" d- H+ t( m3 F- _9 X
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     + P0 W  K$ r) s# F1 V* S4 k. R8 e0 `
107. 
     $ ?% X1 j0 }$ W) c4 x- f3 P
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     5 C: c  a/ c' v' J) W
111.             }
     + t: O6 e5 z* N0 {1 A2 n; w, l8 Z
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     # M# W8 _" v5 F+ ]& k( Q8 O4 b  B- _3 S
115.             }
     % N* L0 _5 Y) H) o9 t' r
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     
120.             }
     6 f5 u9 `" v; `8 z5 C1 O: X
121.         }
     
122.     }
     
123. 
     5 K9 m6 u; a6 Z1 S
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     5 c( u: A- _! O' X8 d+ D, e
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     - u( w* r6 e+ ^
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     8 {" o  Z1 j" `7 v' Y- z0 }
129.     {
     
130.         return 0xff;
     
131.     }
     / L9 a) d5 G5 ~' F! \  v1 O: B! h: D
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     4 n0 @; B0 {' H
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])))
     8 S% Z+ [8 ]  ^7 A( }/ w
137.     {
     ' R' p1 [$ t9 d9 Y1 k/ j
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     + k( ~7 D. e! B/ L  U5 u
140.     }
     
141.     else
     $ ~8 U8 L5 b3 v& N7 l  d/ c0 `
142.     {
     
143.         return old_data;
     
144.     }
     # r- q" ]6 x  B  h9 m- Y; `, u1 i8 W
145. }
     # t. @/ X9 J5 v0 d! E7 j0 A; D

複製代碼

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

2018-2-13 01:52 上傳

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

" i: H1 Q/ n% [, x

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
- k) P# i8 R9 i) Q哇...要100 個銅錢

7 e/ |7 i5 {3 |; l3 j' G" F% T認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
; s* E! N- r3 Z8 L3 g! A* R) b. g能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手