mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   ) E( z% I4 q5 L7 g7 N
4. 
   . l3 Z  Q3 h- s% F4 B# K
5. MeEncoderOnBoard Encoder_1(SLOT1);
   / A# G0 O5 {0 w$ e$ }; @
6. MeEncoderOnBoard Encoder_2(SLOT2);
   : T' S% w: O7 {: J% A6 Z
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   6 q7 y# d) C8 l) E/ H- c3 ]0 F
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    " O# x9 R- w0 I9 x5 L
12. #define Error1 1
    0 ]  _4 e. K. v) \( G
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    1 H! k7 u4 z; a! ^8 s
16. #define Kp 15
    ) Y2 L- ]% O7 X
17. #define Ki 0.15
    8 V7 P. m6 e& }5 p6 S1 Z
18. #define Kd 0.03
    * O; [0 b0 \# A
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    : v& n; k) g9 Z. T5 Q& S8 b+ v+ G. u
22. uint8_t D2;
    " F5 N% V8 q& h! ?
23. uint8_t D3;
    
24. uint8_t D4;
    / v- P, S" v) @& b
25. uint8_t D5;
    7 M. }* x+ B& I: ?
26. uint8_t D6;
    
27. 
    ' V* A* N& ~+ s$ q! v
28. float previous_error = 0;
    
29. float integral = 0;
    ! `& G: C* q7 g6 u) e9 i6 G7 H8 b
30. float derivative = 0;
    
31. int  Speed  = 160;
      j# M& e3 ^1 E+ K5 `. {0 u
32. float output;
    
33. 
    ) n$ k5 H( X$ e* ]% Q5 H: U  G
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    , Q2 v* _% v5 t& \  X; ]5 W
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    ( m1 n) P( d4 L/ z
42.   TCCR2B = _BV(CS21);
    4 `1 |! a: e2 B; w$ x
43.   Serial.begin(9600);
      V3 k! b6 H5 b3 t
44.   buzzer.setpin(45);
      w; Y$ e' }7 P$ l+ `
45. }
    
46. 
    % n0 g5 O( m& L" z1 }
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    3 ]% B  a" l& b/ D6 E! x
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    
54.    int Err = getErr();
    1 f3 d$ e5 E! v1 u
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    0 g9 l4 f  |( r' n4 ^/ d* o
62.    }
    7 ]' U& d" ~4 o
63.    if(Err == 99)
    & h% d& \; m% X/ C, V$ E
64.    {
    7 P; Q; H+ u0 Q% C- ^% V1 f  T
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    ! W  c  x: U7 L3 ?# ~2 X6 D
68.        moto(0,Speed);
    
69.        do
    : |% \- u4 S* s% g" p" C0 E
70.        {
    
71.          Err = getErr();
    0 _: Z1 @5 y/ h; _1 M* I- [( f
72.        }while((D1+D6) == 0);
    
73.      }
    5 k* y% O. P* G4 S( ^  D0 m
74.      else
    : j& |3 g* k# {. A6 m: H
75.      {
    6 A- E) Z+ K. b' H' }
76.        Speed -= 5;
    
77.        moto(Speed,0);
    5 s' n- X. ~! }" J5 w2 s5 c
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    ; T9 w1 X$ o% [1 R- E1 T
82.      }
    + V+ z8 V2 q& t! A: c
83.    }
    
84.    else
    : b5 P8 `- M: f
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    9 L" J% _7 q- z5 d: ~* Z- \7 f
91.      moto(int(Speed-output),int(Speed+output));
    5 n+ C7 W) R$ t" I$ ^3 ^8 q
92.      previous_error = Err;
    * |! C7 A# D6 f6 G) o% Z
93.    }
    8 }# g- F* s4 A; `: q. z
94.   }while(!((lightsensor_2.read()) < (10)));
    ) o' K. D7 ^2 Q) X7 E* }
95.   moto(0,0);
    : F- t% r0 r' E& E
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    # b$ ^9 D, p! p% U& I
98. }
    - `5 E% Z% d7 z1 Q* W' Z
99. 
    ; r& z  P3 z# H
100. int getErr()
     
101. {  
     , B: f& B) u& E4 f- f4 U1 @
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     / P* ~9 h- P$ Y2 a, F
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     / [( F  x" {1 x/ C* b5 K" U
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)
     % r/ y6 V* Y, F3 C& Q- _% {
112.    {
     
113.      return 99;
     " Z3 `3 h2 b+ \# A6 N: a( Q
114.    }
     8 {) O: L3 {8 ?; v
115.    else
     $ N/ x% _( r6 Q
116.    {
     
117.     return upD/downD;
     
118.    }
     
119. }
     ) M) [& T% E$ ^3 H2 u! P4 x
120. 
     
121. void moto(int Speed_L,int Speed_R)
     1 S2 [+ o# ~! T; ?
122. {
     # ?- d( U$ ?6 c$ _& L! O. s- y* o
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     ' x4 z' a! @+ s# _
125. }

複製代碼

MeLineFollowerArray.cpp
  f8 w4 Z3 e8 l

1. #include "MeLineFollowerArray.h"
   * B; i. a: i) O
2. 
   6 v* h' j" k, l/ f# T# p% N4 O# \
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   / I" B- e8 k+ {) e1 b
6. 
   0 t& I; c( `3 Y; e3 V/ w8 z
7. }
   5 z4 Z! A" m' w3 U& {" i
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   : s. R& H+ ]# e( r9 ?8 ?- L" r
9. {
     e, K; x' ~4 b; T" o7 X
10.     _DataPin = mePort[port].s2;
    6 r+ ?" S1 }! Y, r9 L0 H% P
11.     pinMode(_DataPin, OUTPUT);
    2 R8 H1 k$ c. }& Q& b2 S/ l2 k
12.     digitalWrite(_DataPin, HIGH);
    # _3 E! M9 }& B5 J7 G! \3 b
13. }
    
14. #else // ME_PORT_DEFINED
    + }- |7 C7 r0 K8 m
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    : |( w% \& r. z
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    9 v# d0 x$ a/ _! K7 b7 \' R' H
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    : T; |- \+ V; G  P: V* i" ]0 J
21. #endif // ME_PORT_DEFINED
    
22. 
    9 Q: t" b; p3 A* Q: V) y
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    * b: S: b9 U6 m) u( l# i
29. 
    9 h' \: ?# l9 I- j( Z
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    % K0 X6 ^& z* ?8 S1 j* p
33. }
    & }' W3 E5 ?" f
34. 
    & q/ f, t6 y; ^& K9 t9 _
35. uint8_t MeLineFollowerArray::getValue()
    * y( a4 U% T6 h
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];
    $ C. d5 n2 a- U$ d/ F
41.     static uint8_t old_data = 0xff;
    " {6 {; R- D  F3 \1 m: a* d
42. 
    4 M" M( C, ^3 C) c( q, P4 Z
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    6 j5 X1 }+ o6 _" Q& u6 C) u
45.     delayMicroseconds(980);
    2 Y) Q8 n9 }. q0 a/ Q
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    . [: ]* B+ k) H
49.     delayMicroseconds(10);
    8 W) r3 S- }$ x' q9 v/ Z
50. 
    
51.     time_out_flag = millis();
    : a9 n& J6 t9 Z' w, s
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ! H3 d$ E) H$ Q* q3 v1 C
53. 
    9 T9 ^, p, J9 _$ z
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    
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
    * X) H0 J' _* @2 `, Q" ~
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ; n: p) v* b+ \# B6 j
66.     {
    : \- @+ R; K% d7 D) n* Q
67.         return 0xff;
    
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    2 |) i3 @; N, c3 a
71.     {
    
72.         return 0xff;
    
73.     }
    
74. 
    : e! p" S' A3 B8 O# \$ 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
    : E* v: i: e. [2 {! L) I
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    3 B9 l" {4 h& }- y# p. W2 D( Q% K
81.         return 0xff;
    + ~$ I; C9 T# x8 [; A
82.     }
    : J8 C' c( T- S6 Q0 A; Z+ ~# r
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    & y1 G# f* b5 c' U9 z
85.     {
    , _- o3 c. B0 R( q. Y
86.         return 0xff;
    ' R+ F2 l+ d7 ~9 c  v2 \/ @
87.     }
    1 }1 Z: i/ X9 l( p9 x6 R- t
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    + o/ E5 k6 Q/ L( M+ T' g- E: k
91.         Sensor_Data[k] = 0x00;
    ' ^, M2 G, E( Y( @$ l/ z2 K
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    3 d  n8 W. q1 r2 v% a- c" X# n
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    1 K. A9 e8 D6 n1 [6 C% J
96.             HIGH_level_read_time = micros();
    ) F9 w+ ?6 i& _5 c5 d
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    ; |6 v) F0 Z. f" N
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    * b+ {( G- J$ L$ h$ l
100.             {
     ! V$ U6 z3 D% S' x4 ]
101.                 return 0xff;
     
102.             }
     
103. 
     * I, u' t0 P9 `' _2 p$ q& Y! q
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
       P* x" \8 `4 j" M7 M
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     $ _' G/ S8 d* n) M# c4 i3 t
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     0 e( O- ?& I& ]8 h: S
112.             else if(HIGH_level_read_time >= 100)
     * S+ F* h0 `  B
113.             {
     & c+ a' N- M  x. k- z
114.                 return 0xff;
     ) Y/ }( s4 `5 M- K
115.             }
     
116. 
     4 U  Y" F$ |* A" n; L; E
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     " b4 W: {% d( Q
118.             {
     
119.                 return 0xff;
     
120.             }
     
121.         }
     
122.     }
     $ K) a3 a! E! s  [' n  k( S0 R) u4 e
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) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     3 R: l) _7 v7 G0 \" q$ w, B
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     ) G# E. a( @, X! S4 S% T0 r" f
134.     digitalWrite(_DataPin, HIGH);
     + G" w/ [; x8 L2 J# @$ A, ]/ \0 V0 \
135. 
     ( S+ J+ c. ~& d" Z. D
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];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     
143.         return old_data;
     & ^. E4 J, g6 n- `+ n) q+ d6 D* ~4 v2 z+ F/ \
144.     }
     " X. ]; Q6 W# f7 U
145. }
     9 `) e4 m0 k- [- J% X

複製代碼

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

2018-2-13 01:52 上傳

點擊文件名下載附件
閱讀權限: 20
! w  J0 o8 |7 d4 A/ ~5 a4 [" n! m