mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   3 ?& w1 C: Y  P5 i
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   $ j3 X% ?0 G" I" ~9 {8 O
5. MeEncoderOnBoard Encoder_1(SLOT1);
   + n7 _  h  n& a" D
6. MeEncoderOnBoard Encoder_2(SLOT2);
   8 D/ O0 Y# A4 X4 u$ z
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   $ d2 g5 K# ^) b7 C
10. MeLineFollowerArray linefollower(PORT_6);
    & h7 y) t  R3 Y
11. 
    
12. #define Error1 1
    1 }4 w# \1 W* I; N- G
13. #define Error2 2
    0 q+ k; t1 \3 h
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    ! B& ^; l% W7 M( _+ B1 ~2 z
18. #define Kd 0.03
    0 w6 s7 `3 l8 P4 H' X9 B3 B
19. 
    
20. uint8_t sData;
    ; f$ ?9 p- _/ [6 E  Z; t8 }) T
21. uint8_t D1;
    " W; V3 x  _3 I5 ^$ J, S
22. uint8_t D2;
    ! x/ F0 J6 T" m% `7 N; A" f- p; |0 K
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    / ]# r. t" F7 s8 Z$ v
26. uint8_t D6;
    
27. 
    . `  O: ~6 Q: K& n9 @9 Z, t' A
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    : m; C7 r# N  f
31. int  Speed  = 160;
    ; }9 N9 s0 {, \3 T9 U
32. float output;
    
33. 
    ! B  D% t! j6 h
34. byte Left;
    * N1 ?! U* Y; {. u, x, e9 s0 }) T
35. 
    : g$ I1 W1 f- F" z6 t+ G
36. void setup()
    
37. {
    ( x4 X' @1 U2 K
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    ( h$ {, @* b0 g3 f$ c# {/ }
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    3 c! M; z0 {9 |, B' N% c
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    # Q* u6 U$ L/ d' j  q2 M2 y
42.   TCCR2B = _BV(CS21);
    $ `, P) L7 y- M, q4 q; f
43.   Serial.begin(9600);
    - \# U( s; W; L4 }0 ^
44.   buzzer.setpin(45);
    6 Q6 d8 O' z4 Q' w+ x5 g9 l
45. }
    
46. 
    / J- V- E; b5 J1 n
47. void loop()
    ) d" v- g0 o* i4 @* ]) u
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
      |& B3 i1 I* B( l6 p
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    * _2 i: [  O: d& @; ?: ~! o/ T/ m2 c
52.   do
    
53.   {
    . j5 U% J+ }9 U% t* x$ |3 `) v
54.    int Err = getErr();
    
55.    if(D1 == 1)
    " G* T/ x/ U" O* h; I: s5 L# e+ q
56.    {
    
57.      Left = 1;
    # e. Y2 q, X$ m, U
58.    }
    & b) W4 S* z; [5 r4 V# X7 k3 A& s
59.    if(D6 == 1)
    " J) D/ A8 U7 }- G, t& V) ?3 u1 z2 X
60.    {
    ' ^& m; t" S6 ]$ k& A  r
61.      Left = 0;
    ( y' d9 `8 i0 o1 O5 a5 x1 u! G7 W
62.    }
    
63.    if(Err == 99)
    " Z' W0 y; S5 \( N( h2 G# \; w
64.    {
    ' v( s. Y6 o) i/ x. P0 U
65.      if(Left == 1)
    6 F# Q. m1 K8 |, s; B
66.      {
    ' z% m; e) Z: @. p
67.        Speed -= 5;
    , h% l2 k2 h$ Y# L) _
68.        moto(0,Speed);
    
69.        do
    
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    5 ~9 N' |4 @! q# l9 y
73.      }
    1 R0 C( R* ^& f- X
74.      else
    $ C$ N3 i9 K+ C1 F
75.      {
    
76.        Speed -= 5;
    ! d$ B; u' Q  m. R# N0 ^
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    5 |' a* X( n* N* V+ C% j1 b
82.      }
    
83.    }
    
84.    else
    
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    $ u5 W0 A5 x+ A. G# P
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    8 {4 P* q/ a+ |  m' K, P1 ~
88.      integral = integral + Err;
    8 [3 q& A: N! ]+ `
89.      derivative = Err - previous_error;
    9 K# Y7 L$ R7 O9 d) |
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    . n5 E3 o1 C* C! g2 x* q' ?; A
92.      previous_error = Err;
    
93.    }
    . F/ l$ _) v8 c5 W! S7 u
94.   }while(!((lightsensor_2.read()) < (10)));
    $ f  c( m8 {6 @7 P
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    . ]( B; }. _0 r
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     . [+ x( m  N. A& O1 E
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     0 I( J" Y2 S3 W
106.    D4 = ~(sData>>3)&1;
     ! t; P3 p0 R" p3 f- ?5 H
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     5 G$ M: P3 z+ f
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.    {
     
113.      return 99;
     , w6 p! d% c" B1 M* y% I1 N
114.    }
     ! q5 C0 H" L3 G9 A! p3 S( r6 Q. e
115.    else
     
116.    {
     
117.     return upD/downD;
     ! C( V' z6 V& H/ A0 O: ^
118.    }
     6 f0 [- H2 a" n) p
119. }
     
120. 
     4 \  W- z4 \; I; P
121. void moto(int Speed_L,int Speed_R)
     
122. {
     2 n8 |% Y) g4 j; F, U1 C
123.   Encoder_2.setMotorPwm(Speed_L);
     , K. _7 p  m' t
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

9 g- i& t; [; {6 E. N  y0 sMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   ( B$ h# E2 z7 k9 A$ O* n) L* ^4 D/ j
2. 
   , `5 |9 Z, U' G! S! `
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   9 f$ ^, l5 w" a4 D: l) J. [
6. 
   3 V; o: C/ Q- y- Q
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   / [; C% O8 S6 ~0 b0 ^
10.     _DataPin = mePort[port].s2;
    7 F" p' d$ G, }! I. b
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    + g. S5 j2 w* ^. R: R$ H( w. [
14. #else // ME_PORT_DEFINED
    # w, g8 b( Q9 L7 Z% t9 m: M
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    3 k- k5 W& |" S' @& `" a' a" L* J# a
19.     digitalWrite(_DataPin, HIGH);
      Z3 h6 Y, U+ t8 |" D4 {6 \5 ?
20. }
    ; p+ Y. s2 H! H/ N
21. #endif // ME_PORT_DEFINED
    / R% i( ]- g( X2 X# Z& {* ^9 ^
22. 
    
23. 
    9 ?4 ]( W2 e& @" |/ k1 i
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    & E/ `2 v" Z  I  }% j$ x; T3 D2 B
26.     _DataPin = pin;
    ' s9 F! @2 ~2 Q7 @
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    ( W0 J8 r+ h& J0 x: Y
31.     s2 = pin;
    
32.     #endif
    
33. }
    
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    $ u2 a9 P- R6 }+ u1 K3 P, y
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    . Y1 U0 k+ g9 b9 ]/ }
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    % h- Q3 v& _- ^6 h& p& H
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
      Y5 U- [4 W) G7 E
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    " n9 ]1 I5 m) R: L! _  ]9 M6 I
46.     digitalWrite(_DataPin, HIGH);
    * A" t$ n' y8 R* _2 Y: H0 j. e3 \
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    & d1 K$ ~; M' @7 }( o; m
49.     delayMicroseconds(10);
    2 Z0 ^8 A* T( N& V
50. 
    " R7 E; ^0 H* \) P  L
51.     time_out_flag = millis();
    3 l2 T" Q- @# P# o, M. [) l& i( q
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    # U; \8 [6 O: Z
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    , g5 j9 b. D$ l  \/ r% n& V2 V% A- b
56.     {
    9 V  o4 V# R- Z7 N3 W
57.         return 0xff;
    
58.     }
    3 v8 i& u' ?4 t, {
59. 
    . \0 C+ e" {$ Z6 k* [% h. K
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    % A/ t( ?& J. O4 u
62.     HIGH_level_read_time = micros();
    # a3 I" C/ }$ L+ a+ h& Y
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    ) @2 b! S% ^6 |+ M  L* T
68.     }
    
69. 
    + t- S4 w1 d8 c! \9 C6 f
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    . \. q0 N0 l0 F9 _/ ^
71.     {
    
72.         return 0xff;
    2 j- q# S7 j) z& o8 Z. c: K6 |
73.     }
    ; W8 \3 }9 Q. J" d4 }) D' F
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    + r3 F: y1 A6 }8 P. S, c4 |* ]' k
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    - c2 s+ s' J1 P; X4 g( x- Y7 N
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    7 q4 T4 X2 L) @$ H* Q! h1 ?3 X
80.     {
    + f3 j4 N9 J$ F! \3 t. Q
81.         return 0xff;
    
82.     }
    7 T  j* s  i, Z0 Q2 S
83. 
    - z, y3 f2 G" F2 s
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    + [9 X9 }( O1 h) |/ ^/ E
86.         return 0xff;
    4 y: E8 ]+ H, _+ p* H
87.     }
    ' Z. Q7 K7 p' I
88. 
    1 b: |% K8 c1 t0 F/ Y" H* }6 C
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    ) C- F2 U5 L; |; U$ E: c
92. 
    / W4 e* p  e( S5 y! N7 a
93.         for(uint8_t i=0;i<8;i++)
      _4 X% p* g, H. T2 R
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    % {- d; S+ ~( d5 i6 i7 G  i1 S
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    6 L' M8 H: }$ E
100.             {
     
101.                 return 0xff;
     
102.             }
     
103. 
     4 N& a  G' N: X: T" P
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     & w5 Y. x4 M& V  K" R
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     ) k6 o8 @9 R, j3 W7 v
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     8 C/ S* q, O7 _# v5 I( p
109.             {
     - W4 Z7 x6 o3 d. K7 P# X# {
110.                 Sensor_Data[k] |= (0x80 >> i);
       y  r6 Z' g2 H/ P4 \% l
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     8 L# L1 J5 u; `! i. ^, f- T  k
114.                 return 0xff;
     & d7 X0 |2 w% `! `+ [, K
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     ; U9 v$ ^$ u& Z
118.             {
     
119.                 return 0xff;
     3 d" ~/ `5 S, l. ^6 g9 D
120.             }
       L2 w' {7 V8 ^9 ?8 Q/ z6 M
121.         }
     " Q" J8 Q; E  T* N
122.     }
     ! q" l  U! _0 w
123. 
     2 A3 {8 ?1 g2 W1 q& g: [. d
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     , {7 @" K% {% C. u
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     1 }- C' v: ~! R  J7 b; r
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     ) g& j$ V- T- d0 q( Q& j
129.     {
     & n; Y- H3 V) R2 V; [7 L% b3 h
130.         return 0xff;
     
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     & S5 t. a* \" V- L, [1 R
134.     digitalWrite(_DataPin, HIGH);
     & Y& n- T: R( Z* t! u8 t, M- E
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     % D; W+ k8 f0 o3 b4 R  ]
137.     {
     
138.         old_data = Sensor_Data[0];
     7 ~4 s( S+ Z. U+ n7 F( H+ ?& [1 t
139.         return Sensor_Data[0];
     . t5 ]1 d# l6 Q! `
140.     }
     , p; Y& O: G9 ]+ Q% q$ P( f
141.     else
     ! T2 X; k' H* ^/ U
142.     {
     " G# d5 B3 c3 z3 \) j  H9 b5 v
143.         return old_data;
     ! J+ U/ Z! }. m6 V$ e2 [& k3 `$ n
144.     }
     
145. }
     

複製代碼

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

2018-2-13 01:52 上傳

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

# `1 L8 ~% J6 q4 H: }; k3 N, Y4 P
0 O4 e; c) }5 I* h( y& F  x

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
0 n  _9 j' @; {' Y1 c: p能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手