mBot Ranger 高速循線範例＿

: A, t" E% `. O& Q

1. #include <Arduino.h>
   9 i. r; b6 \( W
2. #include <MeAuriga.h>
   # u, ?. ~8 `: C% a+ f3 C
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
     M) ]7 G5 a8 n9 f% R
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   7 r6 b8 r% Q0 x! o7 c3 Z7 O6 q
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    & g* ?6 f2 R( ~7 B# {
12. #define Error1 1
    " H5 f* Q3 ?2 n3 U. ?
13. #define Error2 2
    7 B: S- I: Z) Q" `
14. #define Error3 3
    : X4 G# F( @0 _5 d
15. 
    9 |# k$ ?9 |4 o1 }3 t) o. Y
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    + n  I/ J2 a, M% |
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    6 d+ t6 y0 u# t
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    7 n2 E' ?/ E! V3 x  T6 m1 u
29. float integral = 0;
    
30. float derivative = 0;
    + ~; N# P7 d' a) z9 j( P
31. int  Speed  = 160;
    ; G6 |/ s) L6 T# f4 \3 v  U
32. float output;
    % ~. i2 `$ \* Z
33. 
    " N$ {4 Y  O8 N4 J  |
34. byte Left;
    4 q9 g, w4 L: m" h& T
35. 
    # G% F2 Q! t: ~+ F( i
36. void setup()
    
37. {
    & S5 x/ {0 }: m; v; n  |8 `& h
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    4 S% I! `3 S1 Q8 \
43.   Serial.begin(9600);
    7 O3 e5 r# k) @7 ^0 l5 w, h
44.   buzzer.setpin(45);
    ) L$ F$ l; O1 q# Q
45. }
    0 n5 m0 Q) J7 T( H! x% G
46. 
    * R, P9 T3 A( H) d
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    ; t& A. D3 o4 a& O5 u
52.   do
    ' ]& h/ q6 D, z1 X4 W
53.   {
    ! f: w' h  f' w7 c; J
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    9 c4 S# H3 M9 y+ O1 s5 h, N
60.    {
    
61.      Left = 0;
      E9 x; J+ O$ F6 {4 D* U1 |6 R
62.    }
    $ j* T  l  I* U6 \
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
      l3 L, P7 O% Y2 ]3 V0 l
66.      {
    & p4 j" a1 G( x
67.        Speed -= 5;
    
68.        moto(0,Speed);
    * D6 Z9 R. u. J1 w) H5 {. t
69.        do
    $ `  s) J4 D1 ]7 {
70.        {
    # @, y7 U& c# R2 p( m
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    # r9 a" Q, j4 v; G
79.        {
    0 B  _/ C  w, \3 U2 t) B
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    , O; f! I2 Y, L- i+ S$ N* \
82.      }
    ' r& S: ?) I$ ?; {
83.    }
    ) w3 Z1 D" `" d. ]
84.    else
    + p9 o! }* g, |% {
85.    {
    & J/ e% b- q9 K& S7 M* C) |
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    % a# c: ]  j& P% b- s- F/ X8 v8 M
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    . N" h$ ~5 q& q  i
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    - t$ t6 t8 r. D7 S& J- V; y
91.      moto(int(Speed-output),int(Speed+output));
    $ u) J) @8 c; M
92.      previous_error = Err;
    
93.    }
    + h  e& R* Q: a- T6 R
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    7 T/ Z& |; d# r
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    , Q) [+ i! N  b
99. 
    
100. int getErr()
     $ X' U) v2 d7 b
101. {  
     
102.    sData = linefollower.getValue();
     2 A; E: P! [+ q
103.    D1 = ~(sData>>0)&1;
     # T2 l1 i% O& m) n/ ?' q
104.    D2 = ~(sData>>1)&1;
     ; A1 W; N$ \. o2 g0 Y
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     * `5 P- c2 g. c6 u1 `& Q
107.    D5 = ~(sData>>4)&1;
     * E7 A$ W% a% a
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     8 q' j8 V0 z  H
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     . M/ f1 v8 P! N3 b
111.    if(downD == 0)
     
112.    {
     * a4 I9 [- J7 D
113.      return 99;
     ! v8 S7 e3 Z$ b8 U: h
114.    }
     
115.    else
     
116.    {
     ' F# i9 B: E, N" J
117.     return upD/downD;
     - C$ f1 x' m$ C- W9 ]# k# r' ~
118.    }
     ' H. p: a8 y! ^" T4 V
119. }
     - }( n/ A  r. r8 j1 s: C/ e4 |
120. 
     5 e) |4 W' o$ @' T2 }# d# D
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
       |' A7 `5 S# H4 A7 e6 v, ?% x; e' M
124.   Encoder_1.setMotorPwm(-Speed_R);
     8 w) E5 c/ {5 D+ @) L
125. }

複製代碼

& ~- z3 k# b5 f7 HMeLineFollowerArray.cpp
. u  f* ~8 j: j9 R7 @# L( A" g( X

1. #include "MeLineFollowerArray.h"
   
2. 
   $ z, d6 d" b# H" G" e( k5 n- @8 Z
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   
6. 
   # ]. f. \8 X! B  z" g. h( B# }
7. }
   6 ?: u( s! h$ A7 K0 y1 G
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    ; Y- ?6 U. i. J* k( K" A
11.     pinMode(_DataPin, OUTPUT);
    4 A6 X$ |7 a, [- j( y3 z: E: p
12.     digitalWrite(_DataPin, HIGH);
    
13. }
      L; x; R# S! p) s; C
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    + Y, g' Q2 p5 ~1 w; i+ H
16. {
    
17.     _DataPin = pin;
      w- ?' ]7 \6 C0 @8 S  l! v
18.     pinMode(_DataPin, OUTPUT);
      H7 ?: R3 G8 _% a
19.     digitalWrite(_DataPin, HIGH);
    / ]! y: W6 B6 b9 ^; A+ I
20. }
    
21. #endif // ME_PORT_DEFINED
    % R2 }1 T3 `' u( Q6 n8 l) s
22. 
      P, [0 N) z! x* m" i: |; e. M
23. 
    ) S6 }2 J* \8 i
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    0 A( g: e* T: k& w
26.     _DataPin = pin;
    ( E8 u0 E+ E# d" s' _8 \
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    / \7 R: T' Y4 X6 Z/ M
29. 
    7 O& |6 i) D! S3 r; g7 t0 b
30.     #ifdef ME_PORT_DEFINED
    + y- m6 g6 o* a5 q3 @
31.     s2 = pin;
    $ r3 J# d. `% X$ A- O" X! n
32.     #endif
    ) K! Y1 U& X) F# M6 S
33. }
    9 a+ _; H5 X' @/ d7 N/ N/ f
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    9 C- }$ ]  g: P- x6 T1 S8 Y) y
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    ' g% w/ R' [2 ]5 k- ~0 c
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    # p  s6 J: p7 Q0 g0 d) ?
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    ; ]8 j5 S7 u. C( {- f" c5 A( f
45.     delayMicroseconds(980);
    - f1 N; z1 v3 Z0 G: l
46.     digitalWrite(_DataPin, HIGH);
    9 x- u6 i* m# F+ M! B2 q( S3 w
47. 
    0 f! r4 R' v: @' H: w
48.     pinMode(_DataPin, INPUT_PULLUP);
    & Z" c/ A' w" C4 F
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    5 ]- m5 u0 s) {0 @
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    + u: c( k. S! N; A* P* e' k
53. 
    
54.     LOW_level_read_time = micros();
    2 {, J1 y9 X8 Z# V
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    0 e: F/ [$ w0 i
56.     {
    ( t, O) `* Q8 `* l* ^
57.         return 0xff;
    
58.     }
    
59. 
    , {2 P6 |1 b/ U& {1 G* R
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    / I- d+ `# Z* v/ [) [" Q
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    . K- s) u2 u1 j* a2 x1 N0 _
64. 
    - _& p9 H# h5 d, l# g- H
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    
68.     }
    " w: z$ P8 x, k% J& V1 x
69. 
    ! F% K4 G% z9 |/ H9 S9 }
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    1 `9 A1 e( R8 o' Y" ^
71.     {
    5 i7 H2 x- E0 j; Y3 ~% A6 ^
72.         return 0xff;
    
73.     }
    
74. 
    - N( s! n, |+ J
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ) y0 J1 o4 ~5 ^: i% l, b$ A
76.     LOW_level_read_time  = micros();
    6 w3 S! Y/ X7 E. R4 q: S
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    ' Q, M0 g2 W. E( t$ o$ i$ M
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    6 g( r% R; {/ @* |
82.     }
    8 U% L. }$ j* X2 X
83. 
    0 p+ ]2 ]5 ^5 J6 p- L
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    9 T+ K' g: ^3 G0 I$ @( N
85.     {
    , M& _6 {. W( C( z2 d: G
86.         return 0xff;
    ( k  s& G4 X- c- o- r
87.     }
    . Q$ J6 p; D3 K0 ]9 F& d7 V
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    $ Z, U7 Z  Q" V
90.     {
    # ?; y7 [9 k+ n) f: a
91.         Sensor_Data[k] = 0x00;
    0 ^! T& u! g( ?3 ~; h. j5 ~
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    / S+ A) C8 D. p* c, e
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    ' \9 i2 K& J& [3 \1 ~* m- O1 m
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    0 l  Z  t& W; {, r+ @1 o
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    8 p% _$ V6 P. B5 f
100.             {
     
101.                 return 0xff;
     8 v; H2 T( g  Z  b! D" l) ~$ g
102.             }
     
103. 
     ! q! Y; C5 n* H
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. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     8 F& j, H3 Y( _/ }& @* b8 D/ z
109.             {
     * G+ U# h% c" ^: j) m" A+ e
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     3 f6 V1 E: b1 m0 _0 o7 }
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     7 s5 L7 }) Y* L. D) P
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     0 _7 a* H; b* i4 {, U" T  u
119.                 return 0xff;
     / H+ C' z- A+ l0 O2 X  t5 y
120.             }
     
121.         }
     $ c* G( a7 S7 k  w8 p) Q2 h
122.     }
     
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     % m7 {2 {  M4 @
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     , Z1 ~  p3 n5 c8 P" f) Y3 z
129.     {
     
130.         return 0xff;
     
131.     }
     . _, Z* n( I4 d4 k  M1 ?% T2 e2 r
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     7 q% M. k7 w1 L  ]
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.     {
     1 t. N+ H( T- K8 F* }
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     ! a9 i3 }: d; a7 S& Y- A
140.     }
     
141.     else
     * x& b0 t3 x, ^( j! ^8 F6 B1 D
142.     {
     4 v( F5 J+ v" [" Q: v& S3 ^
143.         return old_data;
     
144.     }
     
145. }
     

複製代碼

' \- [1 S8 {! B' u& ^5 WMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
9 x3 T6 V) U# v$ C能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手