mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   # }2 Y: p4 A9 W: L( p. S3 \
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   * u% T" O( M3 ~% X( M
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   # G8 {/ f1 k' E  i( S! _
6. MeEncoderOnBoard Encoder_2(SLOT2);
   4 k$ a9 Z" u. g2 r' @6 y4 ]
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   . y' E! c* s2 n
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    - |% U4 i4 C1 N2 f: c7 S! Z' t; {0 ^
11. 
    
12. #define Error1 1
    
13. #define Error2 2
    6 z6 q$ o, t, R
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    . u. Z$ ^- W* E' ~: [
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    $ Q0 [/ S1 ?6 }+ n( Y- P1 L
25. uint8_t D5;
    : L/ H8 U. U: d9 w
26. uint8_t D6;
    9 @1 J8 d: ~9 X" F' E, U
27. 
    & z( `" m  p, h' c9 ^8 J
28. float previous_error = 0;
    
29. float integral = 0;
    $ p, A0 F, d) `  Y+ J# b
30. float derivative = 0;
    
31. int  Speed  = 160;
    - y# L7 m3 z! V4 S
32. float output;
    
33. 
    
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    $ K& A" R2 ]+ D) E
38. //Set PWM 8KHz
    2 Z: j# q; ]# |: j9 l; u
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    ! b7 d, t6 h. e4 b. T' h, K* U
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    % I, w- J' M6 O4 y6 {
46. 
    
47. void loop()
    7 N8 T+ o; Y  w2 b7 O# h6 b5 L
48. {
    2 r  P% ?$ C0 k) Z
49.   while(!((lightsensor_1.read()) < (10)));
    6 g* y; o7 L5 x7 U
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    2 q: I- k6 A% b5 }+ }% X
53.   {
    
54.    int Err = getErr();
    : G. D- U, Y7 s; V9 l/ q# m) S. j
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    , E: g) m7 I* J7 J% H0 G2 @: z
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    : i4 M5 l5 H. ~3 F0 b
66.      {
    , I- d$ r- h, C; L: y
67.        Speed -= 5;
    ) M9 |, y5 N+ Z4 a7 c5 \% Y0 J! R! o
68.        moto(0,Speed);
    
69.        do
    
70.        {
    9 D! n$ v0 [/ {5 ]& v8 h
71.          Err = getErr();
    ; \) C6 W( ?3 |. d+ }
72.        }while((D1+D6) == 0);
    
73.      }
      _5 g' x( b' z9 ~( @3 d- c& r
74.      else
    
75.      {
    8 D& K) _! ^. G9 o) d5 `7 B( f
76.        Speed -= 5;
    6 D. S0 |7 e( X/ P
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    * X6 e1 m% j- T- S
83.    }
    : ^+ @* v" O' h) J8 A/ o
84.    else
    ) g/ Q' g, o! Q" ?
85.    {
    2 U$ p1 n' S$ ?7 L& Y6 n5 r+ ~
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    6 @8 p7 V1 P, ?
88.      integral = integral + Err;
    ; M/ E, n' w  Z1 |
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    7 u: ~* j2 Z; I' X, C/ E) H
92.      previous_error = Err;
    
93.    }
    # T) l3 w. K, T3 T0 W5 U
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
      x4 y' V# \  c
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    ; F; K5 e4 k# G0 ^0 T: Q% x, o+ I
98. }
    
99. 
    % W' n- N, _( n( N1 \" |
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     . e' |4 m* _2 {0 [6 B
103.    D1 = ~(sData>>0)&1;
     
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;
     
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)
     , ]- H) Z) E1 e
112.    {
     / m8 ?8 F# A9 f1 [
113.      return 99;
     4 C/ T6 p8 z# ^# w  o' {1 n
114.    }
     * l/ B" T. p' X" Z  R3 |! B7 |
115.    else
     ! Y. d6 J6 V9 @* f8 O+ X
116.    {
     
117.     return upD/downD;
     % E1 k* O% P& C5 j) d
118.    }
     
119. }
     - s+ e" {4 b9 o# e5 s& m
120. 
     
121. void moto(int Speed_L,int Speed_R)
     * B# \- r% G4 D- [# X$ g2 u
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp
+ }  B, T8 I: \4 |3 L9 D

1. #include "MeLineFollowerArray.h"
   
2. 
   : X; r" X* d6 p- o
3. #ifdef ME_PORT_DEFINED
   3 e+ e( ?, ?: f. N2 X
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   8 K7 x" h9 @/ v- f( _3 `6 K% f' V
5. {
   6 I$ i1 R. w5 O* N
6. 
   , a9 F7 R  ?% f
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   4 W. v8 k8 Z& F5 ?- E
9. {
     t/ x9 c& m1 I" |& c
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    ) D4 a5 Z  |( P* S" V
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    % ?6 B5 @: P% ~0 Z% e
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    6 U$ m2 I+ D1 p6 @; t3 l& o2 u: ]
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    , K9 f1 ?" g8 P  p: m8 U) T0 g' b
20. }
    4 b8 n- S7 Q3 w' j+ K
21. #endif // ME_PORT_DEFINED
    
22. 
    ) u9 n, e, B$ a, ^3 l( }* S0 B
23. 
    - k- V" X3 e5 ~/ }
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    ' i7 J2 C* L% k+ p) w1 N
31.     s2 = pin;
    $ M/ E: G( l8 F7 A1 v& h
32.     #endif
    
33. }
    % x$ s0 j' P$ @
34. 
    ; r, ]( v1 a9 j( y6 I/ k
35. uint8_t MeLineFollowerArray::getValue()
    . F! s2 z4 f1 m; v4 ^
36. {
    7 U& w# x8 @0 n) J1 O  G
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    9 k: [$ e- K- C
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    . T  \" R; ^. X2 h
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    7 W5 }/ p" \  \0 v& e/ ~; T5 [
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    ) K: F  e$ o" ~* K* d
49.     delayMicroseconds(10);
    
50. 
    / G. C* D; x! T1 ^( v
51.     time_out_flag = millis();
    3 m" l  z4 a' Z. B
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    " P& [3 Q, s4 S" J5 c
53. 
    7 T  J2 F0 w+ H% B: _
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    # e/ I5 _! \/ K8 W. Y2 r
57.         return 0xff;
    ! c6 }' P" b+ _) ^9 y; n4 S/ ]9 B9 ^
58.     }
    2 d/ _% ~9 b% [1 @- |
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
    , M& |: V& R2 c. r
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    ' @5 m( l- v, h8 L$ x& @
72.         return 0xff;
    
73.     }
    ) L3 z  \3 c1 s! q; Y" A0 a
74. 
    
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. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    ) r1 i8 H+ V' ^2 A
82.     }
    . k3 F9 s  x% N/ L6 ~
83. 
    # l, }9 s6 Z$ d& i( w5 k) w$ X4 _
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    0 {. G) Z% D( |8 `4 P4 I
85.     {
    : `7 W. P2 x7 F  ?
86.         return 0xff;
    
87.     }
    % N# a5 Y6 D, q& ]1 w1 p- W
88. 
    / e* p6 }- N# Z9 Y. J1 H& N4 p
89.     for(uint8_t k=0; k<3; k++)
    3 |6 [3 I1 D( `
90.     {
    
91.         Sensor_Data[k] = 0x00;
    . I# n# l( h2 A' ?( c4 H2 j7 A
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    6 ~5 M$ u  c- n4 T5 V7 I: u: U
94.         {
    1 q  S  _2 \# d8 s! ]
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;
    & i8 N, O7 ~5 T, |. B6 j$ ]# h; L
98. 
    3 E/ d5 ?' d* F( t
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    5 K. y' A- m# {: T$ z0 p2 e9 o
100.             {
     
101.                 return 0xff;
     
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     " y) h+ D/ a) v$ \# a- x& U
105.             LOW_level_read_time  = micros();
     * C$ [/ [1 D' i
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     * _* t4 R: N6 h
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     3 O8 S, l7 p3 p% l$ D
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     5 X7 M5 @- [! v2 J
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     - X7 \: Y2 L" ?, ?- b. E
114.                 return 0xff;
     ' ~' }) g4 |5 Z5 N, l+ `; [
115.             }
     
116. 
     " ^5 g1 {  C% M
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     7 o4 N. @0 t! ?+ ~
118.             {
     * U! A% I: S( T
119.                 return 0xff;
     # F. V: U9 r7 F8 ^" U$ w; n
120.             }
     $ Y, \1 ^& M# \8 D  Z8 A
121.         }
     8 c" f9 X7 S) m) Q& f
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();
     6 \' e( w% q* s4 ]$ g/ p
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     4 X! V3 ~, t; `$ }* Z9 P( @( h2 j
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     ( v8 t+ s4 U2 {. x+ L) x* y; |
129.     {
     
130.         return 0xff;
     ( u" t7 N9 t4 K1 I1 t. k% ?* q6 o9 U
131.     }
     6 u* e# Q. ?; {1 G* a% k4 z
132. 
     ' q9 Z3 K2 y( L) K5 B5 t, m6 f7 K  @
133.     pinMode(_DataPin, OUTPUT);
     4 X" K2 u; z% @5 T/ V
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     1 R, I7 g; u9 p5 }7 V1 u2 ?
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 E, T! l# V9 X# `" h8 T& a5 @6 I
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     * d9 G2 Z- s' W+ m; L- F1 c& ?
141.     else
     ) s% e2 y9 ^) T' G% X. l: ]
142.     {
     
143.         return old_data;
     
144.     }
     0 @6 m1 f) z$ e3 q' X4 r' m
145. }
     ' l  R/ `5 V, @, F

複製代碼

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

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
& e$ b# U% Y$ t' _0 s哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
# v; G6 Z6 |4 @* ]/ |& Y能否用到mblock 5 上面呢？

MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀