mBot Ranger 高速循線範例＿

2 j/ [2 D0 I2 A6 |, a2 X

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   ( ~' p" y: e3 _$ i7 V3 Q# D
3. #include "MeLineFollowerArray.h"
   . f  o" y9 S5 d/ T' a8 G
4. 
   . w: X; c$ h# b* H& @
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   8 G3 S4 b. ]0 v' f* q% M
9. MeBuzzer buzzer;
   - q& O' Z3 F! m+ d- `- ~: N3 G
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    , R' A6 n* k- \" S% L
12. #define Error1 1
    
13. #define Error2 2
    3 J9 J% y6 f6 a1 O
14. #define Error3 3
    $ j  f) D5 n* m7 t
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    0 ?7 o0 X5 ^6 I# }2 y* S
21. uint8_t D1;
    9 U  M2 E3 K2 g( X4 g
22. uint8_t D2;
    ( N0 |% Y0 v' X3 A1 K7 Q5 B: }
23. uint8_t D3;
    
24. uint8_t D4;
    
25. uint8_t D5;
    ' a: O1 X" s' }2 R  ~( |
26. uint8_t D6;
    2 x7 F5 D. m2 ?( q. U& ^% d1 ~4 R7 U
27. 
    & |: W+ x7 d$ ^2 ^
28. float previous_error = 0;
    
29. float integral = 0;
    % X" z6 z3 r# t7 P* Y& I' @/ i5 R) w
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    
33. 
    % L! |7 T# B! X
34. byte Left;
    
35. 
    2 @2 v( X  n, N& b6 k
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
      {) l2 S- X$ D9 [! M
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    3 [' p% Z5 k( y% z6 l  W
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    
48. {
    + [: D( M2 Z1 A! U
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    + C  Y7 `# p; s$ [
52.   do
    
53.   {
    5 J6 U4 m. o, B4 s1 k1 s
54.    int Err = getErr();
    # k8 m4 n" m8 T* G
55.    if(D1 == 1)
      m; r% A- @4 Y2 `8 Q  V9 b
56.    {
    
57.      Left = 1;
    
58.    }
    
59.    if(D6 == 1)
    3 }: Y$ C5 e8 E# i
60.    {
    : p, H5 a% J5 A' p9 U+ U
61.      Left = 0;
    
62.    }
    6 a3 A2 {4 Z0 Z7 |0 l) J
63.    if(Err == 99)
    
64.    {
    , N, r. w: b8 g  N& L6 e9 v
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    - R' A0 L' y, n3 d
68.        moto(0,Speed);
    
69.        do
    
70.        {
    ; K1 z1 b$ C3 r% q* ^$ b) f4 }
71.          Err = getErr();
    " Y+ f7 |; f9 U2 @( z! V
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    - z4 Q: f# J! m2 B7 C
78.        do
    
79.        {
    1 D, L5 u, k. Q) k! s
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    ) w/ U2 M- g9 C- ~, M; @. \# v
83.    }
    - L$ p- u  G$ [" X6 V  _
84.    else
    : r8 c! H5 i8 w; e% d6 Z) f
85.    {
    8 W, z+ c& Y" G' p
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ( R) _1 a8 E& S* S+ \+ _
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    / i+ M1 e  S% x) J9 B' [% i
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    0 V9 E! Q* Q( a' f
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    ; o1 {* B& X1 T% [4 C. ~' `2 I
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    & X1 b7 l: u4 m& W" t
96.   delay(500);
    # \8 f- a+ y- W5 x! O- E
97.   buzzer.tone(262, 500);
    8 `9 I- {+ l% g# ?) q' t6 P
98. }
    * f& J* y7 L1 S+ I& H" V$ ?
99. 
    / [+ l* J7 x/ v; X) A- y# ?1 o% {2 U
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     . t$ b( Z( q% X( g3 l8 t! q3 O
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     - z- ?( J. j5 E6 n: i3 n
106.    D4 = ~(sData>>3)&1;
       i9 F0 i' S- J& ?4 i
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     1 C( \+ _5 z5 k* E
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     ) b, v$ m9 C2 R
113.      return 99;
     
114.    }
     
115.    else
     ; l7 q/ x& X" m3 z7 j
116.    {
     " R2 c/ h$ A+ W% L! j5 m+ s
117.     return upD/downD;
     , [5 B6 q2 V' {* s6 I8 Q
118.    }
     
119. }
     
120. 
     4 F- t8 S% R( }
121. void moto(int Speed_L,int Speed_R)
     3 F5 q/ b% B7 w1 T
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

. Y+ ]) }5 H0 ]* k) VMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   9 M  E; g2 _' |# k
6. 
   5 |5 }0 a' D' T: H% {
7. }
   ' \: [* e. X% s
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   , C9 ?4 C7 J' F9 Q+ k
9. {
   1 f) R% _8 G$ i# y1 t
10.     _DataPin = mePort[port].s2;
    - ?4 @2 R% _$ g
11.     pinMode(_DataPin, OUTPUT);
    - o* v+ \/ o2 L4 J2 s/ u
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    / k) n1 J0 K3 p9 w+ \
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    % m& |8 L4 z" s9 S; s8 f' r+ x
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    " f* [+ v' q) d+ G1 m, w) V1 O
21. #endif // ME_PORT_DEFINED
    9 w5 j- S: Z: l5 U
22. 
    # j* i% @3 b* y- e* J6 b, b
23. 
    ( K# d9 s" I  X  d
24. void MeLineFollowerArray::setpin(uint8_t pin)
    " c% m; u% A1 M( s( W8 `
25. {
    ( t% K3 y0 J/ `) `" j
26.     _DataPin = pin;
    $ y! w6 k  c" U0 l$ m, Q  ~
27.     pinMode(_DataPin, OUTPUT);
    ) W) N0 g  q( v3 Y
28.     digitalWrite(_DataPin, HIGH);
    + E4 V1 v( `9 D. o; k1 p
29. 
    
30.     #ifdef ME_PORT_DEFINED
    6 ~( ^# E: a2 e. ]: b* F9 Q: a
31.     s2 = pin;
      I. A6 N% W& v1 U! p+ x# _  K  U) E
32.     #endif
    
33. }
    / K- u* f0 W. F
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    . E% s0 T4 q& b% L3 I3 M
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    7 R. u: ^. a: {  U$ U2 ^6 A& m
41.     static uint8_t old_data = 0xff;
    
42. 
    2 e8 o: v1 g3 l* e% D* c
43.     pinMode(_DataPin, OUTPUT);
    0 s8 G. D2 J+ U, l3 M2 t
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    ' H: C4 o, O& e5 a" z
48.     pinMode(_DataPin, INPUT_PULLUP);
    + i; [# {* A' [7 Y( B
49.     delayMicroseconds(10);
    # F4 ?' X; h# W9 Y' L
50. 
    6 w' D. ?$ }' {
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    6 {4 ~% y3 P1 L  |$ d  n
53. 
    , w& w- h/ h! y$ b$ ]
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    5 F8 y: s% [" ?, a  a
56.     {
    
57.         return 0xff;
    5 Z  C1 x2 I+ S( F% z
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    ! y! |% h* `; Z- i) G& V
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
    " [9 t% i0 F+ G' y
66.     {
    
67.         return 0xff;
    1 m( A3 w8 H8 P
68.     }
    3 @; j" `( h: S. p& D; u
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    9 N5 x3 N+ `2 Q3 [
71.     {
    9 ?2 A5 s  ?) s* Q
72.         return 0xff;
    # A; f8 r6 i. i9 }$ y4 N8 Z, ~- N
73.     }
    # \0 V0 U& m2 f( k
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    8 Q$ Y5 f/ {" W' t9 D5 R
76.     LOW_level_read_time  = micros();
    + }4 r4 I. ?: C$ B# T0 V/ J% U
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    # G& E& E$ A. A! I1 t
78. 
    1 W4 J& i6 O9 i3 J$ [( P
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    2 z3 _3 h$ J3 @- O) G  E' r' O
80.     {
    
81.         return 0xff;
    ( J9 g6 A2 g+ a" p8 O
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    # _2 e3 @, F# o5 R
85.     {
    ) Y2 ?, n2 o0 V5 j# l
86.         return 0xff;
    . C5 S! `* D4 H5 u0 Y; I
87.     }
    + |2 R3 A  M! D
88. 
    $ z1 n8 D* x' A+ m! H2 h* W
89.     for(uint8_t k=0; k<3; k++)
    2 ~) q' f0 G! M8 ]
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
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();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    " Y$ A8 F# F) ?
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     
102.             }
     ! d9 D( M* b) N# n9 M1 {8 ^$ d. P$ Y# \
103. 
     $ S6 c5 x3 i3 I* ]2 a7 k$ e
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     + ]7 P4 W% \; t
105.             LOW_level_read_time  = micros();
     : C8 b; w9 B* n" u9 q) o; G7 R
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
       ]$ F7 V1 Y7 l# z( s
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     7 z, R2 _, I9 p: m6 Z
110.                 Sensor_Data[k] |= (0x80 >> i);
     0 D' W. a$ O  y, W
111.             }
     0 e; i$ ?* J# B3 w4 c
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     9 p+ t- K0 O% M: |& b$ c- [
114.                 return 0xff;
     # n; f4 _# ~- v9 J0 H. k2 B% B9 t! E
115.             }
     
116. 
     7 Z+ X2 o! V) K$ y# J2 M* V5 @9 F
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     * Q/ |0 L% J! H2 N' A' A" Y6 Y
120.             }
     ) `) G6 q5 _+ a6 P9 w
121.         }
     , Q, a- ?$ @1 s' `. G! T6 Y
122.     }
     % K$ B& s9 f/ l7 h3 I# u. ]2 H
123. 
     ( J+ R' \& M7 X$ V. d
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     7 ?2 R: f$ |* X& q1 e& o
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     8 z) ?7 K3 ^& E: b" |
127. 
     ' K7 ?; i$ ^  v2 m) R9 d
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     0 @) ]  L$ T+ A% p
130.         return 0xff;
     : K/ z! _: l2 L/ N; Q+ O
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     ; x0 ~7 h8 u0 e
134.     digitalWrite(_DataPin, HIGH);
     5 a! O- u+ i/ l2 |# }( z
135. 
     ( X  v) p; @, t  }
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     + Q3 j) V# m' P! O
138.         old_data = Sensor_Data[0];
     # S/ u, w. N, H$ p
139.         return Sensor_Data[0];
     & ~2 H7 d2 S4 c2 D+ _+ p, l; {' j
140.     }
     
141.     else
     
142.     {
     6 i( U# V; `! x" w& a+ t
143.         return old_data;
     0 k  R' T. o3 V$ D; C- L% O
144.     }
     0 ^/ C! [1 ~* g  v- D! }5 R
145. }
     ! N* e; k5 N& Y2 j

複製代碼

7 H& E0 l# j1 k/ g% dMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

. P6 W( X8 ?, V' y
4 J$ {! X. \; R8 f% o8 \( b

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
2 t: T1 u1 L7 v/ c哇...要100 個銅錢

6 G) i+ m( Y3 s# t, v9 X0 G認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
" W  G3 ~" i# B% l9 f2 j! }4 W能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手