mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   : u$ _, |$ U2 P: |" u' A1 U
2. #include <MeAuriga.h>
   $ O: d! I8 k# c0 _2 B
3. #include "MeLineFollowerArray.h"
   % b  L2 w# H! w! F3 m1 Z
4. 
   ; \6 E4 U8 \! {
5. MeEncoderOnBoard Encoder_1(SLOT1);
   - Q6 `% S, B/ \. i  N
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   ! k2 Y, j# w6 q
10. MeLineFollowerArray linefollower(PORT_6);
    5 J& R! w+ Q4 t. e7 u
11. 
    3 g( i  J" l3 V' p7 k
12. #define Error1 1
    
13. #define Error2 2
    , @# i9 t. |* _0 M
14. #define Error3 3
    
15. 
    / b9 r& h& K& E* k3 ?
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    / a1 m4 Q$ W, e0 P
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    ) Z0 m1 Q1 E9 ~/ U. \9 _
31. int  Speed  = 160;
    
32. float output;
    * P3 s! N- w7 N% H6 Y
33. 
    4 n4 c1 B) ]# ]. J8 F) \: P! e6 y
34. byte Left;
    
35. 
    # f3 B' u7 U) K
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    / {$ {( J* t7 b7 |
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    $ e! k8 ]6 x1 @0 H' E
43.   Serial.begin(9600);
    / A4 O* H; T; f5 M1 J6 H
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    
48. {
    8 Z1 C' W+ d) |. h/ x& `
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    " ^: E2 H0 o0 V2 W4 D
51.   delay(500);
    : U/ N7 d: F' ~- S7 v7 R% N" x6 d0 S5 Y
52.   do
    * I; T% p  M+ q& }5 z, `/ w
53.   {
    . Y! `! i+ D: x- |$ ^
54.    int Err = getErr();
    + `- E! V! q- F. A
55.    if(D1 == 1)
    6 A$ r' G- [- B/ p' [
56.    {
    ; ~2 E& `9 [1 |2 \
57.      Left = 1;
    1 g+ V4 k0 ^4 i3 b1 c5 O  ^
58.    }
    
59.    if(D6 == 1)
    8 w# w4 n; T! ], @4 b
60.    {
    
61.      Left = 0;
    . S( a9 N" L4 }+ ]% U% ?$ q
62.    }
    
63.    if(Err == 99)
    
64.    {
    9 T+ B# S' C4 Z; S0 h- e. O
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    6 ]8 ], A0 ~# w% j/ t
68.        moto(0,Speed);
    
69.        do
    
70.        {
    0 Y* M- c5 J5 G# f# b# K: ~& i+ M
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    * a/ ~" T" w" \6 i
73.      }
    
74.      else
    ! A& e0 o* N( m
75.      {
    
76.        Speed -= 5;
    1 @- v9 q6 V& h. z# g- h' }% r
77.        moto(Speed,0);
    4 a+ T# _. Z8 B: H1 o
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    * G& D' r0 _8 U# z; S) k# o
82.      }
    7 d' }& k! @( X: l7 v* W( k" d, t1 q
83.    }
    9 K" Z% E0 q& ]
84.    else
    4 p$ y0 T+ V/ G
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ! [( o$ K7 \! {: i8 ~
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    1 ^: B8 A" @0 L, z  S
88.      integral = integral + Err;
    . q* z; B/ P& L# l, t6 `
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    * O" S' h- t/ ]% [
91.      moto(int(Speed-output),int(Speed+output));
    . G0 h7 C, Y. F
92.      previous_error = Err;
    $ Z: `, T6 ?1 y/ g: H4 X$ c" f& D
93.    }
    8 p8 A& H( Q' l  d  X' ^
94.   }while(!((lightsensor_2.read()) < (10)));
    3 h, R% i" A: d( t8 s3 N! Y
95.   moto(0,0);
    
96.   delay(500);
    7 a* j2 E5 C; B8 o4 G
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    
100. int getErr()
     & |; r# ^3 ?6 ?  l) t% c
101. {  
     
102.    sData = linefollower.getValue();
     1 B) h) F# M3 N2 I9 h
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     9 @  ]6 P; L( T% I! [# V" b9 j
105.    D3 = ~(sData>>2)&1;
     $ U5 ?1 b! O( m  @( R
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     ( v4 g8 W% U9 b# s1 ?1 p# z
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     8 s2 M. Q+ l/ R3 P$ @
111.    if(downD == 0)
     
112.    {
     1 H3 F; p2 t; M5 k
113.      return 99;
     
114.    }
     
115.    else
     9 h3 P9 c5 g2 C% o3 X+ a% Q
116.    {
     
117.     return upD/downD;
     ( P" n& K2 J) o
118.    }
     * C( d  a2 l2 z. c& u) `* `
119. }
     5 l- [. {- j( ?1 ~6 J
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     + E7 P9 F4 E& O( ], j1 h0 b' Q4 Q( b
123.   Encoder_2.setMotorPwm(Speed_L);
     ( [6 ~! y' A4 ?- K, ]( v- Y
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

7 v' J/ J8 V! F, J, VMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   " V6 e; C( A/ Z% G: S0 J
2. 
   ! p: e9 w. x" n4 y$ ~  z; W3 g: Y/ A
3. #ifdef ME_PORT_DEFINED
   8 X* Q0 E, D) V+ q2 {/ v& ^
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   5 M& v  p) h) O
5. {
   
6. 
   
7. }
   . m- r% J" {: _1 y' t4 M- o
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   * l# L" s3 N8 b  h: x  F
10.     _DataPin = mePort[port].s2;
    9 F# F8 ?  s, B% E$ w. C
11.     pinMode(_DataPin, OUTPUT);
    - d6 W: x+ M" X+ A7 J9 h( e6 S( X! u) i
12.     digitalWrite(_DataPin, HIGH);
    + L. Y9 u  L0 Q( m8 \( f
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    / P/ q% o1 M; N% \# D7 p9 F! Y
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    * G9 B. z; U7 w7 S
21. #endif // ME_PORT_DEFINED
    
22. 
    2 N) B2 _8 ~' {' H4 k+ b, u2 @- P
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    6 I6 l: F$ f: I/ ~
27.     pinMode(_DataPin, OUTPUT);
    % w2 z6 l7 q% y- X
28.     digitalWrite(_DataPin, HIGH);
    , J. B2 o. G) G, R% [3 P
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    . w% I" L* b! h. l1 t7 U) O7 n: s3 U
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    ; D/ {) J8 G8 b/ G* b
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    $ o  S: x# \4 Z" P. ?9 u+ A
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    ' s2 I/ p" D4 W9 }+ V
43.     pinMode(_DataPin, OUTPUT);
    # P$ J) e: n# `9 I0 I
44.     digitalWrite(_DataPin, LOW);
    6 P; t1 w: o& g
45.     delayMicroseconds(980);
      ^3 a1 g; m& m
46.     digitalWrite(_DataPin, HIGH);
    3 A( U) H8 \4 S/ E8 P+ J* m
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    ! s& s) P, E5 _) Z: N
49.     delayMicroseconds(10);
    / O" d5 G2 S$ T1 p  ?# F8 Z' m! k
50. 
    7 X8 s2 j5 d, p/ B/ x: o
51.     time_out_flag = millis();
    , D3 A$ |4 T: |& q
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    , }! K0 ?) L  I5 w
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    ; d- v# c7 K  h% ]; t; I, [! e8 _" t
57.         return 0xff;
    
58.     }
    $ n2 u4 E* X$ B; j! Z2 A. U) h
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    / L1 s2 O& s+ X7 {7 m
61. 
    1 b1 U/ T, t9 Z8 j# v
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ; j7 I% Y( s$ Q/ `* h5 ]5 J$ o
64. 
    ) q) v2 |! t; w) O2 C# `' V5 x
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ( }. _$ @% b; X" l. k
66.     {
    . b6 p& m3 m, a  C9 C" t0 ]
67.         return 0xff;
    
68.     }
    & D2 c- p+ S9 L& P0 ^
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    * l! f5 T! O$ O  z% o
71.     {
    
72.         return 0xff;
    ! _: Z/ l" O) A; _
73.     }
    1 d) D' O! U. v% d4 ^
74. 
    4 |+ l, r8 s' L; Q0 E2 z2 p8 ~
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    9 c% |$ H  F( [% |/ D3 K
76.     LOW_level_read_time  = micros();
    * X! m' k- O) W
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
      a  ]7 _3 ^; ^# q* L
78. 
    + l8 b! c" M# e) S$ t, \8 ~
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    & d+ X8 ^. x( k% A, U, m4 w! v$ _
83. 
    3 ~5 @/ h2 O. T# w& x
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    ' N0 o+ b! L, a6 a( e
87.     }
    
88. 
    ) P! I5 f: j. P) `8 I
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    ! w$ C# e5 V- ]2 [6 X4 J
91.         Sensor_Data[k] = 0x00;
    + _, T0 A& Y" i8 `: d8 g2 S
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    , J  \; z2 J' u" ~
96.             HIGH_level_read_time = micros();
    0 n& _% \0 G: `/ o2 t, E3 c
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    ) @, I* l4 b9 ^* |
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     : K1 s1 X& Y  ~( j+ G
102.             }
     4 L: G7 ^# ?3 Q' I
103. 
     
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. 
     ( t) \' A- A# |
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     1 M: N7 v+ D/ N6 }2 H  h
109.             {
     ) F0 I6 j+ B/ w3 p7 J/ C$ r
110.                 Sensor_Data[k] |= (0x80 >> i);
     ' {3 q" B# p/ C3 K0 w% e& f0 u& G
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     # S- b9 {7 J. C* v! h$ X! D
114.                 return 0xff;
     % M% N! ?& ?: q8 h# e8 E& W
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     / ~$ V; Z( C% `1 E: e
118.             {
     0 q  k0 }$ i; b. }" X! X, w5 t
119.                 return 0xff;
     
120.             }
     6 p) B1 X9 a) W3 Q  B) \0 i4 E
121.         }
     $ \; k! o5 }0 @, \8 U2 m
122.     }
     ( x8 O7 U( L  v7 B6 n6 k3 d
123. 
     " x! X  `. W, u' g
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;
     + x7 k, T0 |$ J, I
127. 
     ' X) [  q- T+ v, u8 s) V$ q' K  E2 U
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     3 {+ R% P4 Q+ H
130.         return 0xff;
     # M  @) _, H3 C2 H% P& `3 Q
131.     }
     - S- B) b6 D* H7 m5 u- _
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     % d; P2 |5 ?+ Q% c2 D. u) E0 Q$ _
134.     digitalWrite(_DataPin, HIGH);
     
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];
     8 w0 g0 A1 z2 W$ I3 W
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     0 G( K9 T/ _. F! U
142.     {
     ' A2 j3 b9 d9 [6 K
143.         return old_data;
     ' O) d9 ~8 U( B) W( D* [. I5 t
144.     }
     
145. }
     ! }  v8 D1 m* ^* [" t

複製代碼

2 @5 G; A8 }1 K- z* x& eMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

* _* w2 l' }4 R# t- a
/ d1 x2 z: C7 h. ~/ V5 }

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
- m# q' R) X& s* Z. Q哇...要100 個銅錢

& h+ u  Q; [8 D$ R( Q認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手