mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   6 Y7 c3 @+ d, ]: Z
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   ' Q$ s  W/ b. X4 f. ~; _& f; _
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   6 I5 K$ `  M9 y2 W8 [+ w
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    $ b8 U6 A5 u+ ^$ A  O! z
11. 
    . C  j2 i2 i* j! v# G& K% H
12. #define Error1 1
    
13. #define Error2 2
    ; a: H' P) n9 q4 f! e4 V- c0 @9 S
14. #define Error3 3
    7 F& k) @5 }, Q$ w0 m6 a
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    ! k3 v/ z+ @! S  t9 s* m. ~5 J* K& b
19. 
    $ {5 W% W& {0 J9 B; D* \
20. uint8_t sData;
    
21. uint8_t D1;
    0 t. W' O& a1 N( z! z1 E2 f# ~
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    ! a) \  k5 l2 l4 r& \2 I% C: h
25. uint8_t D5;
    1 L3 Z5 n+ G, A6 _3 i9 w
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    ( m% w8 ?# e' I1 s" T( p
33. 
    5 R. T, q- A' q0 g/ H& G
34. byte Left;
    
35. 
    + m3 A) J! r# c' c: w
36. void setup()
    2 m2 l: `# T' ~/ }
37. {
    
38. //Set PWM 8KHz
    " O, ~* ]/ ^- Y! t6 ]# W
39.   TCCR1A = _BV(WGM10);
    , G3 Z7 {, ]+ [7 b8 y  ^) }
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    , j% W% @+ ?* Z" C& H  L$ H
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    3 s! I- h. h# N& J- g
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    ( H' v1 I* j  ^  y- {2 }' U+ E
46. 
    + {1 p3 s8 L3 q
47. void loop()
      [+ c) e( s$ \/ {; N
48. {
    ( s( F1 M2 D. W; D& r% Y
49.   while(!((lightsensor_1.read()) < (10)));
    & z* k' N0 N5 }0 v& U1 Y2 F
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    ! s! O- l& Z. o5 ^% {, x/ b" e3 P
52.   do
    
53.   {
    
54.    int Err = getErr();
    - f# Z) W. C' k! w+ c5 {( L
55.    if(D1 == 1)
    
56.    {
    2 _# N; X# {1 R* b: R$ i
57.      Left = 1;
    
58.    }
    5 w" x2 T6 e6 P$ B. p: ?( i( o
59.    if(D6 == 1)
    + x- i: ^: d  m. a7 [' j! A
60.    {
    
61.      Left = 0;
    ) Q- u, H; y; W' G
62.    }
    & M* S& x2 V  {) z4 G  Q) C
63.    if(Err == 99)
    % z0 D8 w, Y0 V
64.    {
    + o) W4 h/ z0 H( ~2 B
65.      if(Left == 1)
    / e; \/ J0 G) d- p8 e. i
66.      {
    " C- L* K6 ?# R9 v: m/ B
67.        Speed -= 5;
      A, ~8 ]9 c$ R. J# i- M9 k
68.        moto(0,Speed);
    * _" W  g5 _# o' q# w: D7 y
69.        do
    
70.        {
    5 \: \' J0 V0 e
71.          Err = getErr();
    $ w* }; b9 }, U. r, E
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    4 }/ B1 k5 I" u2 I8 @
75.      {
    6 M! I3 }; Z, C8 ?" r
76.        Speed -= 5;
    
77.        moto(Speed,0);
    * Z' C+ _3 K! |1 W/ P
78.        do
    7 T/ h! {4 i$ u, |7 X2 s4 i
79.        {
    & `/ U3 w2 x4 s' C# m  `
80.          Err = getErr();
    + k; b- }+ Q9 c
81.        }while((D1+D6) == 0);
    # r5 g: H# E& z% u, d% U
82.      }
    
83.    }
    7 r& g8 D- u3 I
84.    else
    ) V2 f8 r) ^3 S2 z7 t! p% p2 S
85.    {
      r$ u. W  s3 S7 u3 B) t
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;
    4 x9 w9 t, ?' f& W8 V* ?
90.      output = Kp*Err + Ki*integral + Kd*derivative;
      b( {$ g4 m6 V8 q4 z4 n
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    - g+ w2 Y3 f2 K) A% D! E( W* B
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    5 U2 n" s& a! W; J/ Q$ Y5 P
98. }
    " s  f# A: ~; T  R/ C8 o% J- i/ v* H1 p
99. 
    2 q1 W/ o7 D. ~4 Y
100. int getErr()
     $ m+ H: m. Z% t: M
101. {  
     4 z) M' G' o7 D  x! u
102.    sData = linefollower.getValue();
     . j$ S; D6 P6 U, T8 `
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     : K& Z  p% D2 H6 C. a
105.    D3 = ~(sData>>2)&1;
     6 u% a# w8 z* W* i9 u
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     - y. g" R# S5 c* C: a4 c" j
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     2 @. O3 `& ^5 M- O
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     2 N& G- L3 n0 p+ }2 Y- q' R
114.    }
     + Y$ A. f9 _2 |4 G. l- f/ S/ u3 l
115.    else
     
116.    {
     
117.     return upD/downD;
     8 W8 `. f5 ~6 @5 @
118.    }
     
119. }
     ( F4 D- l7 a/ c% s+ N0 W. k
120. 
     , c9 t; I; f) x8 @! w
121. void moto(int Speed_L,int Speed_R)
     # F- s( g" L$ Y3 }, z7 ^
122. {
     $ N$ z: c8 ?. G: Y$ o( }5 {! R
123.   Encoder_2.setMotorPwm(Speed_L);
     6 p7 U; }5 I  L. d3 t; y# K3 a- F
124.   Encoder_1.setMotorPwm(-Speed_R);
       V( j4 n/ G% u, _( m* J& P
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   1 v. l% ?# I" K
3. #ifdef ME_PORT_DEFINED
   8 l7 ^& N; a/ c- W0 O* [$ R
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
     C! A. h7 N2 c1 [# x& Y
5. {
   
6. 
   6 y) ?6 S/ s# c. F3 v. C6 e. B6 C
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   4 d3 E* u& j" L* e) g3 ]( J0 x
9. {
   7 b' o1 [( i$ g! m+ t
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    # ?- I2 f; p! C  g  k4 |! `$ C- ?7 n" R# w& W
12.     digitalWrite(_DataPin, HIGH);
    # S$ S. P1 J; y. Q* T; e% h
13. }
    ; p, r; W- k" c5 q+ d
14. #else // ME_PORT_DEFINED
    6 D+ f% J* g5 i
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    " D, w" t7 \$ a) o0 u/ Z$ O
17.     _DataPin = pin;
    * ]2 _: U( R! Y  D. M, u1 P1 I
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    9 ^( l! x6 z: L  G6 J1 S
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    + F' F! t& H& R! g
24. void MeLineFollowerArray::setpin(uint8_t pin)
    + L4 W9 G# g2 S! ]
25. {
    
26.     _DataPin = pin;
    % q6 W$ ~: F& e9 ]' F
27.     pinMode(_DataPin, OUTPUT);
    . k% b1 r/ r$ q! U' d+ a, X( `
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    3 l& u( \4 [0 h$ Z1 k
30.     #ifdef ME_PORT_DEFINED
      W% u# K: \5 D' ?3 ~  M
31.     s2 = pin;
    
32.     #endif
    
33. }
    
34. 
    & H, l  q# v/ w0 E
35. uint8_t MeLineFollowerArray::getValue()
    7 x$ h" m8 Q. T7 X
36. {
    4 Y) W6 S( M  ^
37.     uint32_t LOW_level_read_time;
    0 T8 b  O1 u- t0 g
38.     uint32_t HIGH_level_read_time;
    8 J! V! W# j! @
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    - i9 y( v' M( Z3 G* w. X1 A
42. 
    ' {: x: F7 m; C# g
43.     pinMode(_DataPin, OUTPUT);
    : f' s, V- @" v
44.     digitalWrite(_DataPin, LOW);
    0 j) m6 H* M) ?2 W9 f
45.     delayMicroseconds(980);
    3 O  U6 z! P; o4 _
46.     digitalWrite(_DataPin, HIGH);
    # d" V4 u& m8 s8 [: h. u7 i3 O
47. 
    * g  x3 C- D% q
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    0 H+ L; X7 G. Z( U" a! a8 W
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    3 H, B; c+ Y6 y$ ]* `) ?
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    " d8 @& e$ y8 `& _0 Y3 k1 Q
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    / x$ m' o7 ]6 M
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    : {9 t0 R2 b4 {2 C' j, }' H
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
      [6 N1 p, w! S9 v0 r
66.     {
    ! x/ s2 ?8 O4 v
67.         return 0xff;
    % s6 ]4 X6 d# n+ {0 x
68.     }
    7 Y/ g/ w3 q+ s+ B
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    9 N: O0 k0 {. |! |
72.         return 0xff;
    2 b% Y$ F3 Y; v) o
73.     }
    + }# f+ ^+ T/ _: A6 h1 v
74. 
    1 l0 e9 _" L" |2 U
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    ) F9 ?  p  |* k3 `6 s$ G! G
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    : l2 b& w2 P8 U! l' Y6 w0 D) g. w
83. 
    * F- ]4 D  `* s8 n
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    * \" V4 L0 F. w6 n
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    ) T/ G, c9 z4 B5 G0 |
89.     for(uint8_t k=0; k<3; k++)
    0 _1 K0 ]4 s+ P% U  `6 ^1 p
90.     {
    : X2 x$ [# g+ b, b  K) M
91.         Sensor_Data[k] = 0x00;
    ' z7 [0 e: a9 O$ n$ {
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    5 U: H) E4 ^4 D+ m3 t3 \3 n8 ]
94.         {
    & Y5 J. P5 R) O
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    ' w- U* P; u8 R2 Y8 c* \% j' p
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    ; ~0 w1 v8 t) j' J
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    9 O$ F" {5 i  [( T3 _8 L
100.             {
     6 W& n# h3 {) [
101.                 return 0xff;
     
102.             }
     2 S' `1 J1 h: H9 M- m* E
103. 
     " r' h/ b. N& L
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
       {2 w5 I' j. F6 A! t$ }
105.             LOW_level_read_time  = micros();
     0 B: b( n" j# E% @* H" E" h
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     1 {- s: ~) g1 C# i2 f# ?, |
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     . D" z9 n& Z; P5 q6 r' j. O
109.             {
     - y$ S: \# e4 b7 V, b4 M" v" M+ ~
110.                 Sensor_Data[k] |= (0x80 >> i);
     & t1 p, |( W$ `6 r& }
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     4 {' l1 N; j" T3 |. c
114.                 return 0xff;
     , {$ x/ H0 U* L6 j
115.             }
     
116. 
     0 ], |9 W; \4 t: n
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     7 c# t" C9 }2 r& t* {; u9 d
118.             {
     + {3 ?/ ~8 f3 `
119.                 return 0xff;
     
120.             }
     
121.         }
     
122.     }
     
123. 
     
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;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     6 D' ^# }0 z8 }0 F9 `1 `3 \
132. 
     % h/ [* q# j* w
133.     pinMode(_DataPin, OUTPUT);
     ) |& B7 S2 Y2 G6 e+ Z
134.     digitalWrite(_DataPin, HIGH);
     , l9 S: A4 V" V1 q# E6 E* w/ \+ s
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     , c, @8 t% k* J% u$ \% I
137.     {
     4 `# I/ ~, u! m
138.         old_data = Sensor_Data[0];
     6 _8 x2 A; V1 E4 X
139.         return Sensor_Data[0];
     8 ^# n0 n& i; X) F' L; g
140.     }
     / O: {; i& R, Q( Z0 X5 R6 S: R
141.     else
     
142.     {
     # Z$ i4 o* |( r
143.         return old_data;
     1 B/ p' f* w( h% \' s7 w' W
144.     }
     
145. }
     5 S! }0 e  l+ ?9 j8 o1 z

複製代碼

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

2018-2-13 01:52 上傳

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

* V8 o* S  C, a

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
3 N# D0 s+ U" F% X" @2 O' F9 x) b哇...要100 個銅錢

5 a9 p- X, W. x# }' w% ]5 W( C認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手