mBot Ranger 高速循線範例＿

* T  b, A8 X) J$ ]2 |

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   9 t% _2 i: ?9 s! h4 L9 h
3. #include "MeLineFollowerArray.h"
   1 U. `4 s' F3 H/ ?" [- F
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   ; w, ~' j9 Z3 ~5 k( l
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    ( k) M; y% j$ s7 M! P) O% ^
12. #define Error1 1
    7 o- f/ m5 G# W3 q9 ^" p8 x/ Y: ~( m
13. #define Error2 2
    
14. #define Error3 3
    % w9 S( I- p& p
15. 
    " f0 [  h0 T: W: F: ]* r9 |: L
16. #define Kp 15
    ' X1 R2 [. a$ l  H2 w
17. #define Ki 0.15
    
18. #define Kd 0.03
    1 y8 [  d( ^  u5 A: X6 q7 {. G
19. 
    ; l, v8 ~5 {$ I9 G5 @3 j7 ]
20. uint8_t sData;
    
21. uint8_t D1;
    4 K6 @7 Q5 y7 [/ A  c; q* H
22. uint8_t D2;
    * p5 @0 |1 v2 _7 j- W/ N8 v
23. uint8_t D3;
    
24. uint8_t D4;
    2 n4 b4 j- t! [
25. uint8_t D5;
      h7 |* ~% w- `* O& e* U1 ~
26. uint8_t D6;
    
27. 
      s( c! D) r" S; B  C' T: T- t. v
28. float previous_error = 0;
    6 N+ a% Q$ V5 \" C7 u* ?
29. float integral = 0;
    
30. float derivative = 0;
    ; _" Q0 K2 u  j: H
31. int  Speed  = 160;
    
32. float output;
    
33. 
    1 b/ W. U) p* m- P6 X& K7 S& _
34. byte Left;
    7 h! i) u- d$ y/ A: \
35. 
    
36. void setup()
      e& P5 M6 F- D4 ^7 k
37. {
    6 `, T5 x, M& N: e- @# `
38. //Set PWM 8KHz
    / T" E' U' D% X* Y. T- h' J
39.   TCCR1A = _BV(WGM10);
    ! M1 Z# H5 J4 B5 H% Y$ Q2 h
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    , [; V5 b1 u( n% ~( H7 w
42.   TCCR2B = _BV(CS21);
      v( L, E- A2 K  d
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    - j0 Y( M6 M6 T/ `
45. }
    4 _7 _& ]' O1 t) C# T; G: @2 T* E
46. 
      _' ^4 r6 t& v" M  a# o
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    1 X. {4 u4 ]( R; H' D
53.   {
    
54.    int Err = getErr();
    ) w. z, i7 Q1 @6 u8 C1 n
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    2 r# [: K" i$ S" T# y
59.    if(D6 == 1)
    : F/ n2 R0 B2 \/ O6 {0 \- [
60.    {
    
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    
64.    {
    - ^: s3 V. K% q& ~. K
65.      if(Left == 1)
    , }1 q0 X  W1 z, ?
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    
69.        do
    " Y4 T; [7 ^: U
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    & p0 w! Z7 P: d0 a/ M
73.      }
    # X, K) J) B2 |& R% K7 a
74.      else
    
75.      {
    & i8 q' e- l* L& S
76.        Speed -= 5;
    
77.        moto(Speed,0);
    & T2 Q8 i5 w' o8 L' d% W
78.        do
    
79.        {
    : i/ t! A  d# J" \4 p: d
80.          Err = getErr();
    . r* B/ ?; l6 X7 K- d( [& M9 _
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    * a9 X8 y8 k( O7 h+ B" n
84.    else
    
85.    {
    / A& Z( V) ]3 S: M, I/ c0 q
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    6 i6 Y/ D& i8 @+ C
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    5 F% l% U8 L: u$ R1 C$ x* B
88.      integral = integral + Err;
    5 ]$ ]' h' o& E8 N. s) u+ H
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    & v4 p2 e9 v* t- n6 n  I; F- ^
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    ! T& I) a' R; I5 w  \) v
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
      q8 `+ f+ K# r& G8 a. w  {0 _/ _
98. }
    ! i6 ]9 L$ d! I9 A" Z7 l. j& s
99. 
    0 Y! q1 T/ W; o! [& E" I
100. int getErr()
     7 C3 X7 |/ }7 X- |: {
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     5 c, S& m/ ~7 Y3 `4 Y1 n; h& e0 l
104.    D2 = ~(sData>>1)&1;
     & x% \! H9 U& H& F
105.    D3 = ~(sData>>2)&1;
     $ g3 x+ \8 l# `& m( r! o8 ^
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     0 Q& m+ |3 j5 Q% ]
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     ) D9 L: s+ |. \1 c! ?. C7 m
111.    if(downD == 0)
     - h+ g' f5 F! l
112.    {
     9 h: `$ r5 H( C. Q: J: M- ^" @! A
113.      return 99;
     % G+ ?+ L; l3 a# \$ Q) b
114.    }
     8 ^3 ?: m+ \2 Z) [# {+ W
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     
119. }
     
120. 
     8 Q: j5 D( M# _3 S# b& a7 d* r' D
121. void moto(int Speed_L,int Speed_R)
     
122. {
     3 g! }! y6 ?1 ?3 b, Z
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     ; \" W# E% I& O9 Z- S! |9 z5 E5 d
125. }

複製代碼

MeLineFollowerArray.cpp
* r. j1 S( b4 j

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   / F. g$ B+ u' m  ^* s4 A
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   1 `, S8 |7 P: K+ p- n' g; W- `
6. 
   % }. w8 J1 {4 X" l& ]& G8 {
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   $ I9 ~/ _# s6 |6 f
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    : Q$ l$ {0 `+ A
13. }
    - G+ }  {5 ?9 a  S/ Z. R* \' J; T
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    ; n6 p$ F+ }2 s
16. {
    
17.     _DataPin = pin;
    $ y0 W. C5 G% ]4 K: X
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    & k# S3 O+ r2 w# u# y. S0 Z
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    5 |4 [2 k% q, u
25. {
    
26.     _DataPin = pin;
      h) ?  h; R& R) }/ q/ ^: a7 x2 X
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    3 o1 M4 f  L; I0 ^+ |, n- j! n
30.     #ifdef ME_PORT_DEFINED
    ' o# t8 {. E  G& B5 h! }0 ?( n' {
31.     s2 = pin;
    ( y0 j5 i& d# f9 a. x
32.     #endif
    
33. }
    
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    8 M: a# C" Q+ P9 C
37.     uint32_t LOW_level_read_time;
    3 Y: J  l5 N, C- ]5 k, x  y
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    3 w7 m) ~, I( p- [8 S) t
40.     uint8_t Sensor_Data[3];
    5 g- D& {; L6 \0 X
41.     static uint8_t old_data = 0xff;
    9 y" y1 k" W6 d
42. 
    / Q2 {, }' [6 a% u; f
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    ! t* y# @5 C0 Y1 k# Y
45.     delayMicroseconds(980);
    3 w/ u& N+ [' B  J1 D8 ]" T2 C
46.     digitalWrite(_DataPin, HIGH);
    0 c& i( n: B4 q5 Q5 R3 M
47. 
    9 m) m0 z* H/ N: N: Q' D
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    7 `% n, _" P0 z, w& ~- |, P5 A
51.     time_out_flag = millis();
    ; q/ w' f5 O  Y* v
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ! W' w* G/ Z9 q, a
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    / I/ i! p% `, V/ r+ \1 d7 ]
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    4 ^' O( m! i% x& o* h  U
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    + ~- c' I4 R6 v
66.     {
    / n" V8 [5 _; w. \; K: ^  V; V& Q
67.         return 0xff;
    1 `2 |/ \8 x! y3 v3 k2 ~5 t
68.     }
    
69. 
    1 A' T9 B2 p# J& H/ M$ u& {
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    & d8 Y6 [  r  k
71.     {
    
72.         return 0xff;
    , C2 j9 d) _9 g6 ?2 k, B
73.     }
    2 P- {. t- s# p
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    / x5 K/ p, |) D
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    0 b3 M. _4 }2 n8 A
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
      c+ E/ X+ P$ Q. g' T! Y9 j
80.     {
    
81.         return 0xff;
    
82.     }
    
83. 
    , o4 ^* H5 c- M* A! D2 P
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    8 N) _0 ^8 L5 d( R2 ]
86.         return 0xff;
    / N7 R- g/ `: N1 K' m% y: Q1 `7 k
87.     }
    , {  n+ d* b' E
88. 
    : U- l7 I. ?: y
89.     for(uint8_t k=0; k<3; k++)
    - s, U& @: n6 p$ K. k
90.     {
      t8 `+ A! _5 G+ p
91.         Sensor_Data[k] = 0x00;
    3 Z  ?' X8 W7 o# B6 k6 B
92. 
    3 T$ K8 D" i4 ?8 p/ B9 W4 e1 \
93.         for(uint8_t i=0;i<8;i++)
    2 s3 `% R, B# ]" t
94.         {
    6 h5 m; T" `* N2 I( C
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    & w2 V' G! N8 E1 S: d5 H/ A
96.             HIGH_level_read_time = micros();
    ' A  T; C2 G+ D' h8 `
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    ! H1 g7 ^& {6 u) m* V% E( y
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    : E' L7 Y4 y$ O2 w% _
100.             {
     
101.                 return 0xff;
     ( _) H# T9 s' I8 F1 K
102.             }
     / h* x( K: _2 C3 U
103. 
     . A8 J3 X$ b$ \$ M2 w
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 t+ N, I, L8 Y
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     , s0 n( j2 B- _6 R( D. q0 `! h
114.                 return 0xff;
     0 E3 {6 k% k# q' p8 O
115.             }
     
116. 
     % e: h" n  Y9 h) G9 T% E$ Y
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     4 C8 r* C: x$ L6 B  V& x3 [/ f
119.                 return 0xff;
     % ]/ b) K" }5 `* G
120.             }
     ; ]0 d8 S+ W; ~* x
121.         }
     
122.     }
     3 n9 x# k. G% w! }# h
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     8 s7 O$ h6 g1 B
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     2 k: S" @. a. ]% m9 n1 @% ~2 g' ?) k
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     # ^3 K2 e5 `8 v& T' ^" K
131.     }
     
132. 
     7 C$ j% c# K, C2 u5 ?
133.     pinMode(_DataPin, OUTPUT);
     & [  K: n; l; Y& b. b' _4 b
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     6 T& g9 J) x3 m4 |/ G! ]: E
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     . i- x- ~' v6 y$ X& }2 x
137.     {
     
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     ! L5 d8 G. o2 n
141.     else
     , }& E: a0 `  @3 c) q" e
142.     {
     . U7 l9 ^. x' L8 a
143.         return old_data;
     , a- j+ N$ t7 e
144.     }
     
145. }
     / [) L: @+ w; r; @: ]( z. C& u

複製代碼

) ]1 b, }. j- \5 W4 ^! a6 TMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

$ J8 t. I* M, s/ h- W

強力推廣中

哇...要100 個銅錢

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

3 @! _& ~6 o& m3 }9 P& a認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
) C# m8 x/ x' I0 F+ \3 F能否用到mblock 5 上面呢？

3 T" o8 X# [; h( Y3 E* b. CMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手