mBot Ranger 高速循線範例＿

( t; N4 |$ J. r& d+ C+ J; M

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   9 S; @  H2 Q9 \0 }
4. 
   ! z8 v8 Q( Q! N- X6 J. e
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   0 e. ?+ m* z$ w9 R
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    3 k; ]/ N" F3 C- J/ M6 d7 [4 l
13. #define Error2 2
    ( s& U: o1 r6 }
14. #define Error3 3
    
15. 
    9 h6 u% a( T1 z- r2 N4 B& N; s3 X
16. #define Kp 15
    . Z6 K6 u8 m% u7 H; Q+ x. w
17. #define Ki 0.15
    $ e! K! m  P; ?  N+ |6 L
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    $ @+ J, D: u3 Y3 M
22. uint8_t D2;
    
23. uint8_t D3;
    # }% n' J# y+ w0 u
24. uint8_t D4;
    
25. uint8_t D5;
    9 V: ^0 F/ c; ?6 B9 r% O1 O' w
26. uint8_t D6;
    2 o  {7 p0 f  R4 _( U
27. 
    ! k( |* t, e* W& M+ K* r6 ^
28. float previous_error = 0;
    
29. float integral = 0;
    ( q5 t0 n4 l2 w$ p
30. float derivative = 0;
    0 }* \7 R7 k2 W
31. int  Speed  = 160;
    
32. float output;
    
33. 
    * W# |/ X/ I" |2 G
34. byte Left;
    5 X4 i# c5 c1 y. e% |( \, J
35. 
    
36. void setup()
    
37. {
    4 B8 V3 }1 g. k4 a3 X2 Z
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    - s6 Z: a9 _# k) x7 n/ O  {1 L# L
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    6 M( z$ r, h7 Q  m' R) Y, W+ c
42.   TCCR2B = _BV(CS21);
    $ M2 i* G9 x  g2 K1 {
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    # c" n( r' i6 p% k
46. 
    ) t' c3 {. G4 V# [  c, d8 b; b
47. void loop()
    6 i( l9 m0 f0 }4 H& h3 w
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    , P1 Z! `2 a9 W3 n, [! x/ q3 N
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    6 V/ r3 n& w# {( F% P1 J% C7 c: J) {2 q
52.   do
    
53.   {
    
54.    int Err = getErr();
    1 r5 Z0 y' v- L0 Y0 h9 i# c4 Y
55.    if(D1 == 1)
    ; u% p- N1 C/ ]0 B9 ?1 U* |
56.    {
    ' Y& I. Y1 _2 ^( t  |
57.      Left = 1;
    . e& _$ d, b" O1 C  R5 K
58.    }
    7 B$ n7 m$ ?5 Y1 H5 J: Z
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    . s% _! d. k8 t# Y# c# s
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    . z9 X5 Q  z4 O+ |. }' c. }
66.      {
    # }+ [, s: U: I6 _- V$ ~# u# i) I: y
67.        Speed -= 5;
    - e- c5 G( o) q, ^( D3 l! q
68.        moto(0,Speed);
    
69.        do
    * i9 C( ]$ N! T# ?2 t% e6 [
70.        {
    ' d9 Y$ g- Z0 h8 g5 p& _
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    8 }7 j) @5 q+ ^+ U5 v% U. N
73.      }
    
74.      else
    + E" f5 {+ V1 v7 m/ _4 s% i
75.      {
    $ u/ X" ]8 D) b" _) _3 J
76.        Speed -= 5;
    + C4 B- V- t! A4 H8 V7 N  m
77.        moto(Speed,0);
      T2 Y3 o, M; p. N% W$ J0 x
78.        do
    # Q9 g! q; V5 C3 I: [9 z9 \
79.        {
    
80.          Err = getErr();
    2 X$ f7 x3 z. }4 ^$ N' j
81.        }while((D1+D6) == 0);
    
82.      }
    2 p. Q1 _) Z+ N7 Z2 T  {% O
83.    }
    , k2 g8 S7 n0 q  F0 Y. A$ K
84.    else
    
85.    {
    3 T, D/ p. c: O5 U4 B, F$ x
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ' i) d- o" ~0 z; M
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;
    $ I. D( M& `1 N& c
91.      moto(int(Speed-output),int(Speed+output));
    . J3 `3 n+ ?7 H; k, @# g
92.      previous_error = Err;
      T% m- p) [0 i! c$ p+ a' c3 i
93.    }
    * i  w" s( Q$ w) n, p6 H+ Z
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    3 D# ~4 y1 V  t  ?" ]$ H
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    : F+ x$ S; U$ g" Y
100. int getErr()
     # J! r: s, K4 e  f- t
101. {  
     
102.    sData = linefollower.getValue();
     0 R& k* Q* G+ x% Q% r* v& v! c) I. k
103.    D1 = ~(sData>>0)&1;
     3 C8 g# {1 o2 u0 x- q9 l
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     9 A* B# ^" c8 C( l! c+ n4 ^; }
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)
     & N7 J+ g. W* j! U
112.    {
     & w7 v* W" X1 b9 @
113.      return 99;
     : H$ Q  N, K! |% ^) c; ?6 |1 W0 B
114.    }
     0 O/ p/ r" U; i: N# A' J) }/ ^5 S/ Q
115.    else
     2 p- G1 N6 n8 Z
116.    {
     
117.     return upD/downD;
     1 C- u+ X5 E4 v, ]' }9 O
118.    }
     & \8 I- u! a+ y
119. }
       n' f7 g) q. ]! b$ H
120. 
     " I- ^+ E" @0 R$ H1 J
121. void moto(int Speed_L,int Speed_R)
     4 M& R1 L  q( I  n6 I' n0 @
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     ' f4 ]) f8 \5 K* m; w5 n/ r& Q
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   4 K# M3 u0 S6 ]  `4 t
6. 
   
7. }
   7 _8 Z' t; `7 y7 L% \) r  D8 \! D
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   $ z* b! ]0 L/ D/ H1 e; O
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    ; ^: q! G1 D$ q
13. }
    # o$ S" V9 G+ n  L
14. #else // ME_PORT_DEFINED
    9 \' t, d( a5 t% l% {; ]% K, F$ S' M
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    ; l% H% W4 m2 |7 q* x. n5 w8 }4 c
16. {
    
17.     _DataPin = pin;
    5 M' Q3 z4 k: P( l
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    ' o( z) u8 {$ y( _9 c
24. void MeLineFollowerArray::setpin(uint8_t pin)
    4 C) n4 D6 j1 X0 h- q* h% c6 v: h
25. {
    
26.     _DataPin = pin;
    $ \* M4 o. H, G/ i4 e
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    / |+ {" R" P9 |# M) j
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    ! u6 u- S7 H! g1 ~2 c, A( U/ o
33. }
    
34. 
    & y/ ?4 U- D% x# h6 l  E7 x
35. uint8_t MeLineFollowerArray::getValue()
    3 w2 }) p/ K0 e4 E7 t5 @
36. {
    3 o) J! i/ w5 l
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
      n4 B  e; l+ U4 `
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    ' F% G" I+ i  v- y
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    . Q3 o1 N, }. b, k
46.     digitalWrite(_DataPin, HIGH);
    1 n" `2 Q- a7 B1 u( {
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    9 `8 {/ D  W% V, Y/ K
50. 
    * `! z$ L7 y& Z# P: X3 M. U
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    # f/ ]$ N, K" ~. n8 J3 |
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    . ?5 O/ B' ?* l$ C2 j# f) K
59. 
    8 s% O2 w8 z2 x* \2 m0 E
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    / M5 i4 Q: n+ {+ z' y: F
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    9 t$ y1 s( @& ~& t* Y5 T. Z% m) `
64. 
    9 ~( W( A/ v5 f0 N2 r
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    8 p: `  H5 ^  D) X3 u, K  J( [2 K! }
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    7 U3 U: Q& Z. H
71.     {
    
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    $ X- m* E3 O7 ~% g/ g
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    , `% Q& u) C6 J0 {, ?2 ~
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    7 \+ c& A- ?7 m0 R
81.         return 0xff;
    
82.     }
    
83. 
    " x5 u: a- @$ i- Q% d
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    
87.     }
    , I, G1 B/ L2 X, m
88. 
    8 T7 |, m1 O9 g$ X
89.     for(uint8_t k=0; k<3; k++)
    8 b7 C8 o9 l- l2 R
90.     {
    7 D" p+ I& ]. b" H# l5 v8 [* `
91.         Sensor_Data[k] = 0x00;
    4 f+ h4 V" j3 p* {! B# X+ B
92. 
    ; X# V; \" r* _: G0 L
93.         for(uint8_t i=0;i<8;i++)
    0 D/ j9 L2 B9 U% X7 V
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;
    
98. 
    $ d1 o5 p  l6 I0 y. G3 Z+ B
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    7 f$ z4 q3 c- m1 s9 Z; \8 O8 A
100.             {
     " \9 a7 T# g& v. q7 Z
101.                 return 0xff;
     
102.             }
     
103. 
     9 ^1 Y2 }1 g, I$ {, W
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. 
     ; u% z6 ~7 E( R
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     8 u% A# W8 H/ I+ T& w% {
111.             }
     , c4 Y/ k5 m+ r9 k% M* Y/ ?
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     & [; a* i& T9 Z6 L2 ]# z9 u; Z
120.             }
     % Y- @1 z- w* [" |
121.         }
     : }3 [8 N  I- c- O
122.     }
     ! K: r. S* o1 ^
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) )
     $ c" _1 U7 h7 l& p) X4 F9 `0 i5 L
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     3 @2 Q( y0 ]9 \0 y5 W+ l
135. 
     / D9 S" I0 S- R' P$ H$ y
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     2 J0 v: @# Q% W1 C: H
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     7 n- e. a. t8 Y0 D6 K+ K% f
142.     {
     
143.         return old_data;
     
144.     }
     7 C" |9 m" I1 t" V* ]8 T
145. }
     ! Y% r  `$ \( P' y. Y" V9 n- d* K

複製代碼

, E2 P  S- E7 W3 G: i: L- tMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 9, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

5 D) u/ R8 Y& N7 ?6 H2 c認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks