mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   . v. |! U& M, J/ ]( ^& i( x2 A3 ~
2. #include <MeAuriga.h>
   ' x  J( w1 _0 K! |) Q2 G8 Q
3. #include "MeLineFollowerArray.h"
   5 z9 m6 U1 F+ l1 S$ J' r. Y' Y/ ]
4. 
   5 l+ v/ o" T6 a, Y
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   . E6 S) ~: j7 Q2 Q0 z3 B
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    : X1 f! z+ |. ?) H+ p1 R7 R
11. 
    " z0 |$ }# q7 G6 `
12. #define Error1 1
    
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    / ~! j1 j7 `& N
16. #define Kp 15
    . L8 j  c1 _" z) ]: v
17. #define Ki 0.15
    
18. #define Kd 0.03
    2 Q4 o9 a: p, j, W: s
19. 
    ! g+ p- G) z; A0 ^2 S
20. uint8_t sData;
    ' {, Q3 z* i/ Q3 X
21. uint8_t D1;
    
22. uint8_t D2;
    , I1 |% h3 |# T) U
23. uint8_t D3;
    7 k9 H0 K) z# h8 T
24. uint8_t D4;
    
25. uint8_t D5;
    ' |7 r! g' i+ J& o" m
26. uint8_t D6;
    
27. 
    / P/ V) P9 S5 X0 v* j( i
28. float previous_error = 0;
    
29. float integral = 0;
      D9 y& _$ ~* h! h+ ?/ K
30. float derivative = 0;
    6 P+ ?1 ~9 C1 F! C1 z, I
31. int  Speed  = 160;
    
32. float output;
    
33. 
    0 c' {3 c5 }' m# Q1 _( Y0 G
34. byte Left;
    
35. 
    
36. void setup()
    ! s, ]0 v# r$ K/ K: y
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    " e/ q( R5 N% b- e1 s8 S3 i: [
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    0 g# x9 Z1 R( w
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    & ^# E5 y/ ~$ V& r5 Y8 y0 r$ K* _
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    
48. {
    4 F& h: p4 U* s, M) R
49.   while(!((lightsensor_1.read()) < (10)));
    8 X; g6 ?" q3 |# y9 ~
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
      }6 t+ M; h$ ^
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    0 q$ L1 ]; S5 K6 Y, h
56.    {
    * \( d" ^5 C/ j! x
57.      Left = 1;
    ! O( v" t: f, R5 o9 y) x
58.    }
    3 [) t: r3 x2 d7 ^# ?6 r" K% n
59.    if(D6 == 1)
    
60.    {
    9 y' z' u7 t8 }# U& j5 R& F8 W
61.      Left = 0;
    ! m' Q. t( ~  K' Z2 k2 V$ l
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    ) {% B$ t9 \1 z- l. I  C  z# Y8 U
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    * g9 d4 S. u) c
70.        {
    
71.          Err = getErr();
    ' L6 A3 ?- R+ r
72.        }while((D1+D6) == 0);
    
73.      }
    . W0 r: J6 P6 |
74.      else
    : F% _3 d! T& c
75.      {
    
76.        Speed -= 5;
    ) [6 j' Q, U) {6 _( Y+ u
77.        moto(Speed,0);
    
78.        do
    
79.        {
    ! s  n) _$ b+ j! `2 N" M. _
80.          Err = getErr();
      H2 p! z& T8 z/ z2 O( C: e* S! q
81.        }while((D1+D6) == 0);
    
82.      }
    4 O: f0 W" r+ m! e
83.    }
    . j) r* {* v! c. g
84.    else
      N! I/ o5 _, v8 T
85.    {
    % a/ P$ F8 M) q1 y3 ~
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    + _; p7 e6 }, |7 G& H& ~# u
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    ) G2 S# n/ F( d  q
88.      integral = integral + Err;
    * s" h% A# n% n& n. G, j
89.      derivative = Err - previous_error;
    4 ^' X5 k9 J. ~: W/ b: Y! P3 d' U
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    ; c! Q+ \/ x* U- k( h
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    9 p! |5 C& h) K
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
      |8 H1 l* @7 i) r8 n4 H
95.   moto(0,0);
    8 H2 L" y4 z: t6 F: u
96.   delay(500);
    9 j4 b, o& p8 Q3 q6 h
97.   buzzer.tone(262, 500);
    
98. }
      R5 u  G+ y% }1 O! O) C
99. 
    
100. int getErr()
     
101. {  
     ) [  a4 r' o0 ^9 T6 F) @* x
102.    sData = linefollower.getValue();
     9 s6 S  H- ?6 E' g$ D9 T+ H- N
103.    D1 = ~(sData>>0)&1;
     , @3 E7 ~* X, n( `+ d3 A
104.    D2 = ~(sData>>1)&1;
     9 X, [; d1 C8 S) e% _& T
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     ' W9 G3 z  k: d1 W9 t. `
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);
       q* r' A: C) N  Z  I% u
111.    if(downD == 0)
     
112.    {
     ! M: V. V9 ]! ~% z; o( M
113.      return 99;
     % N% M1 |- D0 H$ c- P
114.    }
     
115.    else
     . p) `  r$ p* x5 |7 z
116.    {
     
117.     return upD/downD;
     * A* w7 J3 y& E* w  B3 v; w9 B2 a
118.    }
     
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     9 n; v! `  c- f* r; A! U
122. {
     * U. W" Z4 S* M" F3 Z! ]% r
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     * ?* k8 [) s4 j" K$ C) m$ O5 O
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   % f6 F3 y! \  w4 r3 n
5. {
   
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
     J. c- E  D1 J0 e: ?
9. {
   
10.     _DataPin = mePort[port].s2;
    3 T7 z; L8 a; p  C( I: w- m/ T
11.     pinMode(_DataPin, OUTPUT);
    2 {. A0 w, E7 [/ g6 R
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    9 z8 B, Q% B+ a
14. #else // ME_PORT_DEFINED
    6 U6 {- `; s5 Z+ o
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    2 O5 s0 E1 h5 N, C. L3 q" h
16. {
    
17.     _DataPin = pin;
    ( t) v4 S% j! Y+ F# e8 e9 M1 L4 M
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    & q# s, h( n" V% k) g( C* B
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    $ ~* E) u# K, X# w
25. {
    ( O/ t. i9 H7 B# q* `  Q
26.     _DataPin = pin;
    : k# m8 Z- w# B! t' q3 e2 ?
27.     pinMode(_DataPin, OUTPUT);
    & R  f0 ~1 Q/ N2 v7 ?# j
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    % C7 J0 z7 m# y' U1 y5 b! h
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    % T) y. M% @2 B: D' r1 v
36. {
    0 Y$ b7 T; a( F  ?% p; ~
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    ; b# A) Y6 u- K$ _/ s
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    ! j7 B% [5 G7 d# ]
44.     digitalWrite(_DataPin, LOW);
    2 D% `- v! z2 }4 m. S* ~- Q  J' Z0 W
45.     delayMicroseconds(980);
    4 ~! l. L" k) {) O3 Q6 x; N
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    + d. x( u# l3 u! b
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    : Q0 m' N* f7 o" x" N1 u
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    7 N' S1 p' O1 V( T5 T8 ^
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    0 ^% C6 v3 v8 ]9 [$ r  f
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    0 n6 X* V: B6 W- b/ B6 A; G7 Y
62.     HIGH_level_read_time = micros();
    9 @5 x3 [& V- H# e9 r# J
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    1 e% I) l8 ]% _; ]  H6 b8 \! y
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    7 j( M: U* t4 e0 n. _
66.     {
    
67.         return 0xff;
    
68.     }
    3 A3 K8 ^6 S) Z; [
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    , A1 f  C/ b' f# v6 a8 i
71.     {
    
72.         return 0xff;
    
73.     }
    
74. 
    . x5 n: ~6 I- Y0 Y1 u' s
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. 
    " c9 ]$ K6 C/ X2 Z
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ) H' W5 p! B( r
80.     {
    1 |6 S6 E( Z$ ?7 W9 \& Q
81.         return 0xff;
    7 ~1 W* b! _9 h1 f
82.     }
    
83. 
    " O5 I. T" s  l1 Q: I  p
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    . @! E* P# ~' q8 o7 ?
85.     {
    + T3 I4 ^3 u' d# x. T& E7 ]
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    6 Z" H3 s+ Z+ O) o" [
90.     {
    
91.         Sensor_Data[k] = 0x00;
    ; T& q: ^, t/ u9 q+ t
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    * ]: V8 D8 E3 K
94.         {
    8 O4 F) @3 @6 T' N0 _
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    ; S. Y% B; l2 Y6 A' S8 s3 h+ p7 u
96.             HIGH_level_read_time = micros();
      t# ?) Y5 c' H5 Y& t* ~: O) Y
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    4 G9 n( D) ~! J* \/ s: P0 a
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    % }. I: O2 o  e& r
100.             {
     " J  T% I/ M* R/ g: d) u1 M, l0 M
101.                 return 0xff;
     : H7 c8 g8 m$ g
102.             }
     
103. 
     + G) \. D! D% V. V2 x
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
     
107. 
     $ T" ~% E4 H& s8 ~
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     9 C* z: g  i( G% N
109.             {
     ' {9 J7 m9 ]# p. R
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     . |4 o2 }& V& h+ G
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
       f, [! O1 ?& ~( W3 p7 _0 t
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     $ D2 X0 P& y: ~3 @/ i# ^
118.             {
     $ Q- r0 k! N% T& n) F$ {
119.                 return 0xff;
     ) H5 z7 K! Z  S- u' z
120.             }
     ( w$ ^/ F# ~4 y' {! d3 x2 Z. {
121.         }
     2 P. ^  x$ {; s5 _1 z0 r
122.     }
     
123. 
     6 s+ u  ~4 s( E4 r# B
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     6 ]' T, l2 S- ^3 U. P, }% h8 g
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     + s5 }* g8 V/ m4 m; \) J! u$ }* Y
129.     {
     % v" i/ k; L5 T2 p, ~) y: u, k8 |
130.         return 0xff;
     6 e4 h- s  n+ W  N# w
131.     }
     
132. 
     & k5 r6 u( g; C2 x8 F. Y; D
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     1 _% b7 S% _' i6 ~+ a
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     " S4 k) \9 ^' \/ V: n. a$ S
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     4 _  n1 ^' w5 J! Q) U
143.         return old_data;
     0 E2 D' V3 a' G; A, }  W* n, `
144.     }
     % T' b- m7 V( |- d& M1 S( x' a
145. }
     6 A) _. b; t& k4 F

複製代碼

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

2018-2-13 01:52 上傳

點擊文件名下載附件
閱讀權限: 20
6 O* l  q# ]( h0 S! X
# b4 q% d: R$ x% _

強力推廣中

哇...要100 個銅錢

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

' U; U" @5 t1 |, u+ {" Z! K9 n認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks