mBot Ranger 高速循線範例＿

1 ^; n& u6 t! x+ M! t& U; y' H% c

1. #include <Arduino.h>
   * K% |( }7 s! _% n7 \
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   # Q) e+ `$ Q: w  q  J
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ' x2 m- }  h, G7 V% o9 d3 {* O) d
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   ' T/ o/ W: l; x* v! S8 r3 \
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
      |# U' w  ~& N  o  e
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    : @% h. n/ B! i# ]
17. #define Ki 0.15
    # d9 H) r% Q$ q( c1 \3 e0 I
18. #define Kd 0.03
    0 a1 q7 `6 r$ t5 }  u" I
19. 
    
20. uint8_t sData;
    
21. uint8_t D1;
    ! J. J5 n7 @4 e, U( x
22. uint8_t D2;
    
23. uint8_t D3;
    . r5 u" }2 V8 K2 F
24. uint8_t D4;
    ! ~* r: V+ Q  G! ~1 w+ o
25. uint8_t D5;
    
26. uint8_t D6;
    # D0 Q; K# C& Y, z+ o- q  f
27. 
    
28. float previous_error = 0;
    $ G1 F5 z* b( w7 h% {" p4 K3 r
29. float integral = 0;
    ' c/ z$ m  _" x% m* N! Z
30. float derivative = 0;
    - ^& Q, m( `, Z# E% @; P
31. int  Speed  = 160;
    1 O* }8 P, j+ c- g, @* N; K; V
32. float output;
    
33. 
    , R8 N5 A$ Z% X6 N" L
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    + h& t" ~+ N- W, w
38. //Set PWM 8KHz
    # H' W) }5 U8 Y7 _8 Y' ~/ M2 h
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    " Y; U, B/ S# O; @* E
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    ) g( S, H' y& z4 Y
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    . ], a. ~8 ^; {
48. {
    ! H" G* W' |- I, g3 i
49.   while(!((lightsensor_1.read()) < (10)));
    6 M1 Y- g% W: a, P
50.   buzzer.tone(1047, 500);
    - ]6 T0 |& L; k; `) y$ F
51.   delay(500);
    
52.   do
    9 ?0 p3 X, i+ T
53.   {
    9 t  T9 P5 u: D3 o: l! `$ e
54.    int Err = getErr();
    % [) ^5 s3 \6 K- E; ?
55.    if(D1 == 1)
    0 m) a3 E% R7 u2 ^4 D
56.    {
    $ M/ @  e/ o( O/ ^
57.      Left = 1;
    * g5 }0 M" E$ z' }, Z1 c  _' r/ a
58.    }
    / E$ L- o6 V7 G; h2 b
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    
62.    }
    + y7 g. T5 E8 H9 E
63.    if(Err == 99)
    % a; V, e) t6 C
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    " E4 t7 G6 L; _1 H  ?" A  H* h
69.        do
    
70.        {
    ( |% T- w7 W7 y5 x* I; z
71.          Err = getErr();
    3 W% I. ^9 K! Y; @7 V; U
72.        }while((D1+D6) == 0);
    - w) U- w" y! k$ G" c  t3 W
73.      }
      Y' `+ ^0 Y3 V
74.      else
    3 }8 N) U0 T, l7 ]7 h& r
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
    8 U0 ]8 f' D! |& {7 ]) r  R$ v
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    
84.    else
    8 k/ D4 o4 E# n/ e2 |
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    . ^: Y2 L: R+ ?% h
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    9 Y/ k. |+ ^. P$ a4 O' v
88.      integral = integral + Err;
    5 r, p; L7 t- a) w* B+ B$ n
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    . i0 V$ j' U* q" D- L: @
92.      previous_error = Err;
    % D! ]7 r& f4 d2 h0 D. x
93.    }
    8 U1 d3 |7 x  |* I% ?" H: n6 Z
94.   }while(!((lightsensor_2.read()) < (10)));
    $ f) O* x( F6 n7 e2 g+ ^% z
95.   moto(0,0);
    4 B: O2 L4 Y9 x9 N
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    6 ?* l' E0 I. r) x& V( `6 W
99. 
    * g( U' W, [  x* N" |
100. int getErr()
       k# }3 {1 N3 P% L0 D4 t
101. {  
     8 L; W1 ^! s$ O; m
102.    sData = linefollower.getValue();
     1 ]$ Y: x! p- T% S+ e) u% f7 R: x7 p
103.    D1 = ~(sData>>0)&1;
     7 e  q8 `6 [: q& s
104.    D2 = ~(sData>>1)&1;
     , o/ O& d0 |9 U% V
105.    D3 = ~(sData>>2)&1;
     : S& N! Y; ]% p% C$ \5 f2 C# D# E! g  s
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     % Q. B" F& O8 E: F
108.    D6 = ~(sData>>5)&1;
     0 P* b, d) M; a& H
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)
     ) {( I& w9 ^) ?& G0 X& Q" a8 C
112.    {
     
113.      return 99;
     
114.    }
     6 C/ Y0 m* |( e
115.    else
     
116.    {
     ! J  o: ]3 k; B0 v2 b) O
117.     return upD/downD;
     6 Q7 Q- [  k9 b# o0 o6 y! x7 b* D3 R
118.    }
     
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     - G; t& b( I0 ~7 M
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     % k- i/ p* n) ^
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

1 R6 Q3 \7 y; l0 MMeLineFollowerArray.cpp
9 w7 O9 A3 ?- m, g7 K3 _# h

1. #include "MeLineFollowerArray.h"
   - d' r' W' U3 [( n' C% D1 K
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   6 z3 ]( y& f+ p9 o% Z3 H/ U- s
5. {
   
6. 
   8 z0 j9 j" Q2 D+ @, ^. Z
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   : r; d7 m6 C. b( `; `4 I
9. {
   ) ], n8 j% C! R0 h5 v( {4 a" M3 f
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    0 _5 X8 ?% `$ o& [( _0 A
13. }
    5 ^3 T3 A: J! u' r
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    ( _1 O) D3 x. \' p
16. {
    
17.     _DataPin = pin;
    * y: T' e% _4 w; b6 D9 R
18.     pinMode(_DataPin, OUTPUT);
    7 W. S: J: u5 w
19.     digitalWrite(_DataPin, HIGH);
    5 t: s3 c% x- |6 m
20. }
    
21. #endif // ME_PORT_DEFINED
    0 }1 v7 e, [  S# ]0 i- h
22. 
    , ~7 R' s& Y: p5 Q3 X5 }
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    " e. s7 Z  p  w
27.     pinMode(_DataPin, OUTPUT);
    4 O9 s0 _9 m# c1 R$ X# E
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    : J; L# r. ?9 S+ X  w! S8 c$ J+ o' [
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    9 A) a) ?, r/ H+ ~/ V) n0 T' P4 G
32.     #endif
    5 z2 O8 @2 `$ l3 F: C8 r6 T
33. }
    6 e% S* {; v4 G  C: _7 e
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    ' T6 X7 a# k+ ^; S' l" t: d; k
39.     uint32_t time_out_flag;
    9 |) d, G+ z/ E9 w  s
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    : M  q( X) u5 H4 x. [
42. 
    : N: N2 Q: w+ K% s: {
43.     pinMode(_DataPin, OUTPUT);
    7 W- Q) i5 g2 n5 _8 u% ?
44.     digitalWrite(_DataPin, LOW);
    3 x& H0 ?9 e' x, M' F, \: H
45.     delayMicroseconds(980);
      f3 n  P( g1 U, v
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    ' T& K9 T6 }% t; I
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    2 ~8 V5 {' w! f" o! _' ]! H
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    + o0 s# p3 Z3 @, \
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    9 i6 F+ p7 g! L. `; ~$ ]
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.     {
    5 s) Z- @+ G: i6 P- L. Y
67.         return 0xff;
    ; z/ E+ v1 G. y& Q/ t) q# T
68.     }
    $ {* ^, P4 z* [3 i* o
69. 
    5 S' f7 A# i: E! ^- G4 B  ]- [
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    $ M9 `, i  V0 E: f  c
71.     {
    9 [1 }9 M2 N9 a. l+ `
72.         return 0xff;
    
73.     }
    8 W( W0 {8 z5 ]& c- q
74. 
    ; O) \0 g. x: S' l3 L
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    3 e3 b* y. H' |7 S- v1 R( t
76.     LOW_level_read_time  = micros();
    ! i% f% ?7 ~% t. ?  p# p
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    9 Z# Z; Q! m2 L) ^1 B$ F5 Y
78. 
      y5 v; p! B3 }5 J- w7 J0 w! h5 F
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    . _8 P( `. P" V# z' m
81.         return 0xff;
    9 x2 `4 `- f6 j0 ?1 L; |8 A, S
82.     }
    
83. 
    6 I3 v4 K6 x" M
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    + t' f0 X/ P" {9 l# D7 w( I7 n
85.     {
    3 n% w2 K6 ]+ r$ Z
86.         return 0xff;
    
87.     }
    
88. 
    , S% |& o/ M2 ?% U
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    9 R; R' e7 p' i$ y* ^
91.         Sensor_Data[k] = 0x00;
    
92. 
    4 h, t. {8 O. m* G+ h9 O
93.         for(uint8_t i=0;i<8;i++)
    - W4 [4 V" u6 P1 Q/ ]
94.         {
    : G' T9 ]9 b' B( @. V
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    , a# T  H4 N) Z- v/ N0 D
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) )
    & p& \( E" T! _: k
100.             {
     
101.                 return 0xff;
     
102.             }
     
103. 
     & O/ ~, N+ a9 H: s1 y
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     9 G" G. l- O5 ]$ W
105.             LOW_level_read_time  = micros();
     / b3 }$ ^2 N& P
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     . }: g/ i/ H/ H0 k* a( y
107. 
     4 n9 |4 t2 X6 Y9 y
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     % v% c' {; p! y  d$ |8 v7 K( i
109.             {
     ; O& ?- Q5 M! ^2 V1 w; l
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     4 o# h4 n. E) f! M+ C$ ~8 @, g
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     9 L: {, ]! f9 s! j, t' o1 d
114.                 return 0xff;
     8 h- H! ~+ ^+ |8 S+ e" c  H3 s2 y
115.             }
     
116. 
     3 J# ~0 E" m+ `* u
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     
120.             }
     9 n3 q0 T& \) |/ T
121.         }
     
122.     }
     3 S, E3 k3 J3 a# |! t
123. 
     8 p% ^- ?* s9 F* I( ~- i, Z
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;
     & d, I% V2 g. v1 U- N  f! P# V: g
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     $ o) `- }% k5 M2 |9 m- n7 r( q
135. 
     1 \! e: H# h( j* y5 w7 q
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];
     
140.     }
     ! {4 h8 O7 x& ^- R) C; `
141.     else
     
142.     {
     4 ~7 ?1 k9 t7 r& k
143.         return old_data;
     " [  Q, e6 V) T
144.     }
     
145. }
     9 J. B" Q- D+ z7 o. s8 v

複製代碼

. b( ~% A5 T' I# c  B) KMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

4 J6 a' j/ q1 V! B+ D$ G
% o$ K$ r: s  @/ u* a2 n, Y

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
6 ]  `+ o  o( k) n能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手