mBot Ranger 高速循線範例＿

, X. f. w: x/ H

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   - e2 _% n1 i  m- ^7 C# F; f' W8 C" X3 ~
3. #include "MeLineFollowerArray.h"
   / \0 J5 B& b& ]! \
4. 
   4 |* K6 K2 _7 k( W
5. MeEncoderOnBoard Encoder_1(SLOT1);
   ! M' ^! E# L# c' n+ T+ h
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    6 a2 t4 k3 y0 z. d3 `8 g- t, f/ l
13. #define Error2 2
    ( ?5 N- ?5 ~! K5 N, K$ {
14. #define Error3 3
    9 T* ?* d" g' s, y* O
15. 
    5 m- h& h7 U7 B6 k
16. #define Kp 15
    
17. #define Ki 0.15
    9 h2 d1 }0 W( z% e9 f8 b
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    : e& [; [' ]5 W
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    5 w) [+ {1 ^' @4 R2 q4 I
25. uint8_t D5;
    8 D  l+ D( G- g- y0 K( D+ E9 B
26. uint8_t D6;
    ( ]' F8 }% N0 S, s/ P7 s- D0 o! R
27. 
    + s1 K9 s% J+ i8 z5 v2 E
28. float previous_error = 0;
    $ m' G3 X' \+ H6 Z: [* i
29. float integral = 0;
    - A, j5 Z4 C* ]
30. float derivative = 0;
    
31. int  Speed  = 160;
    3 e  h& ?, }$ q1 ~6 f8 Y) o" N5 F
32. float output;
    8 z5 X+ G( ~4 x2 l- a( i
33. 
    
34. byte Left;
    
35. 
    4 Z+ J3 X& Z! {" y" C4 n  M) g1 u, d
36. void setup()
    
37. {
    , P8 s2 m) [! W! s9 T) y5 Q
38. //Set PWM 8KHz
    # z7 B7 V9 L. K
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
      g, C# G! b- Q% M' d1 T: F8 S
46. 
    
47. void loop()
    1 t! i2 @+ N% @& h4 A2 O( D- f
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    + ~( m* K$ i: a# i  O1 ^
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    3 `# M5 t- l& B" K: C! q: R4 V
52.   do
    0 @. R3 V1 K3 L* Q
53.   {
    4 e0 J2 E3 }7 u; X- t5 {2 w
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    2 s. S* G2 j! f
57.      Left = 1;
      F  g9 m0 ~3 G  Z" i
58.    }
    - v$ O5 v9 H9 J3 P; G: S1 U
59.    if(D6 == 1)
    5 ], v1 e! d& {( l$ \* n# p
60.    {
    3 n/ C; P; n+ R6 D; u5 t! I1 K
61.      Left = 0;
    
62.    }
    2 i  q) [& P4 y# k7 k
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    + X' N- U9 F) V! v0 Q. W
66.      {
    ! h( d3 w# v7 N
67.        Speed -= 5;
    3 F6 d/ N4 x% f$ a
68.        moto(0,Speed);
    / f8 L7 |. e$ ~6 m" F2 P
69.        do
    - d5 p! [: R2 A6 b
70.        {
    
71.          Err = getErr();
    4 C" G8 ^6 |  [/ ~- z" S
72.        }while((D1+D6) == 0);
    
73.      }
    & a6 n$ n- k& H4 h4 Y9 G  n
74.      else
    
75.      {
    # n0 \5 j3 s3 a/ y4 ~
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
    ( B4 J  T& a5 H. L$ A0 }) {
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    " J3 C- E4 z7 ~( S; ~  m
82.      }
    ; N) Q; |: k3 |: z4 d& C
83.    }
    
84.    else
    1 S, S5 J: d/ g
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    9 b3 L+ n8 ~0 h, y0 ?- z
88.      integral = integral + Err;
    ( W/ d- L6 I" n+ X7 B: z$ m
89.      derivative = Err - previous_error;
    1 i, K7 c8 s9 R: Y
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    6 D# d& z3 t- F1 O
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    9 q& o- W6 M) \; b, k
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    3 t8 Y; P0 J/ L' @6 w, H7 ]
97.   buzzer.tone(262, 500);
    . l3 @. `; `, r5 h* d- u
98. }
    7 W& s/ N8 |! |
99. 
    3 d7 O+ ^, l/ i0 |
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     3 m0 C  v& l1 M  Z3 `
105.    D3 = ~(sData>>2)&1;
     + b5 c9 h7 k, G% N1 V1 v4 h( P
106.    D4 = ~(sData>>3)&1;
     ; {  I( m, b" j
107.    D5 = ~(sData>>4)&1;
     9 e' s6 O6 c! @' S
108.    D6 = ~(sData>>5)&1;
     3 F/ I( u* c/ a2 l; m% A
109.    int downD = D1+D2+D3+D4+D5+D6;
     6 M/ J/ l3 y, n& A
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     ( M; G. Q$ g: v
111.    if(downD == 0)
     7 C& j3 g' v" c4 \2 V% b
112.    {
     7 P. H' T1 u  g( o
113.      return 99;
     7 F/ J- X4 R4 Y* l
114.    }
     / N1 ?  Y) W. B3 x# z7 I; s) ^
115.    else
     . a" u+ l& d/ N
116.    {
     ) V# R6 T$ |4 ]% F) b
117.     return upD/downD;
     
118.    }
     ' S- P1 K, A5 R- R
119. }
     + A2 N7 C; T  X& @8 n
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     + i' l1 r% |. h* N* [* h% g) F
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

  H' W$ R& X6 j% m9 r- [5 ?& Y7 E% GMeLineFollowerArray.cpp
7 i  b4 H+ @. B2 s, o

1. #include "MeLineFollowerArray.h"
   / G2 `0 A9 ^' g2 |& p# h% W
2. 
   
3. #ifdef ME_PORT_DEFINED
   : l& y) @9 F+ j6 `* t( h3 M
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   1 m. l( Z& |' {9 j/ O5 n# g
6. 
   , f" w; n) _0 N* \! M  ?1 O+ J3 R0 ^+ F
7. }
   ( `* W% z: m3 T/ K) Z( ]' J
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   $ X9 X/ Z( [, N/ L) R
10.     _DataPin = mePort[port].s2;
    - f/ ~* u, ?) K+ f: e: j9 b2 A
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    7 L0 o! A2 p  R+ _) a1 b6 B
16. {
      l' s0 }0 E6 K9 J, a
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    , O* s5 a, K$ a2 b
19.     digitalWrite(_DataPin, HIGH);
    ' F+ u. V/ l* d5 H' U& `) A5 m* k
20. }
    
21. #endif // ME_PORT_DEFINED
    " f9 f8 @7 Q* P  Y$ |
22. 
    2 t5 f9 W$ @$ q! d' L' s  E
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    1 o6 Q- s/ h! O
25. {
    ( D, a4 Q6 ~3 t" U$ j0 a
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    ) P% I9 s; }" R3 j1 _
29. 
    . z! M' i; F$ O. s, N3 n2 m
30.     #ifdef ME_PORT_DEFINED
    ! O+ i: |/ B( q# J! k# M
31.     s2 = pin;
    9 {6 N2 D, |# W$ f
32.     #endif
    
33. }
    
34. 
    $ n) x- K" q& d6 X+ X
35. uint8_t MeLineFollowerArray::getValue()
    , F( M4 [, M! g6 }# r" O$ C
36. {
    
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;
    
42. 
    % d8 s  r" K/ ^5 i
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    - D5 S% v, ]4 m2 Y9 A
46.     digitalWrite(_DataPin, HIGH);
    6 @+ V- K, S9 k, u$ ~) x
47. 
    , B7 x) A7 v+ t' W' o7 J
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    ; G/ o8 V7 b; I6 h: E0 K2 E
51.     time_out_flag = millis();
    1 f8 s; i" _: C+ g
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
      b- _* M! }4 D! i- U/ |8 B% b4 _
54.     LOW_level_read_time = micros();
    3 g* r+ T6 @9 N
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ' y; Z. n: F1 {* w6 l7 L
56.     {
    , {! o7 r' @, H" W9 ]$ Y
57.         return 0xff;
    
58.     }
    ; C( _- X1 E. {
59. 
    : V+ N9 ^0 I& ?7 g3 B7 |0 n
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    ; _0 N  G( c' w$ a; ]* b
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.     {
    / b$ v2 P- A5 r4 \9 r
67.         return 0xff;
    : p" p5 g& ?( y  f. ~
68.     }
    % k8 @0 R. D9 I" h) |. r2 j0 f
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    8 q4 D0 \7 Z8 y3 ^. s3 K9 C  I
71.     {
    / |4 N  J) D5 k% t# t2 q7 \
72.         return 0xff;
    & g; ~7 m# e# ]4 M/ J5 H5 y0 v4 H
73.     }
    
74. 
    
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. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    + D$ Q$ s' }& j) \7 n& {
80.     {
    - Q* w7 ]* ?$ Y) O' f3 \
81.         return 0xff;
    * R; T( i/ _7 T. W! S
82.     }
    
83. 
    , f' Q9 U; g3 L* g/ t# l- a
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    * y( b  Y+ r( t( w, r: i
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    4 @1 ]5 q- |6 q- f, v4 k
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    ; L: z' \4 m- D' M6 y1 L
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    + n# x' b. J$ Z! ?! f' C+ L
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    + j  V+ w7 o& f7 P7 E$ C" B5 V
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    6 {6 w+ m" Y1 Q# i- J! O
98. 
    ! W$ p0 t- q4 ~  S
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     ! D/ n" v3 N$ m) g/ W
102.             }
     
103. 
     6 c1 w2 A: k2 c4 M- y( U; C3 ]
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     # C- s5 D, Z4 v
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     . N; u3 P/ R6 F! J8 K
107. 
     7 R0 ^5 ~5 X* E- [
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     7 w+ y4 X  X/ d7 C, \
109.             {
     % {+ d( C' o7 B. v8 z; c$ u
110.                 Sensor_Data[k] |= (0x80 >> i);
     # M; }8 L+ X* q& Q, U
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     + [" ]0 F6 \* N+ v) f+ R6 I
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     1 ~7 [3 T8 c4 E, y
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     * D# U: h* f) D7 f- P$ r) z
120.             }
     / q+ @& G& [7 [5 n8 F  b% ?
121.         }
     
122.     }
     
123. 
       ]' ~: b" C$ ?4 B2 X/ Z0 c
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     . m/ @* c4 E3 p: s/ q
125.     HIGH_level_read_time = micros();
     % W1 L* L  x- N/ y; s
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     ' M5 y1 @! ?- o2 y( w
130.         return 0xff;
     0 B: Z" d) n& _- D6 K
131.     }
     
132. 
     % u/ }3 y; L! H" a2 c8 V6 O+ k
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     & D- U: j* z4 h; N
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     ! s3 ]  C; R- k3 }) j, p: Q& |
137.     {
     7 w: Q5 e9 B  Z- ~% ?' I( m
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     6 j' r% n/ H+ ~0 n  E4 m* z
140.     }
     3 M) K: Y, M$ a" T; ?/ g
141.     else
     
142.     {
     $ |! Q) x' f  l2 I% Q
143.         return old_data;
     ) C: O7 o9 `2 p7 v
144.     }
     
145. }
     

複製代碼

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

2018-2-13 01:52 上傳

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

! O! y0 U, k( T

強力推廣中

哇...要100 個銅錢

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

5 ^( i# c6 e( q1 |  U( x3 ]認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手