mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   " U4 X0 p! e/ q$ o" d2 Z9 j/ ]
2. #include <MeAuriga.h>
   , }* D- d* {% z& F* C  R2 ^
3. #include "MeLineFollowerArray.h"
   
4. 
   ! ~, M( P7 [8 O# v, W; \
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   8 ~& o* z6 T. Z5 W: j& S6 |' U5 j( N
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
      Y1 ~5 @/ S5 w# X$ b, u
11. 
      |* F# e1 U$ r# q
12. #define Error1 1
    
13. #define Error2 2
    1 K0 Q) c5 i: ]" c$ H2 q0 n
14. #define Error3 3
    5 L9 }+ R& Z0 i- L, _, N  J
15. 
    ! q9 E; T7 k, R/ ~: l; ~# c
16. #define Kp 15
    
17. #define Ki 0.15
    6 \' K, u8 V. e7 m  r, o% U9 A
18. #define Kd 0.03
    
19. 
    ) V- y! I% n' u$ |
20. uint8_t sData;
    0 j. g( I; `% }7 ^* t) k
21. uint8_t D1;
    2 {/ S* e5 e* u. C
22. uint8_t D2;
    
23. uint8_t D3;
    % b; L6 b; W1 q' T; J" N' z
24. uint8_t D4;
    2 @( p! W1 I3 y" A/ [% w) j$ \
25. uint8_t D5;
    0 G: V+ k1 o+ r
26. uint8_t D6;
    * m- G& z5 B2 e4 ~8 O5 W
27. 
    / \5 ^+ b) ^/ r1 j0 J: c
28. float previous_error = 0;
    ; v+ V/ h; y. {6 Q- T. q! C
29. float integral = 0;
    ; E# P  \! C- Z3 _" Y4 [
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    
33. 
    & e: R% H; J$ Q. w
34. byte Left;
    
35. 
    5 W: b6 C/ I: H6 t  k! s' B& ~- d
36. void setup()
    
37. {
    ; {" t" |- A7 m# ^" F$ {
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    ) R: i8 H- q6 X, o7 h
43.   Serial.begin(9600);
    * i) P$ v& n9 e$ W
44.   buzzer.setpin(45);
      R% O- z. N/ k) E' w
45. }
    8 W' \) O8 _6 e; I4 \
46. 
    - m7 l9 O4 v5 m/ i7 [
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    " ]4 f: U- U4 Q6 D
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    ! m) R- ?7 |% o
52.   do
    
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    7 l; @, i* N8 Y7 J
57.      Left = 1;
    
58.    }
    5 E% z1 p0 ~# p; d4 ~; c
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    
62.    }
    
63.    if(Err == 99)
    
64.    {
    % o$ Y4 V: B. z6 |) Z
65.      if(Left == 1)
    ! D8 E3 y  T# ]  c5 t; p
66.      {
    
67.        Speed -= 5;
    # t1 `( O+ ]( l6 Y
68.        moto(0,Speed);
    # @% z# s! P- @8 p
69.        do
    
70.        {
    
71.          Err = getErr();
    - Z8 S5 `, N. g
72.        }while((D1+D6) == 0);
    
73.      }
    . L$ i6 ]% w  z4 A
74.      else
    " k4 ]1 }/ l: R/ k5 g+ n
75.      {
    7 u& C0 E* ]$ g8 G. [8 n
76.        Speed -= 5;
    
77.        moto(Speed,0);
    ' h' D0 _+ @6 d& j0 x
78.        do
    ; q9 C1 i  f) l& t3 A9 M( E
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    2 ]) j, p! Y' L: V" L" ~" R! Q
82.      }
    
83.    }
    $ h- W% |4 }3 ^2 ~
84.    else
      W) s8 O: h) Y3 X' v
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    2 |1 }( |; F( c1 C* Z
89.      derivative = Err - previous_error;
    - n2 A* T3 V9 z
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    7 W/ ^2 q* h0 x( S
91.      moto(int(Speed-output),int(Speed+output));
    1 G: u, b; x& ]) D" q, Z
92.      previous_error = Err;
    
93.    }
    ! [. U& E( d9 J8 B# w! r: B' l( V* I3 G: B
94.   }while(!((lightsensor_2.read()) < (10)));
    7 z5 l  _( i7 j) j$ v
95.   moto(0,0);
    2 C7 ?0 h- A; {9 j4 f) Q
96.   delay(500);
    - v! }) s: E( A1 X7 S. T; Q0 t
97.   buzzer.tone(262, 500);
    : V8 c4 z( N9 m! u# K
98. }
    7 ]/ i/ B' F1 S/ r% Q: p9 ~
99. 
    
100. int getErr()
     . z) E5 n) F8 `- E% ?
101. {  
     : T/ k) i1 C) \4 [
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     ( W3 D3 Y8 n3 d; b7 X1 Q5 D
105.    D3 = ~(sData>>2)&1;
     ( K8 N( d' v3 r# d2 _7 U$ W
106.    D4 = ~(sData>>3)&1;
     8 M  |1 y+ e% ?8 r3 I
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     2 X! M" F8 f9 e& K9 ], a% m( V# z
109.    int downD = D1+D2+D3+D4+D5+D6;
     1 r3 h) w* Q- K+ W# ]& t+ q& y
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     $ [  g$ q; ]; N8 r
112.    {
     $ ~# J2 ?0 h( c6 e. \
113.      return 99;
     / w9 j+ ]$ {# ]6 |% e  a5 ]  N% |
114.    }
     
115.    else
     
116.    {
     , L( v* u5 k: |  u: U: `; {
117.     return upD/downD;
     & s+ H2 e: z, j
118.    }
     
119. }
     
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     , e& x( C: U- u+ Q
123.   Encoder_2.setMotorPwm(Speed_L);
     ) p6 `4 P' b, I# k* x* G3 m
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp
4 O$ J3 I; `9 c& o8 [6 H- _  e

1. #include "MeLineFollowerArray.h"
     S5 E$ J3 s, C% ?! y3 g) S
2. 
   0 y6 S) k2 n) `& X
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   1 R, B) d& O* j: k0 \9 Y
5. {
   
6. 
   : J) ]/ a& k+ D! d5 z
7. }
   1 q$ W3 G6 j# H; b( F6 L; A+ \0 Q
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
     j6 N5 A% H, V3 O! z# R' ?0 h5 u5 g
9. {
     [! q7 e& H+ x
10.     _DataPin = mePort[port].s2;
    / U0 r5 C* P/ f6 V) T  l
11.     pinMode(_DataPin, OUTPUT);
    2 N4 @; R7 f/ ?& a/ V2 l  y0 \
12.     digitalWrite(_DataPin, HIGH);
    $ _: S* u( K* u# r2 f# U3 V/ R
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    ( g, b. J' B7 Q
16. {
    & A& Q. m- ?3 H7 r6 i) v
17.     _DataPin = pin;
    & X" \' a, g! Z/ R1 Z. ^: `. Q
18.     pinMode(_DataPin, OUTPUT);
    ! X" O' e+ G; G/ Q& |; Y* }
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    . Y% s/ L9 ^" L5 C% s8 G
25. {
    $ a0 h6 c9 F# j5 {3 y% I
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    + Q8 b# y( ?& a3 K2 h0 N+ N+ s9 f
29. 
    - P8 `) i1 Q& g. A+ V* w2 D
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    
34. 
    7 a& Y: ^8 w5 r$ h% ]2 g1 G
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    & `: F' H7 }# x
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    : S  Z. r/ z4 u+ r- n- q
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    ! }, Z$ w* x& ~% u$ Q
41.     static uint8_t old_data = 0xff;
    
42. 
    1 y. r9 {# l0 a1 W& G
43.     pinMode(_DataPin, OUTPUT);
    , e- R( I7 `6 L, W" @1 ~  L
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    4 w; E8 v4 |0 V+ g: {! X8 z
46.     digitalWrite(_DataPin, HIGH);
    ! @' X/ x" F. s) H
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    ( M" J2 _* b# t8 K! h/ H
49.     delayMicroseconds(10);
    0 w5 }* _4 u* ^
50. 
    
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    . }8 n8 [) q1 X$ M6 C0 |
54.     LOW_level_read_time = micros();
    ! a3 g  I7 r/ f
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    / `) e4 [: r4 f; u6 Q8 [& j' t
56.     {
    # m& d$ r3 ?  u+ p7 e7 W* q
57.         return 0xff;
    # Z! H5 N0 @7 ], ^8 J
58.     }
    3 m! e) W$ ~; t8 b0 c- L
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    " y/ N. J5 w- }& W; s
61. 
    7 `5 t1 P4 ^+ E3 r, a% _
62.     HIGH_level_read_time = micros();
    
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
    + P" k9 X9 m# I3 t+ n& H
66.     {
    
67.         return 0xff;
    
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    , V8 [# P1 {5 I1 C1 T
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    " f: }/ D0 L; D, U6 H' T
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    8 Y% V- v0 n9 h2 \; k
81.         return 0xff;
    
82.     }
    9 X; Z9 L1 h' D' z* s- x
83. 
    0 O! r" q, O0 D# A% M
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    + N8 Q8 T4 L7 N9 F" t# ~. ~
85.     {
    & x; c) Z7 a8 M* w5 A& w3 W
86.         return 0xff;
    
87.     }
    " D' U$ [+ @# J4 i. P
88. 
    5 y( ?8 \6 o% @! A! ^7 Q
89.     for(uint8_t k=0; k<3; k++)
    / a! ?8 Y. p5 y. W1 Y* ]& |5 p
90.     {
    
91.         Sensor_Data[k] = 0x00;
    ( c7 F, @7 t* I$ U# ?
92. 
    7 V7 y1 S' t7 N' Y# h
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    * J( v9 X/ ]/ ?1 e
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     - M: P. v5 \% H4 `: ]; t
102.             }
     
103. 
     : Z; W4 V3 V. b6 t3 G) ^
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     3 B3 R8 {: f$ H$ x8 W* `# S0 W% f
105.             LOW_level_read_time  = micros();
     ) `% q( U! _7 I
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     % X1 [/ F2 J8 g& }% L  ^
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     + L2 [+ A2 r! [9 H, s- x) \! j
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     9 O9 |- d0 R4 n# c: P" P/ Y3 P
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     * @4 j6 h/ a0 `1 ]6 m, X2 L
116. 
     3 _) E+ {- i5 C
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     # D" j4 k1 E$ T8 g1 p0 t0 o6 o/ |
118.             {
     : P4 N( `! D. j" d( ~( j
119.                 return 0xff;
     
120.             }
     : q  e; Y- |" n% W6 m: A: E
121.         }
     
122.     }
     
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     6 N6 k& p$ K' N" _  ^
127. 
     3 Q4 r$ Y8 \' t, C# j
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     * ]2 l7 U0 o0 r+ \( ?6 `
130.         return 0xff;
     
131.     }
     + {8 b& B7 [& x* W; w3 F
132. 
     9 n. @. ?+ p" s/ M7 R
133.     pinMode(_DataPin, OUTPUT);
     $ ~) @; f$ w) Y" D" g
134.     digitalWrite(_DataPin, HIGH);
     5 D9 K3 d6 H  W: E0 N0 \  p8 g
135. 
     : ?3 i  F6 e* F
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     4 B/ K5 p. E: p! h5 S
137.     {
     , R: S4 P! _6 h& m/ ?
138.         old_data = Sensor_Data[0];
     3 y' v% @  G8 O7 M, E# L
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     8 @! N& w6 o% q# R& |
143.         return old_data;
     , Z. c! E) J7 \9 y( X9 I0 s1 s
144.     }
     
145. }
     - A* |2 c- ?* |! d

複製代碼

3 |# H% ~: {& ^% HMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

: Z7 B* W  n0 x- c: {3 @

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
- x8 Y( U4 a8 i& Z4 Q8 J* g能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手