mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   ) Z5 A/ B8 m$ Z1 q9 ^! y
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   $ N% R6 {# H+ z1 U& V
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   % e" K4 W" v# W0 C# E* U, m+ U
6. MeEncoderOnBoard Encoder_2(SLOT2);
   2 q8 _. w  z- F3 c/ H
7. MeLightSensor lightsensor_1(12);
   & ^/ y- T7 ]4 c2 ^# s; N
8. MeLightSensor lightsensor_2(11);
   2 ]$ p6 U* b) x* c4 Z/ m1 m6 @
9. MeBuzzer buzzer;
   * N: p; c) B2 M) j* P
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    ( j& |5 x) g4 w, {
12. #define Error1 1
    # I; t0 A1 r0 X. g  h
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    0 V' ~1 G  x8 E* i6 B- U" f- E
20. uint8_t sData;
    
21. uint8_t D1;
    + K0 I! c* g: W0 g! I
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    % ^2 [" O3 b) t- H* \
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    + r8 f! c1 l$ X
30. float derivative = 0;
    # O4 D* M- u7 R) Z3 M
31. int  Speed  = 160;
    8 b! P- _. m/ t. O: M. U6 |5 f
32. float output;
    ; Q3 l# q, r( U* u% z
33. 
    
34. byte Left;
    
35. 
    : |2 m, U" X- f
36. void setup()
    - J1 S1 t- [/ V$ I( L
37. {
    
38. //Set PWM 8KHz
    0 I1 x& H3 r  _) K7 J
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    4 G, J7 s7 S' N4 U
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    / Z) c" h6 N, F$ u, q1 D
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    5 P7 G! m/ X$ J7 B$ a3 P6 @( U/ U
44.   buzzer.setpin(45);
    
45. }
    : G  y7 p4 r+ g8 c+ l- c# B
46. 
    . q$ t/ e$ `; t( E, Q) }" o' c5 g
47. void loop()
    ) I7 b0 q& O7 ^& V( Y# M: G+ T7 _
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    & M: u/ S4 G# @1 f! t& |8 }; z
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    
54.    int Err = getErr();
    ' }6 B" n5 Z+ A7 ~
55.    if(D1 == 1)
    * J& h& t; b( z$ |3 s, v; D
56.    {
    . c7 R- u. s" D5 v) r
57.      Left = 1;
    1 T  w: H* ?. R: V% O
58.    }
    
59.    if(D6 == 1)
    ) e) t- ~1 Q5 W8 N7 D
60.    {
    
61.      Left = 0;
    5 _: P1 m, v# ]# {: V
62.    }
    
63.    if(Err == 99)
    
64.    {
    ' N% z0 Q' Z1 }2 D
65.      if(Left == 1)
    $ E( P$ x5 l: l: J+ `3 n" e
66.      {
    
67.        Speed -= 5;
    * a9 \4 Q. J1 e  D2 J: ?4 e
68.        moto(0,Speed);
    * `$ H) R$ [0 H1 `" p# }! f! N
69.        do
      V3 R& F$ I- S. e2 @' L7 W
70.        {
    
71.          Err = getErr();
    : q, y7 i- [7 E. A
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    5 _1 y. z- o1 R, t' k- d5 w
76.        Speed -= 5;
    % d; V6 F* j: T( m+ T/ @$ S: V# x3 M8 G
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
    6 |% k4 o* v9 o- |3 g! V! F
81.        }while((D1+D6) == 0);
    / o# |9 p3 d9 P/ C; w* G$ b9 d" C
82.      }
    
83.    }
    
84.    else
    
85.    {
    1 i! w/ x. l4 @9 s) q) r
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    " b8 m* o+ X3 w" P# l
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    - K# ~( R. @' c; |
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    6 j" N4 j5 {  y  @. i
92.      previous_error = Err;
    
93.    }
    2 _1 @5 k+ Q* E) [- M/ V& F" P
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    ) ?' i) o/ E( c0 w
100. int getErr()
     
101. {  
     & K) ?3 u4 }2 }6 }
102.    sData = linefollower.getValue();
     , T9 s3 I5 v4 b, ]" c
103.    D1 = ~(sData>>0)&1;
     * r; Q" z7 S; b6 O, c6 ^
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     5 u, b4 d  K2 U
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     ; V7 Z0 d- Y# E+ f' E
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     ) e' o5 H  x& Q' O
111.    if(downD == 0)
     ! R- H6 G; E2 }5 |
112.    {
     % \# G7 v5 j+ I+ D2 `/ A$ ]
113.      return 99;
     : U' u; o( A' M2 d9 Z
114.    }
     
115.    else
     * s6 ~7 n9 _# T- {
116.    {
     ! C% ^. N" A; |0 B( Q4 N4 c& {, N
117.     return upD/downD;
     ! p) w7 q' ^# d
118.    }
     
119. }
     . I8 ?+ c; R: X3 O% x
120. 
     + o, Q& y4 n9 D, s
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     ( c4 @2 @! {5 ]! T3 m* |( }  f
124.   Encoder_1.setMotorPwm(-Speed_R);
     ( r7 I8 m4 r- O* _& i6 y
125. }

複製代碼

+ d( F% R: b! \" J! ?' oMeLineFollowerArray.cpp
3 {; K) Z2 i8 {1 c* o! L1 a

1. #include "MeLineFollowerArray.h"
   
2. 
   : I: P/ T; c  c  Q! G& V
3. #ifdef ME_PORT_DEFINED
   ) }9 o3 k6 k7 o5 A! y0 U. h
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   - L9 u: g6 ?5 Z( y  S$ u# q! y
6. 
   ; ~2 U% S. d: c1 y
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   ! x1 X6 b3 h! U8 h. [9 ^! N9 U
9. {
   / F' }- m" c% W- H9 _6 [& g
10.     _DataPin = mePort[port].s2;
    0 J) a4 A9 ?$ A1 q- q1 i
11.     pinMode(_DataPin, OUTPUT);
    1 u( T" |+ u: \) o0 n* z( ~' L
12.     digitalWrite(_DataPin, HIGH);
    / Z5 Y' s  \1 z" m
13. }
    9 k+ d  P# S( b* g, m. c
14. #else // ME_PORT_DEFINED
    # p9 `: j5 Q2 T% x) C
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    " ]* H: \% C6 b1 Q- S& R
16. {
    ; q/ V7 D$ t4 g6 D
17.     _DataPin = pin;
    ! j$ j; o4 R& T: H  E, A
18.     pinMode(_DataPin, OUTPUT);
    : A2 |, P  Q6 [$ `/ o: {
19.     digitalWrite(_DataPin, HIGH);
    5 V* ~+ }9 Z' K  B; |
20. }
    ) O" O' D. C3 ?0 ?
21. #endif // ME_PORT_DEFINED
    
22. 
    # ]) A0 Q( v& V" h
23. 
    . X  _- x- \, P  R$ w
24. void MeLineFollowerArray::setpin(uint8_t pin)
    8 _, Y8 ~% y- x- L3 n, \
25. {
    
26.     _DataPin = pin;
    / m) @) o2 _: B9 v7 H3 F
27.     pinMode(_DataPin, OUTPUT);
    % M7 E% w; I4 @1 I
28.     digitalWrite(_DataPin, HIGH);
    9 x' |/ ~; U" M
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    6 @+ l$ c8 {* a, j0 s
32.     #endif
    
33. }
    6 c; W  F& J  q! x8 D# o1 @
34. 
    $ ]* n- u" n- Q2 ^
35. uint8_t MeLineFollowerArray::getValue()
    * d, [7 [7 ~$ n5 R- ^
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
      E! j+ x! M3 J1 W5 ?
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    " ]. F3 N0 Q) O% t
43.     pinMode(_DataPin, OUTPUT);
    0 u' X) B  m" ?+ |% a. c
44.     digitalWrite(_DataPin, LOW);
    6 W3 U/ O7 C* W# k* p
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    . L. m. {" o( L8 h# _6 j" R& o; b
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    , p2 {3 x+ E  r$ }
50. 
    
51.     time_out_flag = millis();
    8 o7 i+ s9 p; H# ?
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    $ t7 K3 T  M7 |
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ' M& |$ r; r) J7 ]; ?1 v
56.     {
    
57.         return 0xff;
      F1 R" c$ W) s* f5 r( ~  {( A
58.     }
    
59. 
    + P5 k# f9 L' z1 W& l0 T
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    , ?" }  J4 h5 G0 W" h+ b! C% L
61. 
    ; H) z! h& H& j9 g& J& \% X0 ?
62.     HIGH_level_read_time = micros();
    " H7 v1 a+ f% E; U4 W  H
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ' ~6 n2 n3 m0 Q" f/ i( p4 N
64. 
    / x# r" G$ B) _
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    4 u# k0 y$ L$ z) |1 }# a* }
67.         return 0xff;
    
68.     }
    7 `. ?- u9 e, a' P
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    ! {# k' \) k' U0 x2 j
73.     }
    : e  H9 A. ^1 b! G0 a; w' [) S
74. 
    & U! }' R4 P/ L; f7 T
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    7 q8 t  b/ P7 I! m: [- E
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    , h( m& s; n4 c% M
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ) }9 |( {' P' h# I5 G; g
80.     {
    
81.         return 0xff;
    # y& K5 P* F. M: l# H
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    8 d5 l" j( \, ~1 s# l
85.     {
    
86.         return 0xff;
    1 y1 N0 @. _0 [. c* |: F% `! x$ d
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    $ \9 _) v; k0 A7 D( \# `( b4 [
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    8 b! O+ D6 l1 W5 `
93.         for(uint8_t i=0;i<8;i++)
    4 y: k, s* m5 q2 l7 K
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    9 T* j) b# G* s2 L
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    9 }5 r0 z# T! v! S! Q
98. 
    + X3 u+ v9 Q' A
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    & K- a4 R9 B6 I% x
100.             {
     
101.                 return 0xff;
     
102.             }
     
103. 
     . O' [2 W- b) [) u2 {( Y' H
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     . n8 x  c* ]* ~" t1 ]9 _% d
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     8 L/ z6 t* P) w# W2 [1 U8 I  ^
107. 
     5 w/ z$ M: ?1 K$ c
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     / u  Q- @" ?7 h! h6 H- ~, A
110.                 Sensor_Data[k] |= (0x80 >> i);
     1 k# {' ~& O. Y$ P/ c
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     0 X. X' X+ r2 S) i# ?$ I
114.                 return 0xff;
     
115.             }
     
116. 
     ! E- ~1 T" v9 G( u' u
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     , @( b' n* }1 w+ g
119.                 return 0xff;
     
120.             }
     
121.         }
     
122.     }
     ) Y. l5 J6 ~; A6 @& ^. a
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;
     / M: V- a  d. ~( M% s, `# x
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     4 N( ~0 v2 g! U- Z
129.     {
     6 l  R5 z+ J+ P4 }) W2 @
130.         return 0xff;
     
131.     }
     
132. 
     ( S* M2 Y. {( X4 U
133.     pinMode(_DataPin, OUTPUT);
     8 ?7 Q! k9 T2 @8 F6 v
134.     digitalWrite(_DataPin, HIGH);
     / J' Q# M* [/ I) C6 }# Z! D- C- V/ w
135. 
     
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];
     & s7 D7 v* k' n4 _- t
140.     }
     
141.     else
     9 c# z6 X* v; I7 s7 C1 w7 |
142.     {
     % D/ M' S  M5 {5 J
143.         return old_data;
     3 w/ w$ ~0 ^0 D: g7 b, D$ x
144.     }
     
145. }
     

複製代碼

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

2018-2-13 01:52 上傳

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

! c) i% O- }1 |. H3 t

強力推廣中

哇...要100 個銅錢

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

) X8 e2 [. b9 W5 o% {認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
能否用到mblock 5 上面呢？

4 B, w( u5 A/ w1 Z8 W& xMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手