mBot Ranger 高速循線範例＿

+ j1 O. ~, m9 }) ?  J

1. #include <Arduino.h>
   
2. #include <MeAuriga.h>
   0 z& ?$ T2 C) W/ {) k
3. #include "MeLineFollowerArray.h"
   
4. 
   8 ]4 [5 \6 t1 v4 ~
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   ) A& s% L) ]! U/ T+ P
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   1 K0 z# Z% N; w% [# r  f
9. MeBuzzer buzzer;
   - q  S' Y' F7 C
10. MeLineFollowerArray linefollower(PORT_6);
    " R2 L; {$ f$ M% N- u* G. L* w
11. 
    
12. #define Error1 1
    9 X* Q5 ]8 p' e7 g7 C9 v0 r
13. #define Error2 2
    
14. #define Error3 3
    7 L* E4 ~# g3 X! H# g
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    # {# C8 k6 v! T$ f+ Z5 w
18. #define Kd 0.03
    ; O+ V  E* F) d) [6 F& f
19. 
    2 O, [5 l) [4 ^6 Y
20. uint8_t sData;
    0 S8 \. Z+ f, N: s
21. uint8_t D1;
    7 Z  ]' G. T: L& V3 L  p
22. uint8_t D2;
      M! H& G9 X8 ~& Q
23. uint8_t D3;
    $ [' v' D. R# J, F
24. uint8_t D4;
    
25. uint8_t D5;
    
26. uint8_t D6;
    ( p5 {& r) l. Z; s4 K) U: _
27. 
    & s$ g1 V0 `# n2 k9 v, ?/ O
28. float previous_error = 0;
    
29. float integral = 0;
      O7 F7 |6 t. j. W! f" A
30. float derivative = 0;
    & U- J3 k# _1 A6 t
31. int  Speed  = 160;
    
32. float output;
    ' W( Z3 Y7 `  n  g$ q
33. 
    
34. byte Left;
    . b$ M2 e- q2 x  W+ B: `
35. 
    
36. void setup()
    & i3 c* G& ^6 F% w* z" j
37. {
    - n8 ?" ?# s9 _
38. //Set PWM 8KHz
    6 w3 m3 j+ {9 ~* U) d
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    8 {4 j% I) d0 R1 U
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    ( [/ \7 R( R* F- P
42.   TCCR2B = _BV(CS21);
    7 G, z. G* w$ j1 G( k+ O; C* L+ a
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    
46. 
    
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    8 m5 b8 d' @& ^& o
51.   delay(500);
    1 p0 c& c. T0 T7 n: X
52.   do
    : I( r2 L, {* U( h- n" a# Y
53.   {
    
54.    int Err = getErr();
    % F0 K  G5 _% j$ a
55.    if(D1 == 1)
    
56.    {
    % s: @" K8 \1 O0 k3 J( A, B
57.      Left = 1;
    & k+ |3 |' q( l# q$ t
58.    }
    ' p' N' o- o1 _/ d
59.    if(D6 == 1)
    
60.    {
    # F( e: F! C$ o# P
61.      Left = 0;
    9 h- Z' k" w. r+ V6 {
62.    }
    
63.    if(Err == 99)
    
64.    {
    ' d# C' u( h( m0 R# h4 j; _$ H
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    . \3 W- V- Z$ d# i8 Y
68.        moto(0,Speed);
    
69.        do
    : P" _1 S* f2 T& c/ F* p# k; W
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    
76.        Speed -= 5;
    . h8 @5 F* U4 W* r, f  @
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    0 D) P( \7 {' G2 B3 W  I: k2 H
82.      }
    
83.    }
    3 j) D& t$ j' a
84.    else
    
85.    {
    . ^4 u& Z/ \' \3 \- @
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    % M' B3 Q1 H$ x9 c, Y5 g
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    ! ]& ^2 `. ?' ?
91.      moto(int(Speed-output),int(Speed+output));
    0 V" i# U, \4 W" h6 P9 x
92.      previous_error = Err;
    ! ~6 C) ]/ u, K) D' Y  ?6 C, o. W
93.    }
    1 v' A' f5 p' s4 t+ W
94.   }while(!((lightsensor_2.read()) < (10)));
    - @1 F# H6 f3 k% o; K
95.   moto(0,0);
    
96.   delay(500);
    / O! _/ g6 P1 P! ]- N
97.   buzzer.tone(262, 500);
    
98. }
    , ^! s4 H3 h* d2 B' f
99. 
    
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     % V' p: u1 N5 f# |
106.    D4 = ~(sData>>3)&1;
     . e" B, l$ m5 k  x( O
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     " S) d% h: P# O9 P; E
109.    int downD = D1+D2+D3+D4+D5+D6;
       ?0 E% h) q6 Y" i7 a6 q
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     % N% B5 o: [+ H9 l6 F; [
112.    {
     ) v* P& ~- Z" g
113.      return 99;
     5 s$ V; U$ t/ K" X( @$ F
114.    }
     
115.    else
     5 Z# }9 h# Y" _: L+ E9 n; M
116.    {
     - L5 [( L, O7 W; [( T) N1 Z
117.     return upD/downD;
     ( s' H  [4 B; N6 O% T
118.    }
     
119. }
     * y# B% @9 J  A! |
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     & g5 e) F; F2 ^! ~, ], a
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   , e" L; |1 S2 A
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   " V  h3 i* ~. M. U
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   . A4 j6 }: b( q, P& h  [- l
9. {
   
10.     _DataPin = mePort[port].s2;
    8 k; Z/ }/ y6 T4 r5 L7 b
11.     pinMode(_DataPin, OUTPUT);
    + }- T+ |  g$ `
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    & c8 E0 y& A2 \8 S2 n6 K
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    1 A+ q6 R$ {2 h4 ]% _
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    1 _; G' q: w% }, q
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    * Y8 k/ J* N' ^  i. F, |' D
21. #endif // ME_PORT_DEFINED
    
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    ! n  ?2 G4 G3 I: P- s$ k5 B
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    ! \0 ]* ^/ y. @4 `3 x2 h
31.     s2 = pin;
    
32.     #endif
    - v: L5 k- l" v4 A# B5 f0 X
33. }
    # w; v! F6 ~4 Y
34. 
    $ e1 _- [2 M3 O* `
35. uint8_t MeLineFollowerArray::getValue()
    3 u/ r' N$ t6 z4 Y+ _+ s2 S
36. {
    , ], m7 h# [( t" u, v
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    ' Z- N, F. e# P7 `, |; @( h
39.     uint32_t time_out_flag;
    $ a. r. Z8 t9 G9 X7 n/ E
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    " O* w& v8 Z# O
43.     pinMode(_DataPin, OUTPUT);
    , _! i/ T' j. [) N% F
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    - r/ w( w' \+ W
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    ' ?/ Z* N; @( C0 ^: Q2 _
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    5 U' ?6 P$ K5 Z  x$ u4 ^
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    9 e. Y' {/ |% y% y! A- @
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    / n3 s4 j  @  ]  i# C/ e6 S
57.         return 0xff;
    0 W/ U  u' j, t5 h
58.     }
    
59. 
    : Y% x( p! b2 y+ F/ k, G+ _
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    # l. H2 |0 I' v& \
61. 
    . E# m+ z3 K* K' `  d. M  P
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    + f' \% |* W2 H/ ~0 j
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    5 Z" @* `: A1 s8 U! y/ ~6 ]; t* C
67.         return 0xff;
    
68.     }
    
69. 
    1 S- ^5 @# j: m, O
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    " j( J0 f+ Z! w; T
71.     {
    * j  B5 i  i2 g# q6 ~
72.         return 0xff;
    7 f; }% a! q6 r6 n' s+ Q
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    
78. 
    8 C4 J# q% u" `2 E
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    % m0 G1 p# q2 l6 s( q: w
81.         return 0xff;
    
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    * t2 n/ T# o& t. U- [
85.     {
    
86.         return 0xff;
    
87.     }
    
88. 
    9 j4 `/ I3 c0 m7 Q7 r4 h/ J
89.     for(uint8_t k=0; k<3; k++)
    4 d( Q; p  F0 t/ G3 p) y, F9 ?
90.     {
    
91.         Sensor_Data[k] = 0x00;
    3 a8 U& b. R/ t1 b) u$ X
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    ) c$ ]+ X# s# ]: [
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    9 U4 g8 u6 z' @* Y( {
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     ' B7 g; T2 Z: ?7 a& j' J
101.                 return 0xff;
     
102.             }
     " i  b8 p0 f( }
103. 
     2 D& t4 e0 e; B) s" c5 @2 L$ r. c7 i
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
     
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     5 r9 B: \% U2 c( u
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     0 b- N* b( {2 t% z
111.             }
     ; C+ |2 l" i: G2 U& I9 E
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     : q: @. J8 [/ s3 u2 |, L
114.                 return 0xff;
     
115.             }
     
116. 
     & R+ t* S' C0 S' P- v6 b0 O- l
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     * L8 O5 X$ @- C$ F* w
119.                 return 0xff;
     : s# ^% R3 c" H6 b& d
120.             }
     
121.         }
     
122.     }
     . d4 }+ i/ N4 i" q$ M
123. 
     * X8 f2 j# T! c  T& r, J6 `
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     " ~1 Z: j, r7 A6 b$ g4 A
125.     HIGH_level_read_time = micros();
     
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     % N4 [5 ?, r5 b# p  b" e
127. 
     3 T( i6 [: d$ J6 K9 X
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     " s: m, x" }6 g, V( `6 m4 {3 a
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     3 J% b/ `0 [/ Y* R
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     . ^" ]7 q7 a0 ^0 U$ U2 H
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     # k3 F( O& h. b$ b
137.     {
     & {! o8 s; X3 w6 n' X: B; q
138.         old_data = Sensor_Data[0];
     1 z: I8 g1 f9 t6 n3 [
139.         return Sensor_Data[0];
     . A0 E  ]: {5 T, }$ W! X$ M
140.     }
     0 Y3 A8 K: c0 s, ]
141.     else
     8 k" d& ]- w8 p( t' v1 d; b* T
142.     {
     
143.         return old_data;
     4 H+ d/ v1 _, m
144.     }
     0 g3 @' _2 R3 |7 i
145. }
     % P. c% F) V( ^( o/ O3 J4 N

複製代碼

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

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

0 ^, ?# w- I+ n+ f7 D( E8 _認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
6 e/ L: D% y0 o能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手