mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   ; _2 P4 E. [  @( a
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   5 f" j8 @& g# P3 |  E* ~9 O
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   4 U  M) [- R7 V) m9 q5 W
7. MeLightSensor lightsensor_1(12);
   . M8 U. @2 M4 y2 t
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   5 n7 S  k9 K$ \) T" l
10. MeLineFollowerArray linefollower(PORT_6);
    % L- o1 L) W9 p+ l) v6 m- d
11. 
    ( H! J3 y# o9 b( M) b
12. #define Error1 1
    ) O7 b' `! @$ l) G
13. #define Error2 2
    
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    1 n( T1 Y2 t- n, }  k* Y( C0 u
19. 
    - F8 _0 h5 j9 y% Q7 s! T5 a1 N) F
20. uint8_t sData;
    
21. uint8_t D1;
    7 i& k6 N- _% z* m" I) g$ t
22. uint8_t D2;
    + [9 O) Y! s0 B' d- H! U
23. uint8_t D3;
    
24. uint8_t D4;
    & P# y/ [( F8 W3 K5 q9 F0 W
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    6 y& s0 s7 D) C% ]" ]
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    ; P" r' v, A% t' J4 ?( T
31. int  Speed  = 160;
    
32. float output;
    " n' e* u  ^4 b1 b' W
33. 
    
34. byte Left;
    1 r' n6 }$ q" P4 k
35. 
    9 A8 u5 n; G( N5 {6 [0 }" J. T
36. void setup()
    - D9 O* |6 ?' S* q0 e+ Q
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    & Q6 P5 H  z% I& P
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    8 L$ R! l: v' h) E
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    , a. ?' h& l. h& M
44.   buzzer.setpin(45);
    2 U$ B7 N9 r0 a* Y2 k5 S0 J
45. }
    
46. 
    & Q- y, ~' @7 l& s
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    / S$ o$ i' k, {  C
50.   buzzer.tone(1047, 500);
    3 W0 N& J- w* b
51.   delay(500);
    
52.   do
    * k, K3 M" `; M9 f$ o. z* ]
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    : f" `! d& ]# t: Z/ Y- M) F% B
57.      Left = 1;
    4 k: T# f, e+ {6 t, p7 Q$ ]5 ~
58.    }
    
59.    if(D6 == 1)
    
60.    {
    5 Q' u- V$ `; c- w2 {7 n. ?
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    
64.    {
    & P1 W8 r( V# W: d
65.      if(Left == 1)
    4 \3 d- a. N- l, f; {$ F* s
66.      {
    
67.        Speed -= 5;
    1 F9 G* j6 x: T# ]6 L
68.        moto(0,Speed);
    
69.        do
    9 K9 W$ P/ y  v- g6 }
70.        {
    $ Y! T9 m2 O+ p/ S
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    ) R* u0 N8 f  d# n5 [4 v. A
73.      }
    
74.      else
    
75.      {
    ) L/ Q" Q3 |# b5 P: p8 f
76.        Speed -= 5;
    
77.        moto(Speed,0);
      a; n1 s1 p$ W+ O9 I
78.        do
    . T% K8 z- n8 ?1 G! s- _* O
79.        {
    : N' z* a2 [4 ^0 M$ u. L
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    8 V; ?- ~+ F1 o6 [3 T% {
83.    }
    2 s$ l* _; ?4 _, ^' d
84.    else
    
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    . m- n- A7 M- c. `" g0 _2 d
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    9 p+ |1 r5 S6 `4 b
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    2 q2 \6 i- Q- O  r0 P6 f! c1 j9 V
92.      previous_error = Err;
    
93.    }
    4 O! q: x' K+ L1 O5 v: q+ f
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    - ?3 ^( y- v7 ?0 [) }$ v& {
97.   buzzer.tone(262, 500);
    6 l$ z; P( \- n9 [
98. }
    
99. 
    7 c! o6 Q* p! a) {1 W9 p
100. int getErr()
     8 x- L- U9 h/ O
101. {  
     8 I& k2 W) p; ~5 @0 K
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     - r& `$ F- ?1 H# i. L$ A& c( G
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     3 o# l/ s8 s# J# D
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     - @4 v& K+ d" ~5 h
114.    }
     
115.    else
     + k, D/ H& ?: O% G
116.    {
     ( R  d1 }5 K1 @# x( n
117.     return upD/downD;
     
118.    }
     - X( V: o  t# s. v' y
119. }
     $ c. }4 V: W. E  S0 t
120. 
     9 i' w5 L% x% o) D5 L
121. void moto(int Speed_L,int Speed_R)
     
122. {
     : l3 d" T4 i  O! F% I8 H# x
123.   Encoder_2.setMotorPwm(Speed_L);
     & _8 Z& [$ C; @) h' K2 y- o  W6 D0 F6 Y% L
124.   Encoder_1.setMotorPwm(-Speed_R);
     ) o9 {" y- N  c2 \" E  v! g
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   ! I6 k( j7 ]* f/ n
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   3 ]; @  S! S. n4 g1 m
5. {
   
6. 
   $ d. Q& C+ m2 A" T8 L
7. }
   7 A* r  \$ K' R; o9 h
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   6 S& s; L7 y  J* j# {: ~
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    2 O6 I" C- ?6 j- R! w8 E7 e! A# I
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    / C9 c7 f9 E4 q  W5 i8 a
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    2 Y$ ^4 m% Y# T% b+ ^
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    8 P/ A% @9 S$ v9 n5 u# j
20. }
    . k' C# D! F  S& p  N2 t
21. #endif // ME_PORT_DEFINED
    
22. 
    7 d* M- f3 d2 Z& t
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    , x$ [8 x  r2 f1 `3 c* R
26.     _DataPin = pin;
    * J$ W/ i: K4 R5 W4 ^' n
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    4 t! \! a+ D" a6 K
29. 
    
30.     #ifdef ME_PORT_DEFINED
    ( F1 \. X# `. _; H# g1 O8 a% g
31.     s2 = pin;
    
32.     #endif
    
33. }
      T$ Y9 d' ?/ D1 B
34. 
    $ I8 c0 r9 G3 j3 a9 A
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    9 z$ `: p) v+ p: i, z, |" G1 y2 D
37.     uint32_t LOW_level_read_time;
    ! X& B) C7 k+ q2 G/ n; m
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
      D: W1 q, ^5 W2 @5 v7 H/ c& Z
40.     uint8_t Sensor_Data[3];
    2 A% X4 I& m+ L- Y
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    $ |4 h2 o8 M& t4 z- Y
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    + p4 V, L* S( e: e# a& A
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    : ]4 g& Q+ {6 o2 O1 u
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();
    * b0 \% _. i* m; ]8 ]# _! w; r& c
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    + P! ?: _3 {7 q, I& S
56.     {
    9 o2 F' U, R' s
57.         return 0xff;
    
58.     }
    
59. 
    8 _3 \3 c& k: R7 m1 Q( X
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    . E) K- O) \4 O. y; D1 o
61. 
      z3 G' s( `5 ]
62.     HIGH_level_read_time = micros();
    ) Z: {& t% a- ]% `
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    ' Y3 b/ |2 p; x( j2 z; t! b9 ^
67.         return 0xff;
    
68.     }
    
69. 
    6 X5 C. t. L6 R, ]7 k3 d  M
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    8 B: O1 ]7 p$ _( t, ^
71.     {
    ! @0 }; \2 I' k! d1 N, L
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    7 S( f  B5 S; {! y
76.     LOW_level_read_time  = micros();
    / X0 M0 C# }* g$ Q4 e# d
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    6 h1 n, O' J0 ]. ^4 \
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    
82.     }
    ' N+ g% R1 x( D! [
83. 
    0 Y$ e5 ], j6 _9 Z, G# r; I
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    & e( ^8 ?5 x* k  p) [/ G
85.     {
    7 _4 ^+ {$ b: Y( \: Q$ m
86.         return 0xff;
    
87.     }
    2 I* u( ^' p' `) b. L2 x
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    $ U. C4 Q: s4 v
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    9 N3 X2 ^) M" _/ `
96.             HIGH_level_read_time = micros();
    # T7 O4 x9 }9 G( m$ k+ R
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    8 @# E" e; H! _3 G4 ]3 k3 {) |
98. 
    * Y. M' L- o# V- f) \4 [# K1 Q- X
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     & X$ z% E2 ~9 V& l
101.                 return 0xff;
     4 S/ v: A* f& q( h
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     * S8 m. r' B, S- q0 m4 ^; i: e2 l+ x
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     0 j  A8 a% ~! S- s4 J% _# e
111.             }
     , F5 B0 {3 k6 _: R4 m9 E
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     9 H* @0 W5 x; I% |7 P
115.             }
     
116. 
     " z& |$ F3 M' k4 _: k- o3 E' z
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     ' t$ }) V, g6 u' M
120.             }
     
121.         }
     
122.     }
     " w! E) v5 N! _. U. w
123. 
     # F8 t" \; a" I! C
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     ' P2 e6 ?: E0 L% j0 F
125.     HIGH_level_read_time = micros();
     6 c- o* P. N; h0 L
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     3 h- i+ \! k- }0 {" n; {
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     , p" @! c2 b5 ~. s
130.         return 0xff;
     # z' `5 H1 g/ V# ?1 [; m% O# n
131.     }
     % R0 h- n8 Q4 M( l. Z7 i7 y
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     & K1 M, h7 i0 h$ Z, R6 f' q5 e  `
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     ! `' Y6 `( C2 ^
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     4 R: T6 g# @9 ^  G, c1 x" g2 I; Q  Q
142.     {
     3 u5 m! T! p4 P9 q* r
143.         return old_data;
     
144.     }
     
145. }
     - c! G( A1 _' A+ p; y! F% h

複製代碼

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

2018-2-13 01:52 上傳

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

: H+ d6 ^# \8 X7 q% \! \

強力推廣中

哇...要100 個銅錢

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

5 O7 R1 D+ E9 a認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
4 R" W. y* W$ Y) H6 c能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手