mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   % e( }# s! k7 [) v, V$ A: e: g
4. 
   & W+ i  P5 n& E+ H
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   ! v+ i% `! S$ I* P
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    6 ~4 I0 r2 r0 E# B
12. #define Error1 1
    
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    9 O, Y1 @- W0 _5 P
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    5 _) [* V5 t0 N4 W9 T. N* Q
21. uint8_t D1;
    
22. uint8_t D2;
    7 S7 p! a: f5 M" Q" @) s1 h
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    
26. uint8_t D6;
    . m4 G* b( ^* s2 O1 D- Z
27. 
    + e9 B. n0 }& E5 K
28. float previous_error = 0;
    9 O4 |* I+ s# u) N2 s2 [4 T
29. float integral = 0;
    
30. float derivative = 0;
    ; _, M, D# c1 |4 J" O( ?
31. int  Speed  = 160;
    + b8 \( Q" y, t6 Y- d3 y4 V6 o
32. float output;
    6 ^2 @6 P9 }4 ]3 @
33. 
      U4 q9 `0 \0 V8 U! }* l
34. byte Left;
    
35. 
    
36. void setup()
    8 Z0 z" z5 Q& s0 y0 P% z
37. {
    + r; q/ ]3 J+ H# C
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    + D( T% `5 j& `
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    . ^4 O( B& K/ @9 P3 Y1 t2 `0 {
43.   Serial.begin(9600);
    . t! f8 l0 B2 _
44.   buzzer.setpin(45);
    
45. }
    % x4 t. V7 ]+ M) @! ~$ m
46. 
    6 l9 h) v) n& X
47. void loop()
    
48. {
    " z  O  g0 o: k4 ]% n6 k  W& H
49.   while(!((lightsensor_1.read()) < (10)));
    & X* V  x: j' S5 h  n
50.   buzzer.tone(1047, 500);
    ; W" {2 R6 \' e8 G. W& W; L) {
51.   delay(500);
    
52.   do
    * h# s( }/ W3 R, |1 N
53.   {
    
54.    int Err = getErr();
    - d5 l/ {3 S' \9 V
55.    if(D1 == 1)
    
56.    {
    6 ^  `7 ?2 k4 U* x
57.      Left = 1;
    
58.    }
    , I2 e' X  f0 `. z
59.    if(D6 == 1)
    ; [! c6 X: _- x7 p: a
60.    {
    
61.      Left = 0;
    1 n/ L8 @) l# C% Z) O
62.    }
    
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    
66.      {
    7 @: p& Z) |7 n. k* ^
67.        Speed -= 5;
    
68.        moto(0,Speed);
    9 ^% b0 \8 j- Y1 z
69.        do
    
70.        {
    
71.          Err = getErr();
    & J' C: e$ e8 G
72.        }while((D1+D6) == 0);
    0 Y5 t0 z% r% D! s
73.      }
    
74.      else
    ' X5 c) X( z& o( c6 O9 O4 x) `
75.      {
    % {7 V9 _. U8 J2 @* T" h$ d
76.        Speed -= 5;
    . ?& z, f& L& h8 T
77.        moto(Speed,0);
    5 ~- @+ q/ n# |/ d$ u& C- ^: j/ l' L
78.        do
    5 j- ?2 I. ~5 P+ R, v+ X) Y$ g! ^
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    8 h  o0 w" H8 s$ Q. s% z
82.      }
    
83.    }
    
84.    else
    8 _& B& q1 X5 U$ F
85.    {
    * u4 V0 a: x( _  o' r/ ~. h9 H) A4 m
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    5 J; D% B3 R: Q. f) P( t
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    . I! l, y& P) }4 D4 i
91.      moto(int(Speed-output),int(Speed+output));
    . g, o: F+ M6 a/ i  M$ n
92.      previous_error = Err;
    5 u( O+ h8 p7 S+ X7 _. O4 y
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    & f) g3 }2 d) j
96.   delay(500);
    . W, O* l! s% m4 v  K
97.   buzzer.tone(262, 500);
    
98. }
    / J9 |9 I* M7 w/ r3 y
99. 
    # F, q: c+ M0 D3 q: n; O
100. int getErr()
     
101. {  
     5 l2 j0 `) L1 d2 d. x# f
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     8 b! F+ }+ C/ M% L, b8 M; p9 B; E$ Q) M
106.    D4 = ~(sData>>3)&1;
     : |7 R8 h+ p( C& f4 }
107.    D5 = ~(sData>>4)&1;
     6 M. Q1 z- B2 Y
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     5 q- M" B$ w0 E8 F3 [% @
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     % Y0 E# ]! t  l0 k7 S
111.    if(downD == 0)
     
112.    {
     ! O( ~( c' A/ e9 Q+ l
113.      return 99;
     + D! i# H6 S- f0 i1 o! ~" w4 Z4 r$ ~/ n6 |
114.    }
     " Q) o; |! `% t; k
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     
119. }
     
120. 
     0 x, L+ Y2 j$ u* F3 ^$ E
121. void moto(int Speed_L,int Speed_R)
     7 h! C/ L. `: e3 M* m1 g% P) q+ {
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     8 r& y/ v" E% z% r1 L5 C
124.   Encoder_1.setMotorPwm(-Speed_R);
     # G, p7 u! H; J
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
     g1 i* o  W  R" F% D6 {# s: H
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   2 x1 R' J& I) m# n
5. {
   9 |5 B( V8 r" Q" D
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   " x0 }- H# g0 D" V: T0 R+ g" u' k6 C
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    5 S( O* T/ p0 A, d
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    ! y+ h9 I1 Z  S; ^. |
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    + X  ]- a- p+ ]7 J; P
16. {
    7 a% L& u( H7 E6 ^6 G. [* U6 y7 Y
17.     _DataPin = pin;
    ! s  l. K/ F" ~/ y
18.     pinMode(_DataPin, OUTPUT);
    # p* B/ f. ^1 i0 i, a& Z# v) \
19.     digitalWrite(_DataPin, HIGH);
    & i  s- u4 q$ o. p
20. }
    
21. #endif // ME_PORT_DEFINED
    # J& h. Y3 E/ L5 i- m- ]9 {* g
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    * T1 S% b- \/ p# K$ `; g
25. {
    " O( y4 Y6 }+ f- u4 @
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    8 Q* }2 Z! u# }3 H* E$ a
28.     digitalWrite(_DataPin, HIGH);
    6 a) d1 ?2 K" l' [1 I; d
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    & u2 E  F( e- V) m
33. }
    
34. 
    : h$ r& W( T& `2 Q4 e/ F
35. uint8_t MeLineFollowerArray::getValue()
    5 s& X/ q8 l4 S# f2 E
36. {
    : h6 l* N3 q# ~( w9 x: @& |
37.     uint32_t LOW_level_read_time;
    . `$ f1 ~! Y2 D! M3 r
38.     uint32_t HIGH_level_read_time;
    . i9 L3 s2 N/ E  M9 z7 T( T. @9 R
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    7 s2 l& E" c) B5 t
41.     static uint8_t old_data = 0xff;
    
42. 
    * g, R2 I8 H( d& `$ l! |
43.     pinMode(_DataPin, OUTPUT);
    + U( ^/ p3 t8 H2 c
44.     digitalWrite(_DataPin, LOW);
    7 k9 M* b+ i" Y. W
45.     delayMicroseconds(980);
    6 l, o" n& q1 O; r+ B( h2 l
46.     digitalWrite(_DataPin, HIGH);
    
47. 
      W: X" v1 n2 F% L
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. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    ; l7 ^0 I3 i0 Y* O: G
58.     }
    8 i! T. \0 r% j, O& E" G) h
59. 
    6 R% c  S/ j. g" v* K" |% \  J0 t: Q
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    : u3 `6 V. o" C: _0 |
61. 
    6 \& j' b# O, ?' U& B- K4 z7 c  v
62.     HIGH_level_read_time = micros();
    3 t7 c* x/ ^7 `
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    2 R1 H+ C. q2 R' J  W( d
64. 
    . B" l4 a7 N3 I
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    $ x9 b  G4 b3 `: q
67.         return 0xff;
    
68.     }
    0 F' W% C" |6 i4 X7 t
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    : x+ C6 u; _; r0 k& _* E" V9 _4 c" A
73.     }
    # d3 L2 r- g( `; C% I" Y: J
74. 
    & w5 V  v8 m; S
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    8 F3 m. s* |% j9 k# M
76.     LOW_level_read_time  = micros();
    * E/ @; W/ @7 I/ U; T: F
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    ( y* H) O5 N$ c/ }
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ! f* O" X2 R8 L+ l
80.     {
    
81.         return 0xff;
    
82.     }
    
83. 
    # Z8 }' O2 Z' s9 S
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    
86.         return 0xff;
    8 ~* H; `* ]2 I+ x" q0 \1 n' N
87.     }
    1 g5 f/ ^% G, G) _
88. 
    - K( u) O  U( Q' z) n) H/ y8 W) h
89.     for(uint8_t k=0; k<3; k++)
    0 {2 M, r% j7 `3 R& F) f- `
90.     {
    , K+ Z$ O9 z; m/ M$ |6 V
91.         Sensor_Data[k] = 0x00;
    ) d% t+ m' W0 J1 d, T* l
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    8 Q( \- K$ o8 l0 d% I1 M) d
94.         {
    / r( m( p* A+ x
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    " ^6 T" S3 [) m7 r; s) L
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    8 `1 w) c4 ^6 q8 E
98. 
    / e: @6 @1 V) d  u' U) y
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    7 ]) k& Y, J! `, g' l
100.             {
     3 S. {' z- s8 d7 n7 G+ O; F
101.                 return 0xff;
     
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     " e4 N8 [; d6 a4 `
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     * C3 A7 U: K) \# Y7 j; n
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     : B9 B4 A) L% H
112.             else if(HIGH_level_read_time >= 100)
     , G# v, w, s/ A
113.             {
     
114.                 return 0xff;
     
115.             }
     9 [& }3 r6 g- z0 F
116. 
     # i! l4 Q  B3 j! W% z% r
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     
120.             }
     : ~: a  N6 b- A# @- W
121.         }
     
122.     }
     7 S; V# ]/ j/ L2 L" D7 |
123. 
     * v/ W& Q; l; I* c5 Q
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. 
       O# n% K  s- V' N% a- y9 P) w
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     / ]$ L9 Q# F: j! B
133.     pinMode(_DataPin, OUTPUT);
     0 {2 Q: Y% I. T" O# X& g
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])))
     " y3 r' F+ V0 O  R# n: G
137.     {
     & Y" a; [& \& D- n
138.         old_data = Sensor_Data[0];
     3 `8 M3 @& S+ l. X
139.         return Sensor_Data[0];
     4 Z9 [  x0 P: S4 L, F; B$ P
140.     }
     ( o& C- C( H. o  |
141.     else
     8 f. a: J$ `9 e+ p# ^- D4 u
142.     {
     
143.         return old_data;
     - N- s# z8 h' b
144.     }
     2 _6 @3 t8 x1 ~& x4 |
145. }
     9 ?( B& \& f6 g: B) ?4 Y

複製代碼

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

2018-2-13 01:52 上傳

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

  M6 h' d2 X+ N' w# x

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
$ k( }. o* E2 q% }* z! L% s( Y哇...要100 個銅錢

3 W& w7 u% b9 o# G$ z6 [認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手