mBot Ranger 高速循線範例＿

; c0 K7 M  P' ]) V+ c$ T* l: z

1. #include <Arduino.h>
   # u' i4 H8 Z" J; b4 X: ?
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   + k) t7 q. W0 m) }$ s! W/ N0 z
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   3 I* s# c" l; \1 X$ N. B6 _
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   " M- M! h/ o, I2 M1 I/ @4 y- V
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    5 O2 ?1 A$ f4 U$ A6 C+ z% y' C( V0 _
12. #define Error1 1
    
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    ; g0 g& V& I1 C5 o
20. uint8_t sData;
    . G3 s" C3 O9 Z+ _: n
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    5 K& [& R* R! Z# I
24. uint8_t D4;
    
25. uint8_t D5;
    7 d5 h) S3 v9 g; |; Z$ x) i2 z: E* j
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    3 w  S! |" ?* K& E
31. int  Speed  = 160;
    % l  [% U; e' v
32. float output;
    
33. 
    ( Q; T+ b3 _; I2 P, Y8 [- p
34. byte Left;
    
35. 
    ' \4 R% i4 [' s- x: K/ I+ p! I
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    9 W; p+ n# @6 U1 Q, Q0 @  E
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    - _% R; Z: s  g5 Z( j% i" ~  K8 n
45. }
    
46. 
    4 ~& B4 W  }" [+ C4 y
47. void loop()
    % ~% b8 E: R' v% u
48. {
    0 G; G  }( \" f2 H# I# G
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    ! B; t/ m* J5 n' j
51.   delay(500);
    
52.   do
    9 l! p% Q& ?/ D: I
53.   {
    % O9 Q& z3 i& }( w6 I
54.    int Err = getErr();
    9 v9 ]1 m% j+ i: |0 X) Q# W8 Q
55.    if(D1 == 1)
    
56.    {
    # d# x! e  W3 Y: @) y
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    4 S9 O4 h3 U! z0 p. c! Y) o
64.    {
    
65.      if(Left == 1)
    
66.      {
    - P( L- e! W2 ?7 f
67.        Speed -= 5;
    + b, K, P: C  |- N  q
68.        moto(0,Speed);
    ' c" z* r1 G0 m5 V
69.        do
    
70.        {
    
71.          Err = getErr();
    " e/ f! M7 @# s. Q0 z7 N
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    * x2 V( n" n) R# [
75.      {
    $ ^7 J. Y8 k" t. P# X7 g2 O
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    , ]# E4 R3 A* k5 r1 `0 t' P
79.        {
    $ U' e; x' `0 C' x
80.          Err = getErr();
    + t4 V4 _% V' K
81.        }while((D1+D6) == 0);
    
82.      }
    . k3 {  G( `8 E; G  b3 O4 n" ?
83.    }
    
84.    else
    ' q+ ^+ Q6 c' i. r8 h% c/ ~0 Z4 |" @
85.    {
    ( R& R# K7 R6 l3 J6 s0 V
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
      P- @  c: x2 T2 c7 e
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    - V7 q' N3 e4 i8 }
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    : f0 [! u$ A5 p# ^* J
94.   }while(!((lightsensor_2.read()) < (10)));
    9 }3 N& v9 U. D2 f2 J# x
95.   moto(0,0);
    
96.   delay(500);
    0 f9 m' @5 G! e: w/ m& L6 c& G
97.   buzzer.tone(262, 500);
    ' i6 G! [  Q1 E) J; K) k
98. }
    & i3 P+ P& s. N$ I) J* l2 e
99. 
    
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     % O: Y) R: s  {4 [0 t' i
105.    D3 = ~(sData>>2)&1;
     : K' F/ [' c% f0 [9 E( s1 J
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     5 a2 R* P! j# Y, D
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     ( d6 T( [( V5 U' ^& F1 B/ c
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     
114.    }
     ( [" x* Q" T( o6 m9 s
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     ; E* @1 Z- O9 O0 J- y" l$ N
119. }
     
120. 
     4 }6 U+ ~9 m8 L8 ?/ s. [; Z
121. void moto(int Speed_L,int Speed_R)
     
122. {
     $ ]; M' U+ M% j: R  \/ O3 D
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

6 T! a3 j9 _' q" P$ Y# oMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   & n0 I  ?5 `0 e
2. 
   0 C" O# q3 p/ {. m. @
3. #ifdef ME_PORT_DEFINED
   + ?' D( U3 t+ y$ I
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   7 q. K0 i6 @3 |8 G; M
5. {
     M. j3 Y# F; y9 \
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   % |! P  M. w5 m, v
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    ( W4 N# A$ E9 r6 j; ]' U) O1 A8 R* E
12.     digitalWrite(_DataPin, HIGH);
    - V  X7 ^' X; @+ `1 c
13. }
    2 l- p; w: F* S1 j# X0 t; e4 |' f
14. #else // ME_PORT_DEFINED
    1 G( a/ L1 |( b. L* G
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    . v& [+ K& O# q% F, k! l( s3 a
18.     pinMode(_DataPin, OUTPUT);
      v$ A% p1 O3 I; f) K: ?7 B0 L
19.     digitalWrite(_DataPin, HIGH);
    $ K% y* F6 c2 |- y0 i2 _; v7 \
20. }
    " x2 w0 S( @5 t2 Z0 o
21. #endif // ME_PORT_DEFINED
    6 K+ `& l# O/ c% N4 \& A
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    7 f1 z3 `3 k# ^, p* z
25. {
    8 H. D* S5 e* X7 i$ N
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    + F4 F% Y! S3 h& ^9 X
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    9 n; r3 n5 R( N" Q# ^
32.     #endif
    / s5 i, k- H5 Q
33. }
    * y, h. q, F2 y/ {3 m, j: c
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    " r5 N) m! t, q/ a5 M+ G% W
36. {
    ; \  s; b) d* A* W- q+ P/ |
37.     uint32_t LOW_level_read_time;
    ' ^! `, T- V/ X3 Y, V" z; d
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    , k0 r9 |) K" l7 `2 Z5 ?
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    6 r( x! ?" y4 I% Y
42. 
    9 o! `6 R1 J, p* N( X: ?# ^" f
43.     pinMode(_DataPin, OUTPUT);
    0 x7 }' W+ k. |
44.     digitalWrite(_DataPin, LOW);
    0 _- B; u* H& B
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    & G+ c" W" i9 Z' \2 b4 T
47. 
    3 g- m' f% e  g$ J% D
48.     pinMode(_DataPin, INPUT_PULLUP);
    . O# g. R" Y$ Y; D, K& I  C) a' J! Q
49.     delayMicroseconds(10);
    + b5 j# H. p2 L( R1 H7 F' ^
50. 
    - A  a5 y* Q/ q+ A0 I# r- }# X
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    - q' T0 d8 J, {
53. 
    : l+ A+ [% {' s: P% F; g; v
54.     LOW_level_read_time = micros();
    % K8 f( B6 O7 G$ X
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    0 P& a* e5 N/ ?
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    1 B$ W2 R7 N# E( t+ a5 U. x4 t
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ) n3 U( ]- m7 i" q& G
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    # W1 {- y  l0 K+ u" E6 `, G
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    % u8 ?+ a! p, z
66.     {
    
67.         return 0xff;
    
68.     }
    
69. 
    8 |2 w% V; |1 `& G/ H" k& d% m
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    2 x* u& h2 L- R( ?9 ~) l9 g
71.     {
    
72.         return 0xff;
    
73.     }
    ( [0 E+ G; P. V& X3 V
74. 
    8 n3 ?8 t2 J% y: e" r0 O, r
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    5 D; `/ b5 K& Y4 @7 {3 y
76.     LOW_level_read_time  = micros();
    % _% }; u: h+ r
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    ' T0 k9 s0 k: h- _0 D# e& Q
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    3 m; x) j9 N! `: u# p( P) y. h, T
80.     {
    
81.         return 0xff;
    * F( L8 b6 G5 S. r, W
82.     }
    # [7 I; o% {/ H" h- ~2 Z
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    3 F% E5 E; \( X* p" R; y
85.     {
    - V/ x( a  m! u9 Q- b  {
86.         return 0xff;
    + h& T! g( R- ~" `6 {
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    5 M, ~' I. r  S+ X* r
92. 
    ! w+ u( W  d- J+ d7 m
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    ( u$ G* b1 H5 ]+ [3 t: N- i; ?
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    ! ^/ l' K. t9 z6 z3 |9 @
96.             HIGH_level_read_time = micros();
      ^5 B+ ~" ]' T- n. q/ ?  Y0 K
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    & {& G* _* o6 l5 V( j
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ; S- v9 d: E' C. b, u& _+ g
100.             {
     
101.                 return 0xff;
     # Y/ R3 W! @% T) L- U4 ~
102.             }
     # m5 |; L. x3 R+ ]+ A. d* d# m
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     2 `2 I) n& ~9 M+ T; r& b8 p
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     " n! k* k, ^' ^, e; d
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     9 ?* T( J+ s0 |% o6 y6 t
109.             {
     4 G% i# `2 V1 [3 X1 O2 J
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     + m$ @' f5 m( s# {* `
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     $ ^' w) w- A+ R+ E6 c: _' b: l, h
114.                 return 0xff;
     
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     2 c, i/ w1 g( b; \3 J1 v5 H
118.             {
     
119.                 return 0xff;
     : W& D* a6 z) P2 G& S2 P1 Q1 B/ E
120.             }
     
121.         }
     
122.     }
     * d8 c( w- u8 i& H1 n
123. 
     . l$ H/ r1 T- N! K8 k
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     2 D* L1 p5 _, ~
125.     HIGH_level_read_time = micros();
     2 O! E+ t: V# W" s
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     + E9 X: W8 `# i* `" W2 l
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
       s0 _% e' R2 ]0 G
130.         return 0xff;
     
131.     }
     
132. 
     5 \3 Q% P- l% F
133.     pinMode(_DataPin, OUTPUT);
     & b. }9 K' I8 ^/ s: u" i' j' \8 z
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     ' V. F  `7 x( w; l  v
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.     }
     + }/ z' n5 G! o( H, P
141.     else
     
142.     {
     9 a. m, [8 |/ Z6 I! i
143.         return old_data;
     ( U3 w" Q1 G8 I% e/ T$ z9 k* h
144.     }
     * y3 e  V0 i7 u6 b2 z, K
145. }
     3 g! A( {. y7 f1 }/ P( o

複製代碼

6 v+ r4 e, J4 a- N1 ]; V: EMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
4 ?2 \2 ~" n1 A/ F; M哇...要100 個銅錢

# v+ i& W- Y( P+ k7 J認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

8 \8 x2 J) m1 u, }0 U* LMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手