mBot Ranger 高速循線範例＿

# Y: B) v, A* i

1. #include <Arduino.h>
   ; r# R: Z! b4 p! U
2. #include <MeAuriga.h>
   . i; t6 k) X& H* Z" O
3. #include "MeLineFollowerArray.h"
   . T1 i; Y" V8 J+ A4 @6 @8 R7 j
4. 
   6 l/ ]: j) q: \; Q) |
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   . G! N, ^) r# p+ S6 m, E. Z5 L
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   ! J# F( k7 M% n' r2 I% ?  _9 G
10. MeLineFollowerArray linefollower(PORT_6);
    + J" {  S0 p/ S
11. 
    8 |1 j* c" V! c; n# `0 g3 M& y, E: O
12. #define Error1 1
      ]7 u" J; d+ k1 L- I1 J
13. #define Error2 2
    6 d( ^: B6 O1 ~0 _
14. #define Error3 3
    
15. 
    
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    5 ]1 B" z' }# f) L
21. uint8_t D1;
    : J+ }2 N, D: d
22. uint8_t D2;
    0 e& x6 k) ?# c
23. uint8_t D3;
    * V" Q! I# V6 Y8 P2 @2 _
24. uint8_t D4;
      B5 r2 K/ \2 k# n4 O  z, l
25. uint8_t D5;
    ' X. I* i8 l, k
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    # r" G5 _3 K1 k
29. float integral = 0;
    * \& @* I' ^2 N5 |; I
30. float derivative = 0;
    ) |$ g$ |- m7 r: x: p/ p3 _
31. int  Speed  = 160;
    
32. float output;
    
33. 
    $ ~& X8 ^, [6 e/ B
34. byte Left;
    ; h' h! O6 @4 E4 u) b1 d3 |
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    ! U4 A  H. i3 Z2 S/ A# [; |
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    ; [. _! ^( G8 ]& [! y
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    " X7 T7 y6 N: E; y
45. }
    ( N8 K2 F/ D# s% c
46. 
    
47. void loop()
    1 E. C! d6 @) ]5 E
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    
53.   {
    
54.    int Err = getErr();
    ) v6 A1 {/ S! R' P# a) C3 s( ]
55.    if(D1 == 1)
    ' F% Q, E6 M' x2 |$ c3 N
56.    {
    ) [* u$ [5 q* _2 f5 P3 B1 f9 R
57.      Left = 1;
    - k4 ^* h  T0 w3 ]* M
58.    }
    
59.    if(D6 == 1)
    ! h0 J0 `* R9 S! w1 y* u% K/ z
60.    {
    
61.      Left = 0;
    , X/ H0 a2 ?0 W0 W
62.    }
    # V& W! _- J" }8 ^. Q+ W+ a  q
63.    if(Err == 99)
    6 B: i0 s& b; Y
64.    {
    
65.      if(Left == 1)
    
66.      {
    0 m- |2 I/ B* U% ~7 Z
67.        Speed -= 5;
    + o1 N% w4 @$ d, S* g
68.        moto(0,Speed);
    3 i$ |5 [. f/ |
69.        do
    
70.        {
    
71.          Err = getErr();
    " |# v  L4 t! b
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    9 ^0 ]3 Q1 g; ?8 V! \, ?1 f! h) E- P
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    . W) W0 K7 N+ b4 r; j+ V
78.        do
    
79.        {
    
80.          Err = getErr();
    6 v- f2 m) [. V# u( L* q
81.        }while((D1+D6) == 0);
    9 R1 u. i/ b& }2 }8 Y
82.      }
    
83.    }
    
84.    else
    
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ) V# v; C" g( M8 H8 k3 M( G
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    7 `% ~" ]' _6 F- W9 j* O2 W
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    & b5 t" O1 c3 \9 y+ }2 a9 [
91.      moto(int(Speed-output),int(Speed+output));
    ; @' z8 \, d) W  k( H
92.      previous_error = Err;
    % e0 J- `& j9 D; D% @8 n! S; a
93.    }
    ) z9 j* D' F% ~$ g& W
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    3 ~2 `" W/ z8 q7 A% y( ^7 ~* r
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    * y& R1 F7 G. h# Y! b1 N
100. int getErr()
     4 S; o/ r5 j& l$ a: N
101. {  
     4 d7 ?3 k: X2 t  ~* j6 V2 y
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     + [8 ~8 M4 ]* q# j, a& I+ I
105.    D3 = ~(sData>>2)&1;
     0 z  I$ P4 @- o  L, a  M
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     # p0 V8 ?' p3 ]5 b9 A% Z
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);
     0 \" s  s5 s  b( @; G
111.    if(downD == 0)
     - \' f# i. f9 R# x
112.    {
     
113.      return 99;
     4 e* l1 p* C6 K( c' P! s5 E
114.    }
     - ~. n. l+ D+ ]- a
115.    else
     
116.    {
     # ^7 y3 G' j) a0 X5 u
117.     return upD/downD;
     ' E+ A9 n- w$ l, e9 u1 [! d
118.    }
     
119. }
     . ^- Z! B9 z$ l$ Y" o+ x1 Y
120. 
     7 |9 [& M, w' I4 n
121. void moto(int Speed_L,int Speed_R)
     $ I4 u; ~3 }+ I) ]9 _& M2 k3 B
122. {
     6 F8 c% {" X1 O+ o
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   ' Q+ p1 o0 Z( g! }& Y' Z( a; A
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   % A0 c. E( f% g3 J1 f
6. 
   
7. }
   # X# F/ B$ c# J; a# {
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   " B' d5 S/ Z& O  K, m, t3 i
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    + m( d6 l- _( G( K
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    / w0 X. Z3 N+ k& L0 A2 [7 x5 I5 `
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    ; G7 @6 H- K0 l5 n& n+ r% x
19.     digitalWrite(_DataPin, HIGH);
    7 b. M) c  e; j4 i
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    + O+ `, |7 Z( Y
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    . Q  _5 q/ ]. G3 M. e/ ^3 z& E
25. {
    1 u. I/ y. f" ^2 F: M! f0 g
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    3 S" u2 K* n. T. O7 R+ ~
29. 
    7 t" A. X) ?! P( M/ z
30.     #ifdef ME_PORT_DEFINED
    4 F2 [9 E9 B7 x: }2 I% Q5 a2 n. k, V
31.     s2 = pin;
    
32.     #endif
    
33. }
    . ?4 h* q% i$ L5 `& O
34. 
    ' D, ~  M) R8 E  a2 d0 E. V7 l9 Z3 _: I
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    / j1 A: A7 N: |
37.     uint32_t LOW_level_read_time;
    / ]. T1 Z- p# W
38.     uint32_t HIGH_level_read_time;
    ! Y' F. j* R3 p& {* S* |
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    
42. 
    $ Z+ t/ d  y) D
43.     pinMode(_DataPin, OUTPUT);
    3 v( `0 u5 e/ ~2 {$ D- S& K
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    6 f$ L0 I* u( V) H* e" N
48.     pinMode(_DataPin, INPUT_PULLUP);
    : f0 v* U8 S+ ~4 a) x, |
49.     delayMicroseconds(10);
    ( T6 O) Q4 [& e8 i
50. 
    " M( k: @7 [! E; m8 D
51.     time_out_flag = millis();
    
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    # ]% \9 z& q* x* b% M- ^
53. 
    
54.     LOW_level_read_time = micros();
    + w0 H5 m/ {) m( v, R: w
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    % b1 n+ g1 F1 R& m; K- E8 ]: Y
56.     {
    
57.         return 0xff;
    
58.     }
    
59. 
    & s1 y& V, u. Y7 o" X
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    9 C6 F$ Z) x% N* S# H
61. 
    
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    " a( _6 Y1 Y4 t' ]$ j
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    * q0 E' y) R) e- I
66.     {
    : \9 ]  \; \1 x6 b% R
67.         return 0xff;
    
68.     }
    . ?* G1 e1 i5 t
69. 
    + ^, [. P9 A4 W. G- v
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    . W( z( ]2 q3 H5 w6 C$ o1 X( F
71.     {
    
72.         return 0xff;
    
73.     }
      l% s  \0 w0 N: b
74. 
    0 |5 s  g7 t" q) {  u1 t) d
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. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
      \+ ]( |7 I: H
80.     {
    
81.         return 0xff;
    
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    9 h# }4 h/ o  e0 t+ g7 F
85.     {
    + w( C1 ]$ l1 g. w; X0 W9 M
86.         return 0xff;
    5 M# R+ u2 ?  t, L, b) v! ?2 {
87.     }
    # d( d) S( n7 }1 V
88. 
    ) S: ^2 a2 t" a/ ?
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    1 w- R# M; K" d5 ?
91.         Sensor_Data[k] = 0x00;
    
92. 
    : L  F% O. d: v$ G' U. p' q
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    % v& [( Q1 |0 `/ D1 \! M
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    ( {: Q3 r) u% F3 `9 G
96.             HIGH_level_read_time = micros();
    9 \' V4 s( P) z5 i& j2 r- o
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    3 p9 X* y: t+ k# A) i( x
98. 
    0 i, {$ i: Q, i& {" z$ V5 n1 Q$ S
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    & `2 g/ L+ V8 G& G+ m2 G( N7 z
100.             {
     + X* m6 {* J1 @$ [5 z, }
101.                 return 0xff;
     
102.             }
     5 D+ E5 ]7 x: d9 C
103. 
     + ~% h) y' ~; L- N. T
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     5 D- X! s( ~5 C
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     : v; @% K2 p+ |: F0 m- c) N3 H0 G( P# h
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     + J8 }9 N6 J+ s: r
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     & m3 ~  r' E2 c% N" G
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     
115.             }
     
116. 
     . T' p: u+ R: O# o: T
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     4 s3 ~( r- e4 t5 |" [
118.             {
     
119.                 return 0xff;
     
120.             }
     ) k9 H! F) l3 i* F# [/ A! M1 l
121.         }
       w6 E& l' w$ l/ i
122.     }
     / B; `. ^8 l. `) N  ^* a9 e
123. 
     % w# R1 i" I7 n. E) l- z
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     1 }6 d- c9 n1 v$ V% N' g, p
125.     HIGH_level_read_time = micros();
     ' ^, X: o% ~5 }! e
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     : [' b* S" S3 G! k
130.         return 0xff;
     
131.     }
     
132. 
       n/ |% p9 n9 t, l, S
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     ! G0 E, i  N; v; F
135. 
     . G. P" W6 ]( Z; T8 I
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     4 |# @! |3 V% x$ j5 V" m, O+ m
137.     {
     2 h4 u8 J0 G- S9 g: U
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     * h. E& D, j8 l2 P; E; T
141.     else
     & ?. A7 _& `2 j/ d1 ~0 b- Y% G% b! c) i+ q
142.     {
     
143.         return old_data;
     6 H- R) U& h5 P+ z% b
144.     }
     
145. }
     

複製代碼

" H4 G7 @9 ?' m5 ?% DMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

4 y; l9 h9 L# I3 S
5 I) t7 o" B0 Y' a; W1 I

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
4 N. ?  z- ?+ h- M能否用到mblock 5 上面呢？

6 ^7 u5 e; N: N5 U% V2 [8 ~MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手