mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   7 ?/ v0 m: ^/ m0 I
3. #include "MeLineFollowerArray.h"
   9 u: P+ X5 I9 U1 w  B4 I" L# ?
4. 
   9 M! A# o2 B& l
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   % U" z0 T$ d! G' v" p; b" B+ O
8. MeLightSensor lightsensor_2(11);
   . H4 f& d6 u# }, ]5 x
9. MeBuzzer buzzer;
     V- E; I( Z! I; N4 C  T- e* d
10. MeLineFollowerArray linefollower(PORT_6);
    % _0 I6 \9 \1 G( g+ `
11. 
    : a  E5 {# k+ d8 [) }: U
12. #define Error1 1
    
13. #define Error2 2
    3 i" M$ u/ k8 a
14. #define Error3 3
    / ]  C* ~' o5 C
15. 
    
16. #define Kp 15
    ( D3 l$ ^- R, t) B+ u
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    + n& T- {1 B4 ]) H; a9 W3 E/ e& h6 R
20. uint8_t sData;
    
21. uint8_t D1;
    
22. uint8_t D2;
    8 X7 @" l+ [+ I7 g: F/ {& t
23. uint8_t D3;
    1 a0 M( K: u& X! f: X" B+ p7 {& y
24. uint8_t D4;
    - U# o+ q, d' o2 v- p  j
25. uint8_t D5;
    
26. uint8_t D6;
    5 V. O; m5 @5 _, Y
27. 
    
28. float previous_error = 0;
    ! n1 Q: Y1 F# G! ?: z; X
29. float integral = 0;
    6 z+ R. b2 l9 W0 t
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    
33. 
    
34. byte Left;
    ( Z- x& \$ N: _+ K$ k$ I
35. 
    & n5 E/ v4 K* D% l2 |  A; V
36. void setup()
    2 S% t) t% {0 k+ i6 p5 a) \7 ?2 i
37. {
    
38. //Set PWM 8KHz
    7 }2 H6 r3 w) t0 g* p6 ~, C
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    . a" V, V( c& @* Y1 ]
43.   Serial.begin(9600);
    % F7 b- }3 g. ^5 }, l1 Y" O
44.   buzzer.setpin(45);
    ' g! l) |7 H- c' `0 J& ~3 K. q, ~
45. }
    
46. 
    8 \" j4 B+ R- t5 y' Z* w
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    ( G8 q: B1 L0 o
50.   buzzer.tone(1047, 500);
    1 a5 e* @, `5 s8 A
51.   delay(500);
    
52.   do
    2 S& x  A1 h  N" u
53.   {
    
54.    int Err = getErr();
    + Z  |1 S' X6 P( N( x
55.    if(D1 == 1)
    1 x  l* w. x" C6 L3 l
56.    {
    . x: |8 t9 O" n$ Q
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    ; b4 i0 J5 z- e4 I) _
62.    }
    6 _! B8 \3 M7 P! O* P1 W) T
63.    if(Err == 99)
    
64.    {
    ! B& F5 F3 O9 x, C
65.      if(Left == 1)
    
66.      {
    # ?2 z- h* t% `' e! q$ t. P* I( i
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    1 L2 X0 n, R+ [5 z8 p. J
70.        {
    & a: x4 g( C4 `. v" G# `6 v
71.          Err = getErr();
    4 g2 ]: `1 @' ?& F1 S2 ]# j
72.        }while((D1+D6) == 0);
    
73.      }
    ; T5 W# s( \7 V' W  d; `
74.      else
    ) _: R0 M6 W) v: j. x1 ^! q- Z8 x
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    5 d3 M8 ?2 `6 x7 Z1 d- T
78.        do
    
79.        {
      u; ?5 }, c2 n
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    6 L6 j7 v; i4 y( A; V/ s
83.    }
    6 [- [& v( C5 P4 V1 \' b; ?/ s% E
84.    else
    % D! J7 Z( H2 X) o9 q7 A) R- {
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    : v: i' a3 D( K: i- ~. Z0 \
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    , T- @* ?  p9 X# e. K
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    0 l  q2 q+ d6 A8 O6 E
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    : I( {% s5 u: i; X, @. H
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    3 M5 z6 o, Q8 `# r
97.   buzzer.tone(262, 500);
    
98. }
    ; n3 i2 L9 D: O  c6 R7 C+ j1 M
99. 
    
100. int getErr()
     
101. {  
     2 S/ T! h7 U9 y9 h- z" m( J% D
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     7 Z1 T6 s2 F0 ?5 C' w! r/ D: x
105.    D3 = ~(sData>>2)&1;
     5 X6 s. [. x( i- S* A! [  s
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     $ T$ D8 g/ p; X8 H- l, j% S( Y
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     ' o: o1 }1 F( C( _2 V' r! V
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     * L& ~; M6 G+ C! K4 s  f- l
111.    if(downD == 0)
     + Z% P  x3 H9 `
112.    {
     ! _+ i) s3 z/ j# b
113.      return 99;
     : R$ E) W! i8 d, N
114.    }
     
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     
119. }
     
120. 
     6 M3 O. V9 \! f& |" m
121. void moto(int Speed_L,int Speed_R)
     , m# N( Z2 h0 \1 N4 k' C& J9 e
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   : w% K  o7 x5 S: Q3 h
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   / x) ?( a) f5 ~: U
5. {
   
6. 
   6 U  ^; m2 y4 f0 m7 F
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    + I& V& H# P9 v
11.     pinMode(_DataPin, OUTPUT);
    + _2 `* K5 y4 s7 K$ ^9 J
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    0 f2 n8 M8 w; K$ @$ s  i
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    ! y4 z* ]+ h! P4 d1 a
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    6 ]4 g8 x; e* ?
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    - O( c3 A1 `. l  j9 ^
23. 
    ) g) v- m$ t6 U7 T
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    ! k: ~$ P) a! J( q5 |( |) g
33. }
    * }9 X, s; \0 H2 L( Z* z# `
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    1 d0 B" e' h$ R3 b4 l* F
39.     uint32_t time_out_flag;
    5 D  G) |, l! M0 {9 H
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    0 `/ Y% c9 x. S) Z; O
42. 
    7 B# L% G, S" G1 w# z/ Q) V4 W) F
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    , I, u2 t$ C9 s! Q' P6 b3 H0 l- @
47. 
    & C9 ]: K; r6 X- p( p! D8 [( Z
48.     pinMode(_DataPin, INPUT_PULLUP);
    7 q8 {8 x9 ^, B6 }
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    # D# l# E0 L3 e
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    , |/ {( c8 r! P
53. 
    ) J% N4 H7 k0 e; g! y
54.     LOW_level_read_time = micros();
    0 A+ r1 `' p8 |0 p0 {- w+ f4 _
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    9 h( L) N5 \. y
59. 
    ) w( F8 {9 @% T- q6 ?+ G
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    7 U0 L. e1 }' _$ r/ ?6 x: C
61. 
    2 X' F4 T4 B, A1 b  T
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ' \" P2 s3 T* |) b) z
64. 
    0 D9 }2 s3 Y  w9 R' C. K
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    . ^) l* Y1 s' ]& T8 `
66.     {
    
67.         return 0xff;
    2 `3 L9 [+ l% G- P: U! _  E4 J! g
68.     }
      A! R4 i2 G% F# Q0 @
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
      J) ], j3 D2 ^
71.     {
    
72.         return 0xff;
    
73.     }
    * X3 l8 ?! ?7 a$ R6 d- G' T' U0 ?
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ) s. ^7 j' Z) D3 _5 |; F) J8 t
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    9 `8 V, v, \, y% o
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    # ?8 x2 T9 s9 M, J2 `& F
80.     {
    . L" R# x# I) G  u/ p1 T
81.         return 0xff;
      F# m, \% ]8 L! z% K
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    2 K* }; U, q6 [7 m$ G1 \
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    5 m8 _6 K6 n' o$ O
92. 
    ; N! M" @4 q6 V! M4 d% O
93.         for(uint8_t i=0;i<8;i++)
    ) k% C4 b4 y8 L$ b- D% O
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    / D, E$ ?2 X5 S3 z3 d
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    " @1 R% x& _5 W: D! k5 `
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     
102.             }
     , C. N. P) y4 p9 t1 t4 f
103. 
     7 w/ T" U! R5 d3 h) O# F2 d! Z% C
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
     6 n; i* s) {- W3 f. D
107. 
     3 O4 h" o( H+ F4 x$ S" i, U
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     " M0 W, g4 C) ?# B4 z
110.                 Sensor_Data[k] |= (0x80 >> i);
     2 I5 j5 k/ J  C1 H
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     " r$ a/ C* _- {, h! D! I
113.             {
     
114.                 return 0xff;
     + S. _0 r/ d5 i! n' |9 i
115.             }
     
116. 
     # O3 H6 ?7 v1 e; x* o( g" l
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     ' m2 h- _; |/ c6 W7 P& f
118.             {
     6 x1 x' {; k) d2 `
119.                 return 0xff;
     8 x4 F9 {7 ]# i- x3 @+ H
120.             }
     8 J5 I: X6 M( e/ _
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();
     + _7 C1 l- W# X, g, w" j7 N
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     - j% x/ I3 k6 ~$ e4 n* P
127. 
     7 b) y3 P3 N. [; l" ]: M2 k4 n( S. i
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     $ m5 t2 {3 C5 Z' ]
129.     {
     
130.         return 0xff;
     
131.     }
     0 n  u* y2 l+ f+ ~, l, X8 _$ W
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     0 a9 f- C! h" ]( }5 R' 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.     {
     
138.         old_data = Sensor_Data[0];
     + R% }1 ^! b9 x3 b6 S+ B
139.         return Sensor_Data[0];
     
140.     }
     " m3 z) M! ^5 Y4 ]: z# j6 _
141.     else
     9 `) [$ x$ a+ A; J
142.     {
     
143.         return old_data;
     
144.     }
     ( E; v4 v, I/ z0 x4 P
145. }
     

複製代碼

( R7 R2 \, E! Q& t/ {% ^7 m. w3 lMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

& _* c9 X) q. {% W: H( M9 t

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
+ r: m5 ]3 x* d( T6 ^% |' n3 g. v哇...要100 個銅錢

6 h* n9 e$ Z8 W& b) v! r認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
" ^. E9 H! P# |8 |0 ]- ~5 b能否用到mblock 5 上面呢？

, \! |) G( e; [* b2 HMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手