mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   8 |% c! a" t; s6 a( ?; z6 @) J
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   . N) N" P$ A8 _8 k/ W! G+ _
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   / P& D' A6 Y. k. U" _2 b" F
8. MeLightSensor lightsensor_2(11);
   2 Q  j8 `1 m/ \& L& Y7 J1 B9 a2 R
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    ! ]2 b0 k1 V" s: }0 L$ s  G# C0 j
11. 
    1 u* R  O: x- Y+ f
12. #define Error1 1
    
13. #define Error2 2
    0 w. Y$ o; I8 _2 B3 Y* u3 {8 r* d; g
14. #define Error3 3
    
15. 
    & U* D1 U; `. P1 T
16. #define Kp 15
    
17. #define Ki 0.15
    
18. #define Kd 0.03
    / o6 W6 @! N( O& ~3 P; D) s
19. 
    : j4 w% f' y5 j) a" u9 H
20. uint8_t sData;
    
21. uint8_t D1;
    + _  J' v% o# r0 f2 q3 u
22. uint8_t D2;
    
23. uint8_t D3;
    
24. uint8_t D4;
    # d$ _8 N. Y' o) V$ h  q
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    1 q% B2 z. f$ T% G/ V9 k  O+ c8 B- k6 C
28. float previous_error = 0;
    : v* O/ j# _" B; C3 T
29. float integral = 0;
    
30. float derivative = 0;
    ; H. D" I6 u& M4 u* B+ N4 r* r9 w
31. int  Speed  = 160;
    
32. float output;
    
33. 
    
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    7 f2 K8 i% y6 ]: m! ?  c+ V9 w
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    ; U9 ]2 I) i' W% ~
44.   buzzer.setpin(45);
      n' u! n/ |! F5 Y7 Y
45. }
    
46. 
    
47. void loop()
    
48. {
    ! I! @+ m! u3 v% D, K/ R% H. j* F
49.   while(!((lightsensor_1.read()) < (10)));
    % k. p# c7 p2 B4 u/ H7 N8 L0 |+ o2 A
50.   buzzer.tone(1047, 500);
    2 K6 u7 p) z3 U; v2 z2 ?$ b
51.   delay(500);
    
52.   do
    " z$ i$ a7 u/ n' L5 b
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    4 c# p- Y. v- Y( z# g& i7 s' I: z2 }
58.    }
    / b, F& D# \9 R$ g
59.    if(D6 == 1)
    1 Q& _& o4 r7 D( W8 D9 }! d+ |
60.    {
    
61.      Left = 0;
    
62.    }
      w8 G; E& Z3 l0 V  H4 G
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    ( `6 o% |! K: R
69.        do
    
70.        {
    7 p3 Y: q8 U; S8 [" L5 P
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
      b& x2 O2 }- s/ l8 C; o
73.      }
    7 ~2 k5 ^. e9 F" ]& ~8 |
74.      else
    
75.      {
    + \/ [% ^( x3 [( J! a/ |
76.        Speed -= 5;
    8 {- s& S, c+ d5 w
77.        moto(Speed,0);
    6 y2 O5 p3 Q" J5 _
78.        do
    
79.        {
    
80.          Err = getErr();
    4 q8 C' j' |, h! \# c/ ^
81.        }while((D1+D6) == 0);
    7 a/ M7 r2 m% X& B4 E( G
82.      }
    + M( u9 n% n& M& R
83.    }
    
84.    else
    ; C- [$ }- s$ W8 `8 l8 u1 X
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    ; i" T$ X4 u3 h3 V
89.      derivative = Err - previous_error;
    9 T! k, |$ e" a" m5 t. o
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    # f  ^  d" y% j9 l3 l9 l1 v
94.   }while(!((lightsensor_2.read()) < (10)));
    , _- Z; q, T5 @$ t
95.   moto(0,0);
    
96.   delay(500);
    - {1 M' x3 r8 j. K9 v1 \, T
97.   buzzer.tone(262, 500);
    
98. }
    3 s8 O4 P/ \4 G7 U- C( A
99. 
    8 k9 u0 ]* g$ z% \0 @
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     0 Q7 o, {  m; }1 [: b
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     8 G. H5 j+ J6 M& Y8 |
105.    D3 = ~(sData>>2)&1;
     ; v& C, \6 C7 N& I7 w0 ~1 V% ^
106.    D4 = ~(sData>>3)&1;
     $ ?) x1 O% X8 w
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     ) Y  a4 [" T0 c% J6 K
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     
112.    {
     
113.      return 99;
     
114.    }
     6 y; m" v8 k, l+ _: x6 t+ ^
115.    else
     
116.    {
     ' D# D. @3 g6 u9 D. m! r; e7 F; `9 ?
117.     return upD/downD;
     
118.    }
     $ F2 w! x0 }8 o
119. }
     # `$ q  c- I2 n
120. 
     & h# Z# M% [, {' B
121. void moto(int Speed_L,int Speed_R)
     
122. {
     
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     / Q. [/ G# j# f2 Z
125. }

複製代碼

5 v3 V8 t3 w! ]  i5 u0 |7 h- D+ @( SMeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   ! l1 G. p- e1 O( ]# ^0 K0 v
2. 
   0 \/ z- F) t. Q) t! }
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   
5. {
   8 O+ ]2 W1 D* H$ {! x8 N# V2 j
6. 
   
7. }
     Y0 @6 T  V  C2 f' {6 j
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   
10.     _DataPin = mePort[port].s2;
    % I- @, E. k! b! n( D! h' g2 a
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    - }' Z! `0 q6 |* i6 q. y3 ?
16. {
    
17.     _DataPin = pin;
    3 \# V) ?. F( L1 y6 l
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    1 s& _7 ~; x. P$ A7 {, `
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    ( C9 A9 P/ k2 n# w# f
25. {
    
26.     _DataPin = pin;
    7 F+ L. b% K; |2 _- Z5 f0 d
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    8 S. h  S7 d6 D* c
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    * Y; Z; B$ n, L  R9 x
32.     #endif
    $ _5 W0 h, m3 B9 H
33. }
    : K, `9 J8 e7 C' N" e
34. 
    ( x- c( _3 F0 b) b( E
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    
37.     uint32_t LOW_level_read_time;
    ; ?4 x! t3 C( e) U' V
38.     uint32_t HIGH_level_read_time;
    . o# ^4 }& F7 i' `, [( O
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    ' W6 @6 c2 N$ C) n  P. ]
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    3 z1 m7 W4 b- n0 l
44.     digitalWrite(_DataPin, LOW);
    ' @( w4 D8 J& P. h# J
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    + K' ?" C  `5 E. g( q
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    0 G$ q# v/ ~% i9 K- i) z* ?4 J
50. 
    
51.     time_out_flag = millis();
    7 n; E; u6 p7 V) I
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    , U8 y5 ^) x1 I# N( @
53. 
    
54.     LOW_level_read_time = micros();
    ) ?  M, j2 x+ m' Y  @' w
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    , _. W3 E; o& y2 y1 e% [
58.     }
    5 y. l4 Y4 _' _% P. J
59. 
    $ M; T% o5 X& j) \' p
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    1 B/ @: E, d( e- y: a4 k7 h1 e( ?0 v
61. 
    ) ]) t  t+ P4 ]) C: U
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    ( B+ f  f6 X9 u- I8 l
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    5 L, f$ o6 @4 k. |3 C1 L
66.     {
    . o: ~5 w& }( G* y. X6 |
67.         return 0xff;
    
68.     }
    
69. 
    0 l  W9 ]7 e& q  @7 G& T
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    " H( [. E4 s# [/ Q7 v
71.     {
    
72.         return 0xff;
    
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
    " ^. ~9 h0 o, I9 R
78. 
    8 X5 A$ l7 v; }5 E' b* v. `- g
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
80.     {
    
81.         return 0xff;
    ( L9 y; W9 G, r8 O! _
82.     }
    
83. 
    + c* ?5 p) o, K& d+ s, v' ~
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    6 E2 C. B: H8 _& ?4 u) O0 P* h
86.         return 0xff;
    - I. K; S, N/ C" w
87.     }
    
88. 
    ' ~9 J0 Y* n; H4 k+ Q/ C
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
      I0 H! N% w) `# P% _# I/ O; Z8 T% m
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    7 n* {3 i8 s8 o. U) r; o
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    $ y* i! w4 S+ e5 S9 F
96.             HIGH_level_read_time = micros();
    
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    4 p- C# n0 f3 ~+ ]4 d" V" W0 \
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ' `" O' G% v) N! x$ Q
100.             {
     
101.                 return 0xff;
     ) h% U* N+ j- E" ?
102.             }
     ! o9 O, ^1 q' F' c/ y
103. 
     1 {# H. S. n9 R2 `2 U7 s" n# x
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     % O4 z9 i* O/ H1 u7 d
105.             LOW_level_read_time  = micros();
     8 _& r' f6 R9 F+ G
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     8 q2 p+ g7 P7 M6 Y& ?$ E4 R
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     * P# K1 }  O, ?5 K( y
111.             }
     ; d  |. l8 E" }: I! w1 K- V
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     ! \, H& c% ]0 u; B: ?: v
114.                 return 0xff;
     
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     
119.                 return 0xff;
     . s0 I* I* l# \4 r' O9 m
120.             }
     # \' x4 n% s( ]" J
121.         }
     * C, y9 R  X# e
122.     }
     
123. 
     & }7 o% N8 W9 ?, Y/ U4 d
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     - D0 H  e. S7 O+ }
125.     HIGH_level_read_time = micros();
     ! z5 I) h2 ^$ H, @& Z
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     $ b. A1 ?# b1 M' T& \4 l, _
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     
130.         return 0xff;
     
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     
134.     digitalWrite(_DataPin, HIGH);
     & t( D1 `4 D$ C9 `
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     : }. V' _% ]; f
137.     {
     
138.         old_data = Sensor_Data[0];
     - Y" K8 F( y* Y" l6 Z
139.         return Sensor_Data[0];
     & Q" m' b6 M% k, C5 V
140.     }
     & r8 ~2 R8 i' c, i; g9 X
141.     else
     9 U9 Y* e, _% h
142.     {
     
143.         return old_data;
     7 ~* s0 B( ~- @9 q- A2 }- ?
144.     }
       [! D# G; ~  z- a
145. }
     

複製代碼

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

2018-2-13 01:52 上傳

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

2 |. I- W% @7 p

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
4 ^* w4 j' h- |  s0 z. F哇...要100 個銅錢

2 L3 b, o% {) p: }  _6 r0 B7 L% Y認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
6 w$ M5 _1 w2 o( B- Z$ o能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手