mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   , D$ R/ A& ~% [3 |0 C
4. 
   1 Y1 Y$ g. n& W2 h4 f* O
5. MeEncoderOnBoard Encoder_1(SLOT1);
   , p$ W6 \: O- c+ x/ N# q" S
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    % V" ^1 R8 I& ^/ b
11. 
    
12. #define Error1 1
    & k/ |4 m# h4 X' f, k7 M" J" c
13. #define Error2 2
    
14. #define Error3 3
    0 |1 ^3 P( ~# }' J  ^
15. 
    ! h3 d: k5 s! w4 g+ v* }2 J$ _
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    % Q1 q3 z3 `$ e2 F
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    ' A' y* ^9 z. L/ b
24. uint8_t D4;
    + @8 R# D" C; [  o" @
25. uint8_t D5;
    2 `& R. o+ g6 C9 y4 [9 ]4 d0 H
26. uint8_t D6;
    ' z* {' X0 S4 G+ M! }+ D+ X6 P
27. 
    
28. float previous_error = 0;
    0 X9 N) u! X6 s- j. X4 P
29. float integral = 0;
    / E: v5 X' x* b( [  ^3 u2 k
30. float derivative = 0;
    1 v  w  Q" Z. B4 T
31. int  Speed  = 160;
    + ]! ]2 F" z' G! y! c
32. float output;
    9 d* m! L' h% |
33. 
    . j& Z: u% H( U' r5 w$ G# C( z& N( k
34. byte Left;
    
35. 
    
36. void setup()
    1 e) T0 G1 S5 g0 a
37. {
    # E/ s' U- }3 N+ z8 D
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    . E. [) A# `2 Q! ^# D
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    % y+ Z$ L# ~- ^; H
45. }
    
46. 
    
47. void loop()
    
48. {
    ! }& e0 N) J* k# {
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    ! n- l5 X% I" L. S! L
54.    int Err = getErr();
    1 N9 w% T6 u( w( H
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    3 G1 L5 b0 u5 t' F, Z
58.    }
    ( u/ V3 i2 t" _+ W& n
59.    if(D6 == 1)
    
60.    {
    . F% {$ T+ N, u# w
61.      Left = 0;
    / ~4 l( p8 T  Y1 w* D, a' z
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    9 p4 \( Y6 K0 U- Z- v5 L- J
68.        moto(0,Speed);
      e3 ^/ W$ b. Y
69.        do
    " l2 D% k& a2 x  F
70.        {
    ( {8 ]  ^6 A& R* {' u2 y/ A
71.          Err = getErr();
    3 X- S# D4 K1 v! p6 [9 y* h' m
72.        }while((D1+D6) == 0);
    
73.      }
    . P: ?5 i0 t8 W# R3 t% `
74.      else
    
75.      {
    ; d# |2 B. z  s. H6 n) L
76.        Speed -= 5;
    * x8 k5 H- I! D
77.        moto(Speed,0);
    + D. F& _5 J5 E- i
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    # [( e, \3 n0 V% V
82.      }
    9 c; r# D* t/ }" S- ?9 \8 S5 g
83.    }
    : w5 p% l6 Y' E8 M
84.    else
    
85.    {
    2 O; I; f& n! ~4 F" I  S8 {% Q9 d
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    $ \2 h9 ]% v. b0 J
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    , q5 }  g4 G& S. _
88.      integral = integral + Err;
    & I' C- H' j" T; \$ o
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    * B# H, u( c1 h3 f; \6 a
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    . x! O( l) K! A$ ^$ |1 a, @
93.    }
    3 A. D  B0 I: g, y  R
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    5 x9 h( g7 V" i1 i0 V- ?4 a
96.   delay(500);
    - c6 B1 o! U( r+ d7 Q7 x  D! v* d
97.   buzzer.tone(262, 500);
    
98. }
    ( {8 P! Q! s8 q9 {3 E( ~8 [
99. 
    
100. int getErr()
     7 A7 C& F# l! m5 z: X' F& u
101. {  
     
102.    sData = linefollower.getValue();
     
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;
     . [  Z0 G2 Z; x: `( _
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     ) H2 Y0 I& s$ S$ g# V* M, a. l7 J0 Y
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     9 a& O. }* `$ f1 {2 K% O
111.    if(downD == 0)
     9 N; P4 B, f+ z; q
112.    {
     
113.      return 99;
     
114.    }
     
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     : T; f7 T8 d+ ^1 ?" Y! n
119. }
     
120. 
     : _8 g" I4 R6 x' m9 J) U; i( C
121. void moto(int Speed_L,int Speed_R)
     - w1 a0 U+ [8 k# D
122. {
     6 [# u. u: r  u8 y% Z# t
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

: p( l) w! z- p+ X8 JMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   % Y- O, \7 \$ J& K' _7 W0 [
3. #ifdef ME_PORT_DEFINED
   * D& a1 n9 j4 f
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   / U' P/ j3 g" _" ~# Y; m  i7 c- J
5. {
   ( O7 o& g! [6 ^  O( _
6. 
   2 F/ w: K; @4 F. S8 L! q* I
7. }
     Y1 M! Y0 o9 A! m+ z3 e1 S
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    6 i$ l3 E! J2 G% \5 A
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    # `, {  {# h  @) p
14. #else // ME_PORT_DEFINED
    * @5 W9 a5 H( {7 W
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    1 M: J0 K4 U' Q3 D( ]! {
16. {
    
17.     _DataPin = pin;
    8 r/ j6 e( v! G% F
18.     pinMode(_DataPin, OUTPUT);
    0 C7 V0 D3 n& ~+ |  |6 I! c9 [
19.     digitalWrite(_DataPin, HIGH);
    2 V6 y& E8 G  y# X6 p/ f
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    ! u5 I. h( r) {" l9 s. R
23. 
    7 l8 l/ Z$ h# u9 K: W' g
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    ; g2 y4 M. b, J0 k* U: S
26.     _DataPin = pin;
    6 ?, f! T/ k8 t7 }
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    5 `; A/ s; ]% ^% m+ I3 P7 |
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    3 t3 i. |5 p/ S5 G
33. }
    / R0 S0 }8 W- w3 y9 g4 A
34. 
    9 O, r8 ]& F8 Y5 }' u
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    / x( T5 Q  ]9 i+ B
39.     uint32_t time_out_flag;
      p, @6 N! ?; {- F
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    4 T! X$ T: Q) u' e! B0 ~
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    " ]: s/ Y5 c0 c5 U2 }
44.     digitalWrite(_DataPin, LOW);
    4 \8 C8 ~5 r5 D- A! ~
45.     delayMicroseconds(980);
    " ]8 y" I* Z4 D5 M# H- z4 o8 U
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    & u5 W2 x( f" Q
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    1 r& I1 d8 o9 M
50. 
    * ?3 l6 Z! }; m3 }2 a9 W0 _8 C
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    8 F! I8 x3 A  H/ V5 ?3 H# {/ q
53. 
    : N4 X) J2 W1 L6 k
54.     LOW_level_read_time = micros();
    / E7 ]- d) }4 k3 B4 S
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    6 L& I4 e) p. D2 S$ j# ^8 Z, k
56.     {
    ' O$ s* f( [* X. s$ q: e2 h$ K% ~
57.         return 0xff;
    . i1 a* q# E$ M
58.     }
    3 s) L4 Y0 x- ~9 b6 G8 |0 \: R
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    / E7 W9 }8 p0 L. |! K
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    4 m0 f4 E( n  p  _3 f( l( I
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    4 d8 X; q% y3 [" G% N- f
67.         return 0xff;
    ! d0 u# p& c0 [/ @. |9 h; O
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    ! a# e* u1 g% t$ O) H% f
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    : B% q  H4 r# l* F9 }" l& }7 w
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    % G$ D3 u+ E' Q
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    
83. 
    % N; J8 N0 v! @; W3 T" ?
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    ) C! a) C+ F! @( |3 z  b
87.     }
    ! d& e( i, K- v2 L) O, q' }7 }& Y
88. 
    - e+ e7 `4 @! t3 U1 g
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    ) q- R- r0 ]! D2 X6 Y
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    $ K' E$ ]/ p9 R, P' ]3 U  U, @6 F
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
      g* n) x" J! i  O& C) G
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) )
    
100.             {
     
101.                 return 0xff;
     ' d* r7 B6 Q: k, P% c' }& r+ U% ]
102.             }
     , H- Y8 q& ?% n* J& |
103. 
     # t5 u" `8 n' O, n
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     3 o) H2 W% r- L4 b
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     , K3 \7 V, |- K( i$ |
109.             {
     % h- C2 O; R4 P  h
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     ' b. X: ]+ c* i" }5 |+ T3 z
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     1 z, {/ t" c# l4 j6 \# k5 e
115.             }
     
116. 
     ( ]2 _6 I% U  Q6 Q
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     2 u2 q9 `- [$ I8 O% U( O
118.             {
     1 P$ M0 C! o0 a  a
119.                 return 0xff;
     7 ^: i- B5 ^. q; F6 ^3 Q- k( d
120.             }
     
121.         }
     / ^& r5 Q* n+ g9 r# Y
122.     }
     ) j. Z1 ^) U. V, K4 M
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     ; `  ]: y+ ]3 a$ }' Y; V) h. M; t4 }
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     ; n1 P+ O& s9 T, ~" T; |
127. 
     5 k" L9 `2 P" o  S3 J9 c+ a. \$ l
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     " a5 `! k- i- \. s
133.     pinMode(_DataPin, OUTPUT);
     " b; v; ~+ w4 i5 n( i% i( k, N
134.     digitalWrite(_DataPin, HIGH);
     $ ^3 R* R$ v  d' U4 j
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];
     $ W+ x; g% Z2 \% y- c) @
139.         return Sensor_Data[0];
     , f7 }$ `3 U$ q. h4 t
140.     }
       E, a# ~( J# n! O, a
141.     else
     
142.     {
     ; b. H4 }$ G8 l3 n7 N
143.         return old_data;
     
144.     }
     
145. }
     

複製代碼

" p' [1 O! O3 y6 M' T9 QMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
  A$ D9 N1 p! v8 n) C6 O7 Z% X$ K哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
) j/ y0 |) i. T6 `$ }' b' Q能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手