mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   % m* B& A" {2 B' l! F5 N' L8 h8 {
2. #include <MeAuriga.h>
   4 r: x0 j1 O/ P
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   * `3 X' B: U5 _, e# a( U; M
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ' H2 [7 M, H/ ~0 Q: V
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   7 ~" R+ E8 e3 }5 K3 A3 ]
9. MeBuzzer buzzer;
   $ s8 o0 H9 w2 S& X- z
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    7 h4 q& K6 |. ]2 i$ O& z
13. #define Error2 2
    6 S" G+ |% g6 m9 E& `' S7 L" c7 W
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    - c' \$ d* Q1 x7 b7 _
17. #define Ki 0.15
    
18. #define Kd 0.03
    7 z  y7 N! W6 S4 n+ S) v
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    , I8 Y7 j; r# M3 l- s. _" `$ J) e
23. uint8_t D3;
    ( ]8 d" y5 C+ ?
24. uint8_t D4;
    + V% x/ a) y; U) C3 Z% C9 u
25. uint8_t D5;
    ; f8 |# N2 i# ?4 N3 ]% x3 U! p
26. uint8_t D6;
    
27. 
    * R- e; |& u. |+ c( v4 p2 x
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    0 q, a+ G! v9 P5 L3 q
33. 
    : ?4 t* q% j( M) u/ R
34. byte Left;
    
35. 
    
36. void setup()
    ( h% B/ e# {( \0 F9 w
37. {
    
38. //Set PWM 8KHz
    ! ^/ [$ G% G4 p6 R
39.   TCCR1A = _BV(WGM10);
    + i+ Y  X( J# G8 t! k
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    $ ]% h% i) e$ p- C, U1 I
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    , A  c8 [9 ~) O) _1 |, `
45. }
    
46. 
    
47. void loop()
    
48. {
    ( K/ ^  E0 f$ F
49.   while(!((lightsensor_1.read()) < (10)));
    ; P. q& O- L9 c6 k: ~8 m
50.   buzzer.tone(1047, 500);
    3 D1 l9 ~( ]' R- y6 ~3 Q# h
51.   delay(500);
    6 }5 o" u. L2 x% Q
52.   do
    
53.   {
    % R8 Z1 `* `  o/ v6 Y/ [& y
54.    int Err = getErr();
    
55.    if(D1 == 1)
    # f8 M9 Z9 i$ O" P8 G3 M
56.    {
    # N/ \2 \% o9 E- H, g* Z8 |8 h
57.      Left = 1;
    
58.    }
    & l) H7 t0 H$ \: c& ~5 x
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    & n9 Z' g2 ]9 ?# |" s" k
62.    }
    
63.    if(Err == 99)
    0 e# g# y0 }! q# A
64.    {
    / u& O3 t. S1 [4 G2 v4 X
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    ! ~+ Y* ~0 @# T7 w+ C# d2 x
68.        moto(0,Speed);
    
69.        do
    
70.        {
    
71.          Err = getErr();
    $ ^/ X. \. {- B, ]) H- z5 h* u
72.        }while((D1+D6) == 0);
    7 `6 T4 u3 u6 @6 ^
73.      }
    
74.      else
    . m- a& p9 E+ q* w' k
75.      {
    
76.        Speed -= 5;
    9 g# n8 }9 X; s# u9 w6 e
77.        moto(Speed,0);
    
78.        do
    . b- h( `  P- X- p. M* Y3 e9 b
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    6 i7 U: |6 S! t* @
82.      }
    
83.    }
    
84.    else
    
85.    {
    9 k* H0 P! w0 h4 e9 i* j
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ' I& s5 B+ u- P
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    ) [  i/ F) O* ~6 D% C3 p0 W
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    7 n5 ?6 k* z8 y- u3 Y
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    * v9 Z  p% O# j2 s! |2 C. B
95.   moto(0,0);
    + w' c2 R( V3 d2 H
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    5 V$ d1 V" b9 V. ~, D, T
98. }
    - o% m' y) x7 N  r7 K' z
99. 
    * ^7 {4 D% @9 \1 Y: B' z
100. int getErr()
     ! Q- q- x, C# e* x7 ^8 C; G8 n; Q
101. {  
     % ?6 `4 D) o6 w: I
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     5 F  X% U" g# [
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     $ ?' T5 c2 L8 o9 }( O* k
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     8 l3 X' D) ?- T: j1 D
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)
     
112.    {
     / \0 \7 m# e, g1 K7 r
113.      return 99;
     ) t' ?( H5 _0 d
114.    }
     7 O  P) F# X: e/ }: b  T& }! n. P
115.    else
     
116.    {
     ( m8 S9 M* a5 o9 F: m
117.     return upD/downD;
     - N  D5 t3 w8 L
118.    }
     / D6 a; _7 s/ \' p
119. }
     0 f+ _3 p" J  a8 l
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     ; {- T. y# o$ f. a  \
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     % k( Q, X* T0 g; E/ z; R9 N  K2 W
125. }

複製代碼

7 D, u0 D( s6 K' \: q9 a4 c0 NMeLineFollowerArray.cpp
4 w$ x% h( ^9 I, s) u8 V% W0 y

1. #include "MeLineFollowerArray.h"
   2 f: i4 |6 o; D$ r5 K5 S2 f
2. 
   
3. #ifdef ME_PORT_DEFINED
   9 D. u3 U1 m9 o/ N9 @2 E4 s
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   + S5 @. U) o, `  J* B) E
6. 
     C/ J+ H. n7 _5 i
7. }
   ( T/ q! H' z# i8 w3 S/ R
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   ) W* d/ k2 G5 [& \8 q
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    . t) l: r2 Q1 d& j; T
12.     digitalWrite(_DataPin, HIGH);
    $ g/ c1 s" O1 V) \- T" g  H9 F
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    * u# s7 `& J- f
16. {
    . b# ]. n" }$ D( x
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    3 @% }. m  t, F1 x# K- R) r
19.     digitalWrite(_DataPin, HIGH);
    ) w: b! l9 X$ Y" x, F7 ?, {7 z
20. }
    
21. #endif // ME_PORT_DEFINED
    - K. q  I/ z1 ^+ Z' N  V0 x3 R
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    # a  E# t/ B) b9 e2 I5 ]
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    , [* E$ _2 J1 G1 n
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    . }* A$ C# p4 T/ X, H
31.     s2 = pin;
    
32.     #endif
    ' w+ w0 T0 k8 Y* y/ l% t$ z
33. }
    ( `# v: d' P$ n
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
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];
    % o4 J+ z3 @" P* D% v5 N4 a% O
41.     static uint8_t old_data = 0xff;
    8 D' F1 y6 S* ], t- J
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    5 p" W9 m2 ~. `: F' O
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    ) ^1 c( u# N/ p: H) @9 d, H; |
49.     delayMicroseconds(10);
    
50. 
    ; a  n( [5 C) r. I$ S2 ^* z, T
51.     time_out_flag = millis();
    1 `2 T8 U# P; @" o
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    9 e+ x4 j7 |9 f+ e9 o
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    % I+ |3 ]- R2 E' E( v
56.     {
    
57.         return 0xff;
    4 {5 Q) k, P; c- X2 k
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    : Z* Q1 l8 u' ^8 q; ~3 P: s6 r$ m
61. 
    
62.     HIGH_level_read_time = micros();
    ; Z4 o+ u  j0 ?$ B* ^/ R
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    , }( [& w+ w6 w6 E% j6 S
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    8 ~! u9 I0 L( T! l: X$ l+ i
66.     {
    
67.         return 0xff;
    
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    : v& ]2 n. z2 n
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ; F( R( J9 w7 Q% i6 @8 U
76.     LOW_level_read_time  = micros();
    $ O) I2 W$ V, Q' ^3 u0 O" r+ V
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    " Z7 o9 w4 o1 k$ b; }  V! ^7 f1 V! J8 \
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    " C' a5 D2 l1 [' y; a
81.         return 0xff;
    4 Z. \& I' n$ I- a
82.     }
    
83. 
    6 \& J8 I3 ]; I* }! |
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    # S6 u4 F2 ?  W1 b: x2 w
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    2 X# ^  e( t* L3 X6 V7 B( o
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    & P0 }3 q7 Z& ?, w  \
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    4 g. a6 n  j$ T6 ?! l
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     9 e" p5 y4 z, Y, [/ S
102.             }
     
103. 
     ( m3 K! [/ y: Q
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     4 s. e# S) b7 Q
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
     . H1 v+ p! s) g
109.             {
     , x/ q# X7 h1 C4 q3 @) {' o2 O
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     : G, F* J6 z7 s: @" W( O9 w& h
114.                 return 0xff;
     
115.             }
     
116. 
     3 ?1 Z$ H# }  v6 n2 G
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     % l% C- \, b0 `8 |
118.             {
     
119.                 return 0xff;
     
120.             }
     
121.         }
     
122.     }
     
123. 
     $ |4 g9 a" G6 v( e" A
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. 
     9 K7 e/ b7 D# ?
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     % ~# x  Y5 R( f0 w% A/ Y
130.         return 0xff;
     
131.     }
     5 e  n3 V7 A, b5 P7 `) H
132. 
     ' _% v. [4 V# @5 q, G
133.     pinMode(_DataPin, OUTPUT);
     % a7 `8 J( J  {" V) c
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     ! Q  P# a  o% x2 @  f; R) T
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     * J0 i3 e/ m2 G1 U# L) O7 c- M
138.         old_data = Sensor_Data[0];
     4 A6 q) i; }0 q( K
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     % n' n  I% y7 a$ i- x
143.         return old_data;
     3 f  M( z' Z+ {* S/ }* P6 v
144.     }
     
145. }
     

複製代碼

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

2018-2-13 01:52 上傳

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

哇...要100 個銅錢

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

認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks