mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   - g- S7 D5 Q: c+ z" b, m
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   + l2 N) g8 d5 K% X# B- l
4. 
   - t  u3 O. N0 N" w  }% J  @) H2 ?6 k
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   ' e2 D. `! Y6 @' l- V* r% X  D0 B
8. MeLightSensor lightsensor_2(11);
   1 F  `7 K9 @9 k: S8 a
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    3 z& W5 h& |9 U: o: i) F
12. #define Error1 1
    3 r/ g, {1 b3 u8 k( r3 q$ o) C
13. #define Error2 2
    , S  Z% q3 h5 u; m6 r
14. #define Error3 3
    
15. 
    1 A4 |7 X- D' T. i1 I
16. #define Kp 15
    : v) S9 r4 M. z& {4 u3 K
17. #define Ki 0.15
    * u" q+ S" g; W; _: H
18. #define Kd 0.03
    
19. 
    : h3 Z! a- e% D
20. uint8_t sData;
    
21. uint8_t D1;
    " x# ?. a6 y; D- f
22. uint8_t D2;
    
23. uint8_t D3;
    ! Z: i( D( g2 h, R' G7 i# e
24. uint8_t D4;
    
25. uint8_t D5;
    4 P* A; j2 s) s" _% O
26. uint8_t D6;
    
27. 
    - G0 |% `: L9 D' C5 B
28. float previous_error = 0;
    
29. float integral = 0;
    " ]. [# V6 o9 ?: A, b' e
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    
33. 
    
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    5 Q- o6 s) J  W2 A: x
39.   TCCR1A = _BV(WGM10);
    5 {9 h3 t* `6 C- a. ~
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    ' s( w/ Y% p+ y4 R4 G
43.   Serial.begin(9600);
    ; w/ y. w8 Q. i( g7 G5 E; H* [! S
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    ( E! j, I( h5 ]" W0 ]: b
48. {
    7 L# a6 N- I; q1 S# |
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    0 q! Z& U. A$ ~( M1 T, e5 i4 D7 [
51.   delay(500);
    , Q; ?- o) h$ }9 j3 q2 N5 }
52.   do
    
53.   {
    : X3 `+ p0 T1 |! F! ?5 M* e
54.    int Err = getErr();
    & X" S* c( R0 M' X' S* g) I
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    6 s$ t! f. V1 j' c" }( ]6 V
58.    }
    
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    
62.    }
    - J) s  j& f% X( n
63.    if(Err == 99)
    # v4 _% {$ p! C7 v. _
64.    {
    4 w1 w+ X7 v1 t
65.      if(Left == 1)
    
66.      {
    4 _5 k* s  z2 A$ f2 P
67.        Speed -= 5;
    ; {0 \# z$ G5 \7 {' ]% H, `
68.        moto(0,Speed);
    
69.        do
    
70.        {
    
71.          Err = getErr();
    4 F( H' x9 v$ {0 c9 D
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    ; t- }. r$ [0 g: G
75.      {
    2 e) D1 x& B: b9 a  V0 w8 I) d
76.        Speed -= 5;
    6 \  g' K* F/ R2 q9 ?
77.        moto(Speed,0);
    
78.        do
    : K, p, L7 [9 ^* w
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
      r* L) {5 e/ z" H
83.    }
    % _9 u' b5 X& w$ E# a* t
84.    else
    
85.    {
    8 M, \) f; \2 w
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    " D$ k6 v* o9 F7 e8 ?) z
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    7 r% |2 M5 b, d
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    : ?" w" w/ O: W$ W' J
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    , q1 q6 j+ w1 a. v( S/ d2 ^8 D
99. 
    3 a8 |8 s; M- N, S4 d4 w9 e" E. K
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     * m  g+ F0 s9 ~* @6 F- o: d
103.    D1 = ~(sData>>0)&1;
     7 ^4 [3 ]7 d2 ~" g- z' }: G2 k/ a8 {
104.    D2 = ~(sData>>1)&1;
     ( z- P" S% N6 M- Z; t- E
105.    D3 = ~(sData>>2)&1;
     3 ?; O4 O3 n" ]: @' u  c
106.    D4 = ~(sData>>3)&1;
       W) T9 B- R* S- l; \
107.    D5 = ~(sData>>4)&1;
     * D; ]* Q( H! i
108.    D6 = ~(sData>>5)&1;
     4 n. m: ~2 {6 }4 V; n8 v! Q3 _
109.    int downD = D1+D2+D3+D4+D5+D6;
     + {. Q5 r  z- n& R
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     ) K' t2 [3 ?" ^0 y' k& `
111.    if(downD == 0)
     
112.    {
       X+ w4 l1 E: H1 {* _9 N( E
113.      return 99;
     
114.    }
     
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     $ {! N( M$ d( O
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     9 F) \, K; N3 S/ a+ `  {
125. }

複製代碼

* q- ?/ Y$ \1 I. _, B1 ?% R" HMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   0 c2 t. _& `0 p9 p  _
3. #ifdef ME_PORT_DEFINED
   ; E* b  U; M8 ^) Y! H
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   7 Y$ |% L5 `9 F% T# q. S
6. 
   - t' a9 n) b; c' A: S6 n
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   ( G8 j8 V+ g8 R- U  I
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    0 v- b0 }! Q/ g; |" a& Y
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    5 D  h& Y9 q; f+ P3 S/ j* r
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
      l5 ]$ |+ N5 ?7 _! p% b
18.     pinMode(_DataPin, OUTPUT);
    . u* @9 x% P" ^+ I
19.     digitalWrite(_DataPin, HIGH);
    % i' B8 g3 u6 Z  c9 e
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    % S7 G8 p  r3 p, d4 k
24. void MeLineFollowerArray::setpin(uint8_t pin)
    ; d0 {) m) v8 L% A1 e
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    : {% w# X4 [: p4 C: V
32.     #endif
    9 n: S! U- R5 K4 I
33. }
    
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    4 Q3 t6 @6 G5 J7 |3 ~- T
36. {
    
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    3 H% X; k$ V0 I1 M( P6 g
41.     static uint8_t old_data = 0xff;
    9 J7 N% D% D0 T9 ~1 o9 c
42. 
    ' @/ @+ Y6 c( `0 W/ x! V5 Y
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    / B% A, J* c2 @. C! s; u
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    + U) N* A! J2 P9 Z
50. 
    " x# c# o6 {8 u
51.     time_out_flag = millis();
    1 Z9 g2 N& n. U' h# P9 z" z
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    7 W. w/ l( k3 L2 K
54.     LOW_level_read_time = micros();
    # |3 \8 D" D# X1 k. \* i
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    1 i8 s" W$ y; {4 D
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ( Z, p8 `  z" ~9 k$ ?0 N3 ]. Z
61. 
    
62.     HIGH_level_read_time = micros();
    - D" Z; _" W( P9 p
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    1 i4 A' ]# L* g; z* s3 {
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    ) l" o* p/ r7 e- a0 F/ w. M
67.         return 0xff;
    6 c# N5 P; V( ~4 L
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    . G5 P; z6 t. ?6 D
71.     {
    
72.         return 0xff;
    * d1 ?! K+ }) z0 \8 X
73.     }
    
74. 
    
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
    : F8 B7 Z& _9 y' R7 X$ w! \- _. r
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    9 _$ g) Z- s* C
80.     {
    
81.         return 0xff;
    3 n9 k. X; m6 o  N+ T, K
82.     }
    7 ?1 w8 Z$ U9 }5 g" j! u: ]
83. 
    5 C9 u1 F6 S+ F% {
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    ; I( K* F, z9 d5 Y- T
86.         return 0xff;
    : z4 h6 h- A% ^" Z0 y
87.     }
    
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    : Y' ?3 o- Y: z- R3 A; ^; _
91.         Sensor_Data[k] = 0x00;
    0 E, v/ u8 n' K4 J" M" v2 O& |7 j
92. 
    . ^$ a. k. g7 S/ y1 w% c
93.         for(uint8_t i=0;i<8;i++)
    . l6 ]1 _4 o8 Z. ~+ ]/ T1 J( V
94.         {
    2 ?, K# D2 O4 k7 R+ I" i
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    , d* w7 r& S5 @' F8 W+ h
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    , T6 X! P! t6 K/ k" [" B7 a
98. 
    + ]$ c7 L' z) t, Y8 g) Y
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    9 U3 z8 P% x2 w: I) h9 ~/ R
100.             {
     
101.                 return 0xff;
     4 K  `0 u- X  ~2 M' P- k6 H
102.             }
     
103. 
     * j) f4 v8 J4 A2 v. [0 {/ d
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. 
     4 v! K4 P+ R6 [# e
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     / [. @9 t: V$ I, L( b
109.             {
     1 p# u: Q* D2 N8 t1 L
110.                 Sensor_Data[k] |= (0x80 >> i);
     4 R% E2 o4 a' s$ k' j+ J
111.             }
     , {1 ^2 E0 W5 d' X1 N
112.             else if(HIGH_level_read_time >= 100)
     5 R% Z9 ~) X9 t; K, K$ H
113.             {
     ( ~5 F: F  K' N+ w% F$ m" d
114.                 return 0xff;
     
115.             }
     7 {. T& D7 ]* P* Y6 @2 q2 b
116. 
     , V; E7 U! I$ g, ]8 U" L. W
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     - ^& {. e$ B+ T1 W3 [1 h$ p
120.             }
     
121.         }
     
122.     }
     
123. 
     " f2 e8 ^. N  ]
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     8 `: g% B) R- x8 B* ^
125.     HIGH_level_read_time = micros();
     3 g* [- |: D( _, m& q' T5 U
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     ( A3 U  _' p8 q. ~" ~
127. 
     % [) |: Z+ |5 i2 o3 B( B
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     - o7 J( F, p- `. m1 [
130.         return 0xff;
     ) x* Z) `* P2 W1 n/ \" e
131.     }
     
132. 
     9 Q6 [5 v) p& s$ x+ k: I
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     2 O' U" R: a; t- A3 Y+ g) r
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];
     
139.         return Sensor_Data[0];
     & B% b; E# s3 k9 T) z1 p# u  k
140.     }
     1 g% T, \$ Q* E' t0 u* T0 V
141.     else
       `# r( C$ _+ ~3 A  @( k) e8 I( M2 K
142.     {
     ' `/ f& z/ j0 f0 ?% O, _
143.         return old_data;
     , W7 W- i  t, [- i$ m' H" [
144.     }
     ! u- `, m1 G9 j3 H3 E* X
145. }
     , {2 d# @$ r) l0 _" r) o1 ^( G: A  v

複製代碼

MeLineFollowerArray.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
0 h. C7 |( F' r& z: s0 }2 D能否用到mblock 5 上面呢？

; b* a( z8 R, e! ?5 y& v: mMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手