mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   + n  i  ~7 h$ x" C; b
2. #include <MeAuriga.h>
   8 C7 T. {6 R, R$ n
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   3 q1 j3 W# u( ?  A$ L
6. MeEncoderOnBoard Encoder_2(SLOT2);
   3 [; V, i& O4 i6 \5 h# B
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   , \& Q* D- @. H# p' @; F
10. MeLineFollowerArray linefollower(PORT_6);
    1 Z6 [4 _: K/ {2 q; q, o0 z6 x' j
11. 
    
12. #define Error1 1
    
13. #define Error2 2
    # z; ?9 y! B$ L; P  X
14. #define Error3 3
    
15. 
    / S) {4 O2 `: F8 b
16. #define Kp 15
    
17. #define Ki 0.15
    / B( ?+ d8 r1 T! l4 c
18. #define Kd 0.03
    3 f) {8 F1 P& }3 c
19. 
    5 F; p" N; Q: ^; \
20. uint8_t sData;
    + R) l; |, G# ^6 U9 B4 n
21. uint8_t D1;
    7 }$ x  Z7 Y, `+ B
22. uint8_t D2;
    2 K/ {, B6 Y. h  f" z
23. uint8_t D3;
    " W' @" ~- F% _; l: ]" F& I4 E9 J
24. uint8_t D4;
    7 A/ F  l5 O6 v3 Q
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    " r' L4 l$ _' q$ [8 F  q$ y
31. int  Speed  = 160;
    ( f1 z% K( ^- L- |
32. float output;
    - S$ y" a! B) X/ F6 P
33. 
    8 J- V4 |' H) i, `1 ~
34. byte Left;
    
35. 
    " P4 T/ b; P) l3 R! R
36. void setup()
    
37. {
      F, x5 p4 N& Z& h. O. ]! ?; V4 ~
38. //Set PWM 8KHz
    - ]( Q$ D: `8 N$ x8 X: d$ `
39.   TCCR1A = _BV(WGM10);
    . I/ v& L, L/ d$ k
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    , I3 f, j$ M2 q0 z& g1 @1 H: B( k1 _
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    3 d% x5 K8 f3 O. O3 {
45. }
    7 s  z) s6 |8 w/ `" a8 N$ m
46. 
    
47. void loop()
    
48. {
    * [7 U+ S# K( Z+ S, L
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    8 k8 t5 x" C3 p) c6 |0 `# Q! p
52.   do
    
53.   {
    ( n$ v3 K& |' J3 A' S& f1 \1 n6 k
54.    int Err = getErr();
    & Q7 A( u" c+ z( [5 T
55.    if(D1 == 1)
    
56.    {
    & c& g# [8 r7 z8 n& D2 P: E
57.      Left = 1;
    1 Y" @9 @4 C) b5 y* E
58.    }
    * }9 M* y. ^( n" L5 q2 g! i, c2 t) l' ]
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    ' M" I' K' K$ B2 w3 m+ v6 o
62.    }
    - S$ ~( d. l; L  B, p
63.    if(Err == 99)
    
64.    {
    : q6 J  U* {2 r( P5 R' `! U& ^7 w
65.      if(Left == 1)
    9 F) r9 S/ \! y: P  \1 r
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    6 l% L; }3 d$ i+ z* g0 S- M
69.        do
    
70.        {
    
71.          Err = getErr();
    / Q/ B/ A1 `  f! b; S$ _- o! I; k
72.        }while((D1+D6) == 0);
    
73.      }
    ) n- ^& d; c; M: }! o
74.      else
    
75.      {
    : j6 b3 a0 I7 v* T+ i6 |; I  u& Z0 Z
76.        Speed -= 5;
    $ |" _7 G7 J7 X
77.        moto(Speed,0);
    ) B+ N3 C, M$ N
78.        do
    " s/ ~  f. L9 X! y9 A" w( k
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    2 P$ E* }: Z5 }0 v, I4 `
83.    }
    ( w2 W' V1 `  ~2 c) }
84.    else
    0 ^$ W7 V( }8 }+ J, e4 a# }
85.    {
    3 o3 V* \& X6 e, |5 [4 g
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ! D8 l" v/ u4 D# ~4 m8 S3 v
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    $ Q) o8 C8 b( w9 n! k7 g' [2 }
89.      derivative = Err - previous_error;
      b- [% X- N. z4 R6 @
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    8 p, [7 W( W$ v. o, e+ x
93.    }
    * |/ o  V! d; S6 J
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    . C+ N8 Q9 X$ ^. a! ?6 |! {4 O
97.   buzzer.tone(262, 500);
    
98. }
    0 ~! d5 Y/ F. |$ z  [( v3 D3 H
99. 
    & H( p# |; d% R0 C1 B. ]
100. int getErr()
     4 v. j' W0 g" {0 U2 O
101. {  
     % [7 }$ s  _) F: k8 m
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     - @4 D( ^4 ?+ g' C
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     : O( h& Y9 l! k9 B8 @+ c
108.    D6 = ~(sData>>5)&1;
     # ~* u% G+ F, ^# I0 }
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)
     + a: l6 K9 ^) V# W3 d
112.    {
     
113.      return 99;
     
114.    }
     
115.    else
     ! l! Z1 k( v- ~, K8 D
116.    {
     
117.     return upD/downD;
     1 I0 `5 I9 i4 I! m- o5 `- o
118.    }
     & ]9 E. M! }- W+ h  n
119. }
     ' S! o6 T( d, j1 q% Y0 [* l. d3 ~
120. 
     . s2 M/ R& q; U4 `" i! u
121. void moto(int Speed_L,int Speed_R)
     3 A( n# E) m) z. N3 `$ V, v
122. {
     2 I6 q  s9 Z$ D5 I+ V
123.   Encoder_2.setMotorPwm(Speed_L);
     & M2 n' A2 T7 c5 Y- r+ a
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

' m0 j( H1 m: }MeLineFollowerArray.cpp
* q+ C- \$ `/ Y

1. #include "MeLineFollowerArray.h"
   ; f) X0 q+ e: r" r6 u
2. 
   
3. #ifdef ME_PORT_DEFINED
   ) ?6 {6 N; A6 f' P1 I
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   ( U% s8 z0 k2 i) ~" `( K3 K- ?  O/ A
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    5 C' |1 T6 w) ?" y4 }
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    ) n. s+ R3 `# O6 U
13. }
    6 f9 ^# k* B; S' r, r
14. #else // ME_PORT_DEFINED
    # a3 i" }% s, ]$ k3 Y4 W9 `* L
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    ( ?: q6 k7 d6 q9 [. [
19.     digitalWrite(_DataPin, HIGH);
    ' @$ }# H1 L$ \% Z& Q) V# q% C- `
20. }
    ! E6 w  w$ n/ s8 R( G
21. #endif // ME_PORT_DEFINED
    
22. 
    4 g4 L  I' e3 C: o7 q
23. 
    ) k, ]* j# }% l
24. void MeLineFollowerArray::setpin(uint8_t pin)
    , H2 I- \; P: o9 |* j: D
25. {
    
26.     _DataPin = pin;
    ! g8 g8 J) y" x; A' j  ^5 n; p
27.     pinMode(_DataPin, OUTPUT);
    9 d- J, f6 Z! }- m8 W$ z
28.     digitalWrite(_DataPin, HIGH);
    ( x( k# U0 U# V
29. 
    0 v! O0 X/ d6 M3 I: F
30.     #ifdef ME_PORT_DEFINED
    1 D" ]/ J8 }& e( j/ C2 b! s
31.     s2 = pin;
    
32.     #endif
    ; t7 X- w+ H4 ]; K/ N/ J  W! ?* o3 f/ h
33. }
    6 A2 p( m4 @8 m
34. 
    $ a4 C5 g) }) [3 c
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    . z* Y$ _! S( F( _
37.     uint32_t LOW_level_read_time;
    ! F, U; l9 _7 i4 L& W# a
38.     uint32_t HIGH_level_read_time;
    . D* _# S. u$ V/ ~$ _; s& l( ~
39.     uint32_t time_out_flag;
    ' E$ S9 S4 w6 v4 F; \1 X4 C
40.     uint8_t Sensor_Data[3];
    " Q+ o0 x. B, k* r, R- [: Z4 s
41.     static uint8_t old_data = 0xff;
    
42. 
    ' [6 L4 m4 I, z- ?! y
43.     pinMode(_DataPin, OUTPUT);
    ) u& O9 R2 O" Z% i
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    7 f" Z( C: u% o* L
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    # F; g" X6 L3 I& L% W
49.     delayMicroseconds(10);
    / |; r9 O3 p, O: n
50. 
    
51.     time_out_flag = millis();
    ; r3 L# i6 }/ C& R  v# T
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    - _0 I! v4 J* U% f
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    ; K' B& O3 y) {# M; p
57.         return 0xff;
    2 Y  k" W* A' _
58.     }
    8 ^) ?0 }+ j: T6 z
59. 
    2 V& ?/ T3 w( c4 I
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    ) O+ d$ f6 Z& [3 k
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    / i7 J6 W) U& K
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ) V: c7 E- G5 v- Q4 t4 e- u
66.     {
    % Y# d* o6 v( s4 L. I
67.         return 0xff;
    * s1 a7 G4 \2 z; Z$ g% P
68.     }
    . L7 ?  }) X. }; b# Z6 P% q+ R
69. 
    ; b& ]" p3 l1 P$ N: H; L5 e
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    / B/ E7 ?  ]' Y: J- k( I
71.     {
    
72.         return 0xff;
    % r& P0 I1 w( |
73.     }
    
74. 
    * V  U/ l/ \) F+ F
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    + g; p3 [. W+ }6 Q
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
    * H* M+ U% D, A, F. T9 z
80.     {
    $ }4 F) n9 @! m( R
81.         return 0xff;
    & s3 ~' w' k8 K+ r
82.     }
    5 ]; ]/ m* [- m& `6 y2 c3 \, f0 {) [
83. 
    . T7 i& ^- n2 D2 |: ]' d, `
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    / m) C( T  `" i8 p+ T8 S: \
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    1 M4 C3 o7 Z/ g0 c' n2 y0 f0 L
91.         Sensor_Data[k] = 0x00;
    " G  B$ k, b5 d! K3 d; c
92. 
    - k) L0 X( `- s$ e) S+ I
93.         for(uint8_t i=0;i<8;i++)
    2 `! G; {# m6 a. }# c3 P% }6 `
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    6 D* K  c6 D% a* R# J: S. D
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    6 Q8 \0 U7 ^  v# F
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    2 N6 C2 e3 T1 T% B" i
100.             {
     0 N$ {, V" M9 q( ?% H' z( W2 q
101.                 return 0xff;
     
102.             }
     : W6 z0 }1 W. @- {
103. 
     & K4 ?0 K. V: W) c  Y
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     , C1 A: k/ s" }7 I; b0 u' J3 u% Q
105.             LOW_level_read_time  = micros();
     3 B( R8 E( p$ V( ?4 F
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     $ k2 j9 }0 R3 c& O7 f; O4 x
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     . H* `" d8 ~6 V& g
114.                 return 0xff;
     
115.             }
     ) C. X' o4 c. N& D0 M% n3 ]
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     / v+ q) l8 P- Q# Q
118.             {
     ) H, b; @; x8 D! e- r6 d- _
119.                 return 0xff;
     
120.             }
     . ?3 B/ t3 v7 Y' ~( x+ j' r
121.         }
     
122.     }
     ! w! q/ h. H" r% a9 Y1 t# M% o
123. 
     $ m. F5 Z3 g2 k# Z
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     0 b! K6 t1 ?; |1 b% q9 Z
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) )
     
129.     {
     
130.         return 0xff;
     8 |1 U( s  e2 p& ~; Q
131.     }
     / }8 ?$ w* d; g2 X: b- v
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     5 Y1 _/ N+ I# ^2 M3 d2 y& i4 K
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     8 N% c* h) S8 ]3 o
137.     {
     
138.         old_data = Sensor_Data[0];
     4 ]% I: o  W# i) y% \( N
139.         return Sensor_Data[0];
     : |; I" W  F* P) c  \/ H
140.     }
     
141.     else
     : x3 m' |. Q6 @
142.     {
     - u; A' s5 \+ h% F6 E( t
143.         return old_data;
     
144.     }
     8 T; ^! J2 j* B
145. }
     , M4 t3 j9 {: P

複製代碼

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

2018-2-13 01:52 上傳

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

% Z. t. q( R: U7 B9 V

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
+ x5 M/ e" O# ^2 j. _9 O% W哇...要100 個銅錢

9 Y1 K4 Z  y0 @( x% T5 ~  K5 f認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
* e8 q/ L2 E1 }9 }( y, H( ?能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手