mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   1 q  A0 K0 x# H; A4 X5 y# F
2. #include <MeAuriga.h>
   , G2 K) v0 v8 V3 V/ X' w
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   * k: r7 p) N  {( `% Z" F
7. MeLightSensor lightsensor_1(12);
   $ ]: a/ i" \, o
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   4 d6 y/ K8 _3 E; p
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    
12. #define Error1 1
    . V' H6 {- F  r, _; ?5 O
13. #define Error2 2
    
14. #define Error3 3
    $ c  j) v8 V$ N- l( c3 w& z* s0 ~
15. 
    . ~8 \8 n! O# \9 x6 ?: Y
16. #define Kp 15
    . o) l0 F& M" y& I/ [% A3 [
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    6 K. U. ~7 w: B! f
21. uint8_t D1;
    
22. uint8_t D2;
    ! b! z3 A: s: u% n9 q0 F. u
23. uint8_t D3;
    9 q9 Y6 ?6 t/ C; V
24. uint8_t D4;
    
25. uint8_t D5;
    , w* t* z, t  D, }+ x7 H
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    + a5 C: \7 w( d  t3 w9 w! \4 m6 g
29. float integral = 0;
    
30. float derivative = 0;
    1 u; j* S9 `5 Y5 z
31. int  Speed  = 160;
    
32. float output;
    1 J% w0 I$ j& }5 N$ \
33. 
    5 n8 e2 F/ s5 S" q, X: I" Z
34. byte Left;
    9 E8 F/ H& `+ m, T$ j  K
35. 
    4 }: \4 b9 Y% S; m+ Q* A
36. void setup()
    
37. {
    ; Q$ _5 q/ d/ S
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    % Y5 P  R) h) a
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    
46. 
    8 c; Q$ @1 F4 @! @3 ^; G
47. void loop()
    
48. {
    $ d' Y) X( j" {" `: F) u& o- f
49.   while(!((lightsensor_1.read()) < (10)));
    8 h  K& D' F0 C) l3 z1 w
50.   buzzer.tone(1047, 500);
    ' W. X3 h/ O' }) ~# h7 [
51.   delay(500);
    1 q) y* t: O1 k0 y7 x: n7 n& r
52.   do
    * `9 ^0 C! V4 o8 q1 e# V- f& K( [
53.   {
    3 @6 N$ {5 F. x& m5 D
54.    int Err = getErr();
    4 J7 z1 K; B2 ?: a! l) j
55.    if(D1 == 1)
      K4 O2 E7 i/ Y
56.    {
    6 f& c! @' i1 E# A8 g& O
57.      Left = 1;
    2 i: U: g& G7 H% a# A
58.    }
    
59.    if(D6 == 1)
    4 @6 [* W  p6 o$ [2 f$ o
60.    {
    , x" i8 ~" q6 N9 t
61.      Left = 0;
    ' @5 D. M  {2 m" E0 q, F# U: E
62.    }
    
63.    if(Err == 99)
    & F- V0 g5 W/ H& [8 R
64.    {
    / y+ d# H) D9 I$ M7 i  {  G0 ]3 V9 B
65.      if(Left == 1)
    3 b' y) k1 K# E3 O$ e
66.      {
      P  a4 y: d+ o
67.        Speed -= 5;
    ! y6 ]- X' \4 H# x' ?: a
68.        moto(0,Speed);
    
69.        do
    ' Y  V6 w; [$ I$ T
70.        {
    * @. G* |7 b1 f: p, E
71.          Err = getErr();
    ' y2 ?5 C- ~2 E
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    ( N( E% F) m% t- S  a$ _  \
79.        {
    
80.          Err = getErr();
    - J2 L" y: K: M; B5 I! @. F, {9 u
81.        }while((D1+D6) == 0);
    
82.      }
    7 z3 _) z  q! n# W5 M9 A
83.    }
    
84.    else
    0 e% N. P; _( H
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    % h+ ~/ k- a" z6 z" X  [8 U, Z
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    + i# H- E7 p1 U: }$ i
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    0 X% j  H  k) F$ j
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    ; ~+ u7 f1 g7 P1 ^+ O9 _) }0 U* d
92.      previous_error = Err;
    . K* S+ @4 i1 f
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
      E* [: V" w. O+ r
95.   moto(0,0);
    
96.   delay(500);
    ' M- X) C" x" G" A
97.   buzzer.tone(262, 500);
    - U' Y' T" j% G. [. u
98. }
    
99. 
    
100. int getErr()
     
101. {  
       d! J, d- i. s+ {( W$ M7 Z5 B
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     $ {& ?# E5 B% f. z# _: K
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     : G+ j5 w5 P6 l, u" C" ?
107.    D5 = ~(sData>>4)&1;
     
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);
     ( M+ I& h5 H4 w% y: E0 ^$ M
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     
114.    }
     5 {: Z# J" m3 V, U: k% N
115.    else
     * \/ ]& n+ v) M6 I7 p4 Q
116.    {
       n* H4 c$ j, j6 n# |8 \
117.     return upD/downD;
     
118.    }
     
119. }
     ; Q) ^. H0 R0 R5 p2 Q+ ^
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     6 e( U+ Y8 ~1 Y1 [6 p! X1 t3 Q
124.   Encoder_1.setMotorPwm(-Speed_R);
     , G& y, G3 s! B7 o4 c
125. }

複製代碼

0 \+ d# y% v, d% o0 l" eMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   $ j& }6 ?7 X& S" f! O# @7 _$ s9 ?
2. 
   
3. #ifdef ME_PORT_DEFINED
   . T) e: [0 y! ^7 Y, Y& b. g  [
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   7 m* @" A- b5 s6 X' ~/ y* B
6. 
   
7. }
   5 @4 t4 y$ D+ z' o/ l! k  L
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   - O* Z" V4 h! E- e) d' e
10.     _DataPin = mePort[port].s2;
    - f* r0 s8 ^- l1 g0 e
11.     pinMode(_DataPin, OUTPUT);
      H) ]3 q) V. e+ y  E5 Y
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    : h7 g9 m; q  l1 g, ~. s% i& l
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    8 d3 L6 [% N5 ?4 k0 c
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    7 j# N0 a/ Q* U' x7 q& E* ~
21. #endif // ME_PORT_DEFINED
    * o) H/ j) [; g  I1 g3 X; N
22. 
    
23. 
    * H: L5 H" z0 U9 w$ s9 K
24. void MeLineFollowerArray::setpin(uint8_t pin)
    8 K. n  p. p3 j' [! I# \
25. {
    
26.     _DataPin = pin;
    6 \0 a7 H4 F3 w9 {, \" m
27.     pinMode(_DataPin, OUTPUT);
    / a! b+ w7 o3 [5 U
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    9 a! W2 i' x: t9 n
32.     #endif
    
33. }
    5 y9 C+ k1 n& f
34. 
      @" |; m9 H! y/ G$ t: q) g  v
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    ( M5 Y; d, C2 E* L
37.     uint32_t LOW_level_read_time;
    1 B. R; |! u7 k0 L( v$ I# r
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    3 M2 T% H* ?6 {% d# `6 s
40.     uint8_t Sensor_Data[3];
    % Y9 W  Y7 Z# D8 q7 L
41.     static uint8_t old_data = 0xff;
    
42. 
    6 u* f# S3 m2 ?; C3 H
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    - ?) z: c, B) Y5 Z1 g' L: \, g
46.     digitalWrite(_DataPin, HIGH);
    # X2 d4 r0 `. }/ e! s
47. 
    6 s; G  [' s7 B" c! @
48.     pinMode(_DataPin, INPUT_PULLUP);
    ! t+ J% w9 n9 c: ^
49.     delayMicroseconds(10);
    $ M& J4 t8 h6 [5 g" H8 b8 X7 V
50. 
    / W9 Y; H2 k% o! ^# i
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    2 [4 {' w8 q( ~9 \/ H
53. 
    
54.     LOW_level_read_time = micros();
    - M' @% h' a  ?8 r7 v8 X% [; |
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
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();
    * f% x! {! v& `( z- r/ K
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ) M0 q  c  U; a& W" C* F* _, A
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    9 A4 I3 i! H% D1 V% y6 N
66.     {
    0 Q% `# A5 @& c' B1 n+ c! t" u( [
67.         return 0xff;
    
68.     }
    3 B4 u5 ]& K! Y/ u; r- f
69. 
    1 Y* n$ Q3 H0 q, @  r1 E
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    + A: b$ c5 H( r
73.     }
    
74. 
    - d# M* k) n" m4 N/ S. u6 x( W+ F
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    6 h  ]' d+ e  L7 o0 P5 U- I5 n
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    " C! g' s3 S( G1 Q3 `
80.     {
    3 c2 }5 ]# I& W  w
81.         return 0xff;
    % L0 h* o0 n4 @/ H6 I( x1 \- m
82.     }
    % K+ I/ `0 |) {) M
83. 
    1 u& E2 g- q, B6 i$ u2 p% |8 U9 a
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    + {) J9 ]9 N9 T$ R+ m" k
85.     {
    
86.         return 0xff;
    ( Z- o6 u8 I. h8 D. e8 q- V
87.     }
    2 m& [4 o3 r5 b8 f
88. 
    0 A, g& F5 s3 N% Y+ B- D& w; v
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    ; _- b/ h7 \% o+ Q) u" w3 Q! w7 }
91.         Sensor_Data[k] = 0x00;
    7 t5 F8 U$ c& O% c- k1 M" |
92. 
    $ Y6 p3 \0 J1 s4 [
93.         for(uint8_t i=0;i<8;i++)
      q5 w7 i* }! X: l/ ?
94.         {
    9 D2 Q: Y, z& [
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    0 [  `/ o$ e% `/ V: _
96.             HIGH_level_read_time = micros();
    8 ~/ c5 h  W  e; z, _5 }
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;
     
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     8 o# j+ V1 v" X/ n
105.             LOW_level_read_time  = micros();
     0 W0 ?8 r% E. X9 H- L$ U
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     ; n+ d2 ?/ q  i, ~$ V
107. 
     5 i/ b' i# v; p
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     * f. S4 e2 i& D7 G( F' K
112.             else if(HIGH_level_read_time >= 100)
     & X4 F" v: C) }8 C! F4 [/ f  R. L8 @
113.             {
     & O" m: a) p7 q" }5 L2 O) E- S5 V
114.                 return 0xff;
     5 M1 L# ]1 ~, Q- U; J8 `7 z
115.             }
     . D. Q6 L1 e. r! }
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     5 ?$ o5 {; u3 S7 o2 s
118.             {
     & I4 S! F0 `* R
119.                 return 0xff;
     ) c, v  o6 J# @0 K( n5 M2 _
120.             }
     ' E" t. h8 n5 j6 O! p5 Y' b* U
121.         }
     7 \- o2 l( ^4 f& R
122.     }
     , q$ Q7 S0 m& k( X: L, c* l
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     * e8 G/ g( f' W: O9 `$ D0 I4 Z: C# d
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) )
     2 |* O7 {+ _# p
129.     {
     4 i" T% x& N; v4 r9 A: }; h
130.         return 0xff;
     
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     1 _7 L8 p: E2 C) {: o6 a5 ?. D' {
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.     {
     ; q: @: r/ x; ]$ {) p
138.         old_data = Sensor_Data[0];
     + k: a8 I9 y9 [, K- y! [0 u4 z' p, t
139.         return Sensor_Data[0];
     : _( U: }& y" p% ^& W* C3 O
140.     }
     # s  E4 M7 h- ]7 I9 [
141.     else
     3 k) ^7 Z( l: o, F0 C3 A; N6 Q
142.     {
     
143.         return old_data;
     
144.     }
     
145. }
     ' ]  ^# g' R. n9 w; |0 z

複製代碼

MeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

2 l& A: Y) n$ p: Z! |* V
0 F/ I' b; Q( O

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
6 O* B( `6 r  f' _4 n哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手