mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   3 U; H0 |+ p) M8 m
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   ' Q9 n0 V9 j+ n. a' @
4. 
   * A" \9 W, o+ q" |
5. MeEncoderOnBoard Encoder_1(SLOT1);
   & T9 C9 {. \  v7 i# M
6. MeEncoderOnBoard Encoder_2(SLOT2);
     T6 B! c5 r. D, _2 t
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   4 y$ M4 w& g) X4 s7 ?& j6 D& O, V
10. MeLineFollowerArray linefollower(PORT_6);
    : T) Q7 ?4 t. s# f
11. 
    " |1 s3 y. k7 Y  `
12. #define Error1 1
    
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    ! x; v$ V  }2 g( O! B5 L9 _
20. uint8_t sData;
    5 h; D1 \5 X7 m# J* t
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    ( [5 y  S. V4 t8 a0 q* O, n) N
24. uint8_t D4;
    ) J3 f; v6 d" D4 b/ z. f
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    6 t4 l" l# v" @& c% J; x
30. float derivative = 0;
    & n1 N& [. e' F5 U+ b
31. int  Speed  = 160;
    8 g& z0 A* ^, g& j5 B
32. float output;
    
33. 
    
34. byte Left;
    
35. 
    
36. void setup()
    ) o# S7 a* D* ^+ k
37. {
    : p  A3 p  f. b
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    / v- Q2 d; x, I" \9 ]7 k5 `
44.   buzzer.setpin(45);
    4 @' T* W! Y# K5 c( x
45. }
    
46. 
    
47. void loop()
    9 A. ^9 i! [* n# ]% B3 K# r
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    4 A. E8 G( ~) D" U& u4 G$ ^  j
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    ) D. E+ O5 K2 V7 }5 R- [* e( Q' E! A
52.   do
    0 U) J% ^9 K. D2 b  M
53.   {
    0 H& L* V. V& {- b
54.    int Err = getErr();
    
55.    if(D1 == 1)
    ! }( _  D, T$ q) C9 w
56.    {
    
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    - v0 o; G& U8 n  w# V( }
60.    {
    
61.      Left = 0;
    - Y% P. x( K5 q& _  A
62.    }
    ( b! z- F2 W( Y
63.    if(Err == 99)
    % g) l8 v! e6 X: h7 f9 ]
64.    {
    
65.      if(Left == 1)
    
66.      {
    * A3 U: {. Y% E- G' ^' d
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    
70.        {
      E( e$ Y: c" t" N/ n
71.          Err = getErr();
    & n# F8 f3 f/ p. U
72.        }while((D1+D6) == 0);
    ' b' N9 k& Y$ C- o/ r  C& v
73.      }
    
74.      else
    
75.      {
    8 q' B% E. J2 F) X7 Z7 \+ D9 x
76.        Speed -= 5;
    4 F! E; x0 f" J( O: p( A3 H! G
77.        moto(Speed,0);
    
78.        do
    + i3 O4 X6 p% Z3 ]) Y: b
79.        {
    5 S; C6 T- K' }7 y, m1 _9 ]
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    " G3 Y" `2 S  m( v2 S: I
82.      }
    3 ]4 B5 F* V& x: r9 x! D8 G; I
83.    }
    
84.    else
    3 Q1 k5 ^1 k# R# J! w( |
85.    {
    5 H: [4 m' }: [3 W0 P
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    - o! k3 u$ N8 n3 S" M- K4 n$ b
89.      derivative = Err - previous_error;
    : y( k. J9 y2 y
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    1 Z4 z( I. k' H; N( f
92.      previous_error = Err;
    3 o, F$ s& a% P  [3 N
93.    }
    0 N9 I5 L0 {/ k% g  j
94.   }while(!((lightsensor_2.read()) < (10)));
    7 L$ o( T. O) m
95.   moto(0,0);
    : j+ R8 g/ I! I# i  U& d& l
96.   delay(500);
    3 y- G2 Q3 ^% E
97.   buzzer.tone(262, 500);
    . W" I" c+ T, q! I$ n& x
98. }
    
99. 
    7 b: M5 Z$ ?! P* z
100. int getErr()
     ' B3 o0 O8 j4 r2 L
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     : O) Y+ j# S4 `3 S$ y
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     + C( u  S% @. \% M  e: W* W& Q
107.    D5 = ~(sData>>4)&1;
     : q: G5 K% w2 V2 H# Q
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     $ l" s' p7 f- I" i4 r
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     ' a+ \5 \$ x8 Z/ T" c2 c
112.    {
     : R( `# k- L$ {3 I3 ]4 e7 R; }" N
113.      return 99;
     
114.    }
     
115.    else
     ! M9 }8 H8 u/ W4 S1 T3 `+ f
116.    {
     
117.     return upD/downD;
     
118.    }
     
119. }
     + h  S6 O9 \+ w& B
120. 
     " q7 |+ q0 G5 {; i) y: a0 M9 W
121. void moto(int Speed_L,int Speed_R)
     & X  A8 I! T0 ^' B$ E  S
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     + x0 Z: f- x3 T( x
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

8 h7 N. H) c6 @& F# M5 EMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   $ ?/ i0 c7 I9 u! a' v
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   " P) z! [0 U* G/ W
5. {
   
6. 
   3 y7 l; i6 f! `3 W! S! v1 J) Q
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
      }: r* T" l  E0 L" D' e5 A. n
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    , f0 }, z9 n) G8 z) s8 A) m7 c
14. #else // ME_PORT_DEFINED
    ' R' B2 U6 N7 K6 k( I- D. w4 T  k
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    " a& g$ Q7 ^0 o# F$ |8 e
16. {
    8 z2 Q, @  }: Q# v
17.     _DataPin = pin;
    9 [& s! I2 n7 Z' P
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    , K% ?2 _* I/ _8 A4 a& G
20. }
    6 K# s! o9 L& ^& u
21. #endif // ME_PORT_DEFINED
    
22. 
    6 T+ ?, a$ U/ n# c, ?& ^4 O
23. 
    4 W" w7 y: a2 x* z0 |+ x  K
24. void MeLineFollowerArray::setpin(uint8_t pin)
    7 Y8 V6 U" ?* G" p9 x8 X
25. {
    * ~9 e5 r% w- ~( N2 _% m- q
26.     _DataPin = pin;
    1 X) d4 w; F4 M  h; `  ^
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    ) E$ C) R% o, W3 Q
29. 
    0 f. h1 L9 h( S
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    ' L0 I4 V, s$ o* D
32.     #endif
    % R: k; U: R, F6 L- v0 ^9 F
33. }
    3 }9 V0 j: u  \+ e
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    - ^  Y! e) u6 f  D/ q
37.     uint32_t LOW_level_read_time;
    + @, ?! _. f: j8 m5 [+ P( K( n
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    2 T2 |3 [" L, D- D' a. x, K# Y
40.     uint8_t Sensor_Data[3];
    $ ?* c- u, J9 p. R. T2 m4 B
41.     static uint8_t old_data = 0xff;
    4 V0 Q1 E7 t6 A) w
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    ' F  a9 m/ C& u2 ?1 F- t; [
44.     digitalWrite(_DataPin, LOW);
    ) v; X) [5 e  s# x/ \# C! v1 f# X
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
      b% n7 G* M/ ~, ~* F1 ?
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    ! }% E7 l! q+ s0 N3 ^8 O
50. 
    ( Z1 h4 p4 r; X; Z0 ~
51.     time_out_flag = millis();
    : ]" m: z) }* y2 K( x
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    , P  r: y, d/ ?" v; @* U
54.     LOW_level_read_time = micros();
    ( ~0 |/ l9 N6 M% y  D4 b2 C- _
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    . v8 u& U- a& P. q6 a) v1 u
56.     {
    9 \' m7 U! s( B9 k2 Y  {, e; R
57.         return 0xff;
    
58.     }
    
59. 
    2 S! u, @) b" o
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    9 }( F6 _$ y' V1 t
61. 
    " ]- c- ?5 G. Z8 d7 B+ e
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    / Y2 o7 M" I" ]+ y+ B; \$ h
64. 
    ! i& n9 w* ]6 ^  K, q
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    / v! P0 S6 J3 T  n
66.     {
    , G4 ]" ]- P" `* M+ J
67.         return 0xff;
    
68.     }
    - a3 A, s: B9 j6 ^! m" i; [
69. 
    - P5 y% {4 S& m' t& h. x
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    " |6 E0 W, g7 g& ~" T" J" I
71.     {
    ; _+ I& D7 B, E8 z) c; r! K4 h
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    7 ?4 }  |8 A9 m: d& k
76.     LOW_level_read_time  = micros();
    + l" S% B, g) M! U% z
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    7 M: i" y# }3 c  w! S) r. ]
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    # f) u8 B' Y7 q; T+ b/ U/ T
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    ( z3 W# q* R; d4 W* c1 }$ F- G
86.         return 0xff;
    
87.     }
    . ^- e& `" {' p; x! v
88. 
    8 z  `9 v- o9 m9 Q% U
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    - ]+ h$ c7 |8 \) H& R
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    * E# e3 ?/ B3 F, m2 ^
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    8 ?4 [/ P1 Q. s% n
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) );
     
105.             LOW_level_read_time  = micros();
       F- y5 E+ x4 n9 O
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     7 r+ B+ w4 k; D" w
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     " |$ r) d; g2 K/ u' _$ I4 |
109.             {
     ; D6 x  |* o1 U9 S
110.                 Sensor_Data[k] |= (0x80 >> i);
     % ~" c$ |% z* @0 A8 B
111.             }
     ) A: O- S$ b" l* }* g' J! u# o* |' ~
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     * x7 M1 J+ g# K8 l- G. I$ F
114.                 return 0xff;
     2 ~7 L# C$ k: e& o
115.             }
     & R4 K! Y8 h1 l) c3 V# |( k% \* v
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     ) _, I( r) f$ k- h/ E: I7 c
119.                 return 0xff;
     " j' Y1 Y! x3 ]7 R" n4 }
120.             }
     % ]  Z. m% W9 ?3 l2 u* v# q9 t, Z
121.         }
     - S: a$ @: }& x6 `. W
122.     }
     
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     2 d8 m4 Z& f% x% X% o
125.     HIGH_level_read_time = micros();
     ! B; v' ]( F( f# K1 C, }1 I
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     + h- k! s9 D4 `7 ]  Q1 o3 }
127. 
     8 t. [0 ?: L3 f: g5 {! I
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     8 o' l: f7 |. T0 A+ N$ A
129.     {
     # E! n+ b3 [+ S+ A6 R$ y
130.         return 0xff;
     
131.     }
     
132. 
     ! @1 j/ r% `+ y% ?9 M
133.     pinMode(_DataPin, OUTPUT);
     
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];
     
139.         return Sensor_Data[0];
     8 @, z+ V: }6 e4 \2 s9 M5 i
140.     }
     
141.     else
     : ^$ R: J5 }% ?8 I" ]* X
142.     {
     
143.         return old_data;
     4 _  U/ \$ m$ H4 G" F  G5 K
144.     }
     
145. }
     

複製代碼

. K/ j& B' H  [2 z) |$ `; A) jMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

+ b0 x5 M, q1 U! D

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
哇...要100 個銅錢

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

# T& l) t, h% e7 {; Q1 J! LMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手