mBot Ranger 高速循線範例＿

- z8 B: K3 @* d  _# N

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   2 \) b1 b( x: w& [. ]9 t
5. MeEncoderOnBoard Encoder_1(SLOT1);
   9 N7 ]/ v& r, h' X' p3 N
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    - G3 O  s4 \' K* b9 d) A
12. #define Error1 1
    
13. #define Error2 2
    . Y4 B7 Q; I% Y) f( Z; X$ l
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    5 h/ u6 X" `! I5 c1 \" O: m* o
17. #define Ki 0.15
    $ b: J# r" X( Z* I  p/ P
18. #define Kd 0.03
    0 z' L) P5 |/ q' U  j/ n0 G) q2 n
19. 
    
20. uint8_t sData;
    . a7 o" X$ L2 Z; L4 ?2 v0 Q9 w* b: G
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    ' W& n5 Q, w/ y2 e' p$ ~1 X# u$ n" r
24. uint8_t D4;
    , K: f3 g% V0 h! Y6 x
25. uint8_t D5;
    , L' F7 @2 D6 b0 E" ?4 |
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    . q0 W$ J8 A/ _, o
29. float integral = 0;
    . H2 k$ o: E" z$ c/ ?4 I1 y1 t; b
30. float derivative = 0;
    7 w' `8 t. m+ D- k. Z( b
31. int  Speed  = 160;
    
32. float output;
    . z& A% D4 M' Z" K  o
33. 
    ' [7 ~: H1 |6 k/ R
34. byte Left;
    ! N5 ~5 j; v% M. O
35. 
    1 U  j1 g% c5 T& v* o
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    4 `2 i5 N' v- W$ u9 U8 R$ J3 H
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    7 N5 B2 r' ]7 _- {
44.   buzzer.setpin(45);
    
45. }
    9 u/ _2 M: {+ O' I7 t
46. 
    
47. void loop()
    
48. {
    : t$ M+ ^4 U9 _' m; E! k  A: b
49.   while(!((lightsensor_1.read()) < (10)));
    # O. P; m3 N) I; x! j/ `# W* H' J
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    2 H( R, w/ N) u, _
52.   do
    % o' L- T* ?" N2 R
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    % L( w1 ^% V/ Z9 D# B- o, B* r
58.    }
    7 q, Y+ ?! }4 t2 p7 q
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    % }: D3 V& ]$ G
64.    {
    ( t1 S* g( g% M- D. g
65.      if(Left == 1)
    & }* L, A: i/ @" Q8 `6 N
66.      {
    
67.        Speed -= 5;
    - X* {  F. H# i
68.        moto(0,Speed);
      K! i; j/ w' j+ d/ N( e* ~
69.        do
    
70.        {
    
71.          Err = getErr();
    . P4 i& y4 V/ M5 M& ]
72.        }while((D1+D6) == 0);
    ; G- g# a+ ]2 N
73.      }
    
74.      else
    ! j+ k5 r/ h) k
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
      T9 o0 s' q2 |. H/ \$ L9 n: [) {
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    8 _6 H/ r4 v0 X4 S; k  \
83.    }
    $ _2 K9 z# V# g. O1 y  ~
84.    else
    
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    6 G4 k+ P0 z3 g5 n) s# [
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    * M. {, R( o- m0 m6 j& c5 j" w
88.      integral = integral + Err;
    0 N$ W0 ]7 e7 f8 C' S- n
89.      derivative = Err - previous_error;
    ) a* c' p+ |8 A. O) k
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    & @0 ]& t. k  x  }0 G7 Q$ W  Q
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    " n6 T; h' O! @; g" }$ e, e+ w
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    5 W$ N# S* ~; A# ^6 N0 G
100. int getErr()
     3 n9 e9 g. e6 b. ^! o: E
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     " B& Q+ \$ B; v1 M
105.    D3 = ~(sData>>2)&1;
     1 n: h3 s9 }. |$ `# h! Q- O
106.    D4 = ~(sData>>3)&1;
     4 s6 M) o( U- v3 \6 q- g
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     / N, ~+ M1 D2 Y& a
109.    int downD = D1+D2+D3+D4+D5+D6;
     + u2 y* w( N% \) r) F- }
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     ! s" l$ R" z! F2 @4 G( G
111.    if(downD == 0)
     . }5 S9 T  w- [2 S" U% P9 T
112.    {
     % u% V1 S3 T, C6 I
113.      return 99;
     
114.    }
     
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     9 x" O6 }6 L& @4 f% M. ?
122. {
     1 b- e- {7 L2 ?/ ^+ ]! j
123.   Encoder_2.setMotorPwm(Speed_L);
     8 F2 p( ~) I; u+ ]) O4 Y
124.   Encoder_1.setMotorPwm(-Speed_R);
     3 |# k, l# n2 K9 \: R4 M
125. }

複製代碼

MeLineFollowerArray.cpp
( w9 c$ ?* Y) L8 }  _$ p7 l0 Y

1. #include "MeLineFollowerArray.h"
   2 ?6 {+ c8 j: K% m5 Y/ }, b
2. 
   $ [+ w; ]  L$ b
3. #ifdef ME_PORT_DEFINED
   : w5 }/ ~; G# J! W9 `% N  a6 `
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   $ J  R" r$ S0 L# [
6. 
   7 h4 ~" Z2 M9 P- l1 f
7. }
   - G) n: g/ S! @; E( o5 h
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   9 }# }7 h2 k: k7 _3 }) q0 g" T0 z
9. {
   
10.     _DataPin = mePort[port].s2;
    ( }0 `4 e, _: ~% Z1 d2 J
11.     pinMode(_DataPin, OUTPUT);
    " d# \/ F" Z6 y
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    2 A9 Q% }5 W- D5 t' @% H
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    8 {% ^: x# q5 Y8 ^
16. {
    , D% e4 U# K( W. g# t* }
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    ( _$ F# t- a" j1 d3 S+ O, a
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    ! J9 t0 G/ \1 l" y' L$ g
25. {
    3 h1 G2 C! x7 k% Q9 M
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    % E  H' [7 y# G1 e1 D' N. ^
29. 
    
30.     #ifdef ME_PORT_DEFINED
    1 A! w" Z" n$ `  f
31.     s2 = pin;
    $ U" w7 V; ]7 y5 g* H
32.     #endif
    
33. }
    2 D( i9 b/ W$ \4 i' y4 y
34. 
      s3 L6 s8 G" o& J( Z
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    9 `2 a6 k3 P- w# T- B% f7 [
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    ' K; s# C5 d4 F/ f
39.     uint32_t time_out_flag;
    ; }& Z" o& Z  u
40.     uint8_t Sensor_Data[3];
    " a" a/ ~/ w( \
41.     static uint8_t old_data = 0xff;
    
42. 
    5 L3 W. H7 {* D* b
43.     pinMode(_DataPin, OUTPUT);
    3 o; h: i; X; A7 N& _
44.     digitalWrite(_DataPin, LOW);
    9 q3 v0 W) u  W# S5 `4 o
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    0 r# }% G( G7 K) m: ^
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    ( m- _/ x/ p: F" N' l
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ! i0 t" |/ [' V& C* R
53. 
    
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ) H  @5 x4 j8 C3 K5 U. `# y* ?0 L
56.     {
    
57.         return 0xff;
    5 ^3 A- Y3 M9 c- H  y
58.     }
    - f8 e4 n0 C. P7 ^* R% n) d' V
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    . d* X7 m; z/ w' t6 D
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    * k2 O8 }  t2 [0 O, p$ E8 a
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    5 c; i$ b1 z" o; F
66.     {
    
67.         return 0xff;
    
68.     }
    ! L6 n4 a6 ~/ U' T: V' s% W
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    : I" |+ X' H0 [/ n% H
72.         return 0xff;
    
73.     }
    # `6 t7 v9 V+ F; A2 e0 ?
74. 
    ' O" H: H2 m3 d. {0 v
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    4 n/ |1 k+ ~' g/ ?: M4 L0 g+ Y
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    5 N4 o4 p/ t2 B
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    8 |! `- R" [* d; M6 [2 J& B
80.     {
    
81.         return 0xff;
    
82.     }
    
83. 
    ( ~, Z9 {$ t* p' c# G" I) I. S2 |
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    ! ?& e7 U6 m- s& @1 u2 m
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    4 v0 @& S3 A6 ~% J
89.     for(uint8_t k=0; k<3; k++)
    8 j! S9 a4 G3 ^9 R7 G) g* r5 L0 ?
90.     {
    & }6 c# L" q# P# L6 Y
91.         Sensor_Data[k] = 0x00;
    + H( z! `$ p0 B2 F
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
    
96.             HIGH_level_read_time = micros();
    " G1 g- h1 K8 Y  w# _
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    . o5 D/ G: k, W9 t6 I
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ) n4 g8 Q& j9 Y" o
100.             {
     
101.                 return 0xff;
     / ?" G( X4 v8 {2 _( M
102.             }
     
103. 
     
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
     * }5 m; M! p# q) \
107. 
     1 e. y7 k3 v$ c" J
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
       _5 I1 t4 i" Z3 J/ B/ z
111.             }
     / |8 o0 T* I  f
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     6 G5 j. x$ Y. f6 _8 C6 u4 e* D: d7 V1 y
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     ; @* M3 Z4 \$ ~$ k3 e5 V
119.                 return 0xff;
     9 O" h4 i# \7 r1 L* R' n; ^- F. z& X' W
120.             }
     
121.         }
     
122.     }
     
123. 
     # m: Q# L. b4 m% m1 \7 g
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     / ?, P6 W& I$ ?4 f$ r
125.     HIGH_level_read_time = micros();
     3 l3 u. U" w# q5 {4 j6 q: ?! z" |
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     6 c2 x7 [' U- u6 \
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     ' r" P/ m; l4 \" g+ E
129.     {
     
130.         return 0xff;
     
131.     }
     3 X2 K6 F# _1 |
132. 
     3 j5 V4 }8 `5 I8 n* t" D2 X" f5 e
133.     pinMode(_DataPin, OUTPUT);
     8 P9 P4 X& i% i) s! y! _
134.     digitalWrite(_DataPin, HIGH);
     : N+ y% [$ T/ V% {) a- R
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.     {
     8 E1 _- e! E0 M# t& V' @
138.         old_data = Sensor_Data[0];
     " o+ u! w9 F* }% s7 }" I
139.         return Sensor_Data[0];
     ; A* b% c* P! k- t- B6 N; _' N% l
140.     }
     
141.     else
     
142.     {
     
143.         return old_data;
     
144.     }
     0 K# p) b# \; X# A
145. }
     8 O2 x3 H2 c3 {/ \5 a% q

複製代碼

" m( S$ ^" F# R- r6 V' SMeLineFollowerArray.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
$ p, h( S( a; G% u* Z( R能否用到mblock 5 上面呢？

) X9 E, u$ s) i6 p' P  j  [! ^MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手