mBot Ranger 高速循線範例＿

Demo Video
1 ~: c( Z4 j4 ]( b

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   # ^/ Z$ ^3 y+ l# R2 ]6 q+ F) i
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   ( g0 W/ s5 x9 Y5 R
6. MeEncoderOnBoard Encoder_2(SLOT2);
   * d! ~" \8 V+ F" u1 R4 K6 J6 w: m
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    : R# M4 c, A- d+ @: b. _
12. #define Error1 1
    
13. #define Error2 2
    2 f' b: w# j" ~  N  V, b6 m% G
14. #define Error3 3
    : \" s8 W3 |7 D# Q  }, y
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    , Z' P; J+ s* \
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    5 I1 O: k7 L# R8 }5 j( x- g
21. uint8_t D1;
      Y' @  i7 t* v9 c+ M+ I, s
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    ( \$ Z$ ]* A- N9 E0 i# a: q/ C
25. uint8_t D5;
    4 g4 c, W2 b2 s: _* Y2 V1 E
26. uint8_t D6;
    . S. N! Y& s9 p! Z  p5 Q
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    
33. 
    / \, f# I8 ^8 W, |
34. byte Left;
    0 v/ w! B& m6 M
35. 
    ! v7 _) |% @% \0 M' B- G; Y
36. void setup()
    ( f9 w: D0 H- g2 L" L7 \: `
37. {
    
38. //Set PWM 8KHz
    & i, q# u$ ^2 S
39.   TCCR1A = _BV(WGM10);
    ; [$ L# U7 `4 _( e* m) T! k
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    8 A4 v/ K) n2 Y+ @1 k- [
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    % p' ^$ T7 ], d/ u
44.   buzzer.setpin(45);
    7 L) l- m9 D: a
45. }
    
46. 
    $ P- [7 I. i) M3 t4 q
47. void loop()
    
48. {
    + r3 T$ R9 W7 z3 c& \0 W
49.   while(!((lightsensor_1.read()) < (10)));
    2 z# U6 s- P, ^0 i1 Q- v
50.   buzzer.tone(1047, 500);
    1 ?/ f! {( ~9 G, |9 `
51.   delay(500);
    
52.   do
      |7 Q/ j8 d% H' ]
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    & q, k" @2 r, i$ F  O& Y
57.      Left = 1;
    ) X+ w8 [5 M; s: x& L
58.    }
    
59.    if(D6 == 1)
    9 T. O" n" A, b3 N5 o* a9 R) R
60.    {
    
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    6 J! S: U' R; ~2 P. Q
64.    {
    ! t* M! s0 H! p6 T: P
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    ( d- S7 g# |$ k3 n9 W1 x
70.        {
    # F! ~3 F& u! H) \) L
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
    ! f) d7 V( X4 e6 o6 a2 w+ ?
74.      else
    , \2 E, L( O& ?2 ?: i
75.      {
    8 d, L/ j/ V" j$ O! c5 i
76.        Speed -= 5;
    8 j% Z5 }9 f, ]" B& r/ M8 d
77.        moto(Speed,0);
    * C) K  `* K: k0 P
78.        do
    
79.        {
    
80.          Err = getErr();
    0 D9 g: D+ i8 f: r
81.        }while((D1+D6) == 0);
    9 E+ J7 s  ^+ b$ s, m
82.      }
    
83.    }
    7 I8 |6 c. @/ @- q
84.    else
    
85.    {
    # {: t* |1 e$ z" y1 a1 G
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    ) [5 p3 D' v$ Y; f4 W# d$ ^
88.      integral = integral + Err;
    5 r8 O5 `0 K% ~6 L
89.      derivative = Err - previous_error;
    ! w7 K' H& h/ f2 F0 Z
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    $ E/ C% Z* C6 ^0 z1 \
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);
    3 N! P" O9 a! T' U2 s/ x
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    ) T* R2 f/ q/ B, ?4 Y
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     5 _7 K, x$ V( h5 C3 [5 p6 o) [$ f
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     ' S1 a9 m* s2 O
108.    D6 = ~(sData>>5)&1;
     6 O! v  N  f) ^1 K
109.    int downD = D1+D2+D3+D4+D5+D6;
     - F; @1 j! b" N4 T4 [
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     , w, I- }9 C' K
112.    {
     ( t4 `% F: M" [; L! L' L) a
113.      return 99;
     $ r7 x! \. `- s) y3 H( b, i
114.    }
     % e3 P  e& Z  s1 Q* V/ p) y9 A
115.    else
     
116.    {
     5 C! H- ^% E$ |
117.     return upD/downD;
     8 w  R& z+ O0 M: a# w- m, `  a8 {
118.    }
     
119. }
     
120. 
     ' i8 P, ~- Q9 X0 E% I
121. void moto(int Speed_L,int Speed_R)
     
122. {
     ! n: ^) M! z4 C6 L, I5 ]
123.   Encoder_2.setMotorPwm(Speed_L);
     3 r5 {. Z" x  d3 q4 Q$ @4 u( Z
124.   Encoder_1.setMotorPwm(-Speed_R);
     2 z1 e  P5 t: n! d+ q! k
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   . C/ F- D. y* W9 t" e& l4 I
2. 
   . k+ j3 C# ~! O) @' n+ u$ p" o% a6 E
3. #ifdef ME_PORT_DEFINED
   & {0 n! h# I' a8 X! [
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   ' Q6 b5 a. d8 u0 [2 J/ Z  @
5. {
   7 ?: J- O7 t$ B$ i
6. 
   3 p8 F/ A0 f; @5 c- w
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    + t2 d( `; o2 U7 y$ _
11.     pinMode(_DataPin, OUTPUT);
    . f0 ?7 v( [9 f
12.     digitalWrite(_DataPin, HIGH);
    4 j- v2 P- a+ @2 M2 g1 r& z
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    / @& O* m9 V: _, y
16. {
    
17.     _DataPin = pin;
    9 }7 e* A, s' d" R) _; V% u
18.     pinMode(_DataPin, OUTPUT);
    ; g/ B0 N0 v1 V
19.     digitalWrite(_DataPin, HIGH);
      f/ S' V$ ^/ c) ?
20. }
    0 M- h) p1 T' Q* {7 P
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    * O& O# b9 N. N$ h5 H( Y
24. void MeLineFollowerArray::setpin(uint8_t pin)
    3 M& R" _0 D& {( ^
25. {
    
26.     _DataPin = pin;
    " l9 `: p1 i( P9 a) j) E
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    & [3 y- @" n( g4 R0 v
30.     #ifdef ME_PORT_DEFINED
    6 G. B* ?1 C0 _1 U( Z6 z
31.     s2 = pin;
    
32.     #endif
    
33. }
    % z  I) O7 D; _/ H" x  x7 G" n
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    1 {; K" z( w0 m; B) ^: h
36. {
    3 s& i; E( W: S% i
37.     uint32_t LOW_level_read_time;
    ( D9 Q& f! P' I$ Z) G
38.     uint32_t HIGH_level_read_time;
    2 M& X5 U3 Q$ ~* }% Y! m# }
39.     uint32_t time_out_flag;
    $ Y0 g0 o, W; s" V6 t4 R0 V
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    2 ]( n' h7 u6 x
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    , B, U* H% I( P
44.     digitalWrite(_DataPin, LOW);
    & U" Y/ j/ t3 {7 l
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    ! @3 L$ V9 j" P2 |0 s3 n0 r0 M
48.     pinMode(_DataPin, INPUT_PULLUP);
    3 b# _- n" |: ~( k6 e
49.     delayMicroseconds(10);
    
50. 
    ; r2 Z9 w) O2 V2 V+ |  o+ Z
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    $ c1 p/ s1 L2 @: |9 \' o
54.     LOW_level_read_time = micros();
    : X4 }" Z) T, f9 x( V
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    % u# Z+ ]; w  ?7 }$ M' y/ M
56.     {
    
57.         return 0xff;
    + _; u$ s) ]0 H7 T  E
58.     }
    
59. 
    1 I) O# L" M) ?, ]# c4 b
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    * V% ~8 v) t9 \6 ?, a* Z
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    ; W/ [8 V4 {) @, _$ s+ _  I& G/ O4 E
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    6 O3 `4 [" v- c' X
66.     {
    
67.         return 0xff;
    8 o2 m3 h7 Y- J6 w: y
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    , O7 }0 ^* w2 B$ g9 g# {; K
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    8 V4 w2 z0 p' f
76.     LOW_level_read_time  = micros();
    3 F! X  L6 F9 G1 [
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    & e8 Z. Y; Q# d% e& N2 `3 T
78. 
    $ q" U# O5 C  |* _- `9 b" N) t; ]- y
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    0 T" |1 P. w# c2 k+ b6 \
83. 
    - H% C9 I  ]* N. h8 p+ I' F
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    2 t+ S8 S& _+ L8 c% I! ?
85.     {
    4 w0 I& Q9 I# {- h+ k
86.         return 0xff;
    ( R  a0 j% n3 \8 B( ]9 V  _
87.     }
    . \  h' H) I" l1 H0 U7 {0 t
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    . _/ J% s1 S& H7 z+ ~& o: w
94.         {
      o$ m1 M6 B9 Y8 |; {; N
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    7 `9 [! `5 f9 n' U  p4 h
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    & c: [3 u% ]0 e" ?3 M0 m2 t" [
100.             {
     1 R% {: g- g. d
101.                 return 0xff;
     
102.             }
     " I7 ~2 `0 s! s. g' c9 R! v* d/ G
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     # i, A5 X  m/ L+ O9 r
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
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     . P* v( I* C# Q4 j( U2 ~4 B
113.             {
     0 n0 ?4 O% E$ t" N3 O7 i
114.                 return 0xff;
     
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     8 s% K  _* z2 G3 g2 ]0 T
119.                 return 0xff;
     + J5 N; ^6 h: O/ e* J# J
120.             }
     
121.         }
     
122.     }
     
123. 
     
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. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     6 p" i* r( X% S2 P5 X" @0 u, u
129.     {
     
130.         return 0xff;
     
131.     }
     6 c2 Z& Q1 [& a( Z7 F+ f) c0 u! A
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     5 h5 W7 l/ P% b5 t* ?
135. 
     6 F+ u2 w, N  `; k' P+ M
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     ' [1 v: I8 D/ [! X) v3 Y6 c
137.     {
     / U; o* z( z6 T  R; p5 Y" m
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     8 t6 T4 Z4 t0 }- `1 Y% d" B
140.     }
     ; d+ {$ g" ?, `# H6 A
141.     else
     " V9 w- w1 }' c- b. v& w
142.     {
     ( d2 W: D* V! J
143.         return old_data;
     9 ?9 V1 [$ c6 G2 k- B
144.     }
     , r  t0 _' G( `- W! I
145. }
     / P0 q$ z7 ^3 R

複製代碼

5 |3 p) x# X% s/ x# b  H- OMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 0, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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