mBot Ranger 高速循線範例＿

( V( z1 u3 y4 U7 c5 I% E

1. #include <Arduino.h>
   7 d" y4 s  s; ]# y( }& H, S
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   7 }4 Y, {/ {2 e' b( i: t; u
5. MeEncoderOnBoard Encoder_1(SLOT1);
   / w5 W4 |' C5 a! M  O- Q: ~& x7 v
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   : s, q% F+ J8 c% A" D1 _: `
9. MeBuzzer buzzer;
   3 D0 ?! g# [0 K+ ~
10. MeLineFollowerArray linefollower(PORT_6);
    & {' Q6 C! g7 ]
11. 
    ; g6 \" }4 J7 W( f+ w" `$ C
12. #define Error1 1
    
13. #define Error2 2
    
14. #define Error3 3
    ) v" P# T& X* N- j$ `
15. 
    2 i. y  N& o! v! H1 [
16. #define Kp 15
    % Z3 v* ]1 F3 v9 l
17. #define Ki 0.15
    7 F7 y- `$ Y7 d2 i/ U( m
18. #define Kd 0.03
    
19. 
    + ~+ i# e2 ]7 f3 Q  x
20. uint8_t sData;
    6 o% @8 O0 O$ Z
21. uint8_t D1;
    
22. uint8_t D2;
    , @  F+ w' F# H5 x- p4 o; f
23. uint8_t D3;
    2 V0 f9 f2 `6 m
24. uint8_t D4;
    
25. uint8_t D5;
    " N& k- s5 z2 g! A* }( w
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    1 M9 f; _* Z7 \8 R( O5 i
30. float derivative = 0;
    ( o* v' b. R# S3 E' \
31. int  Speed  = 160;
    
32. float output;
    
33. 
    
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    , b  s2 w8 i& u% }: [
38. //Set PWM 8KHz
    : a# p: j% s4 K3 T* o, V) c# N
39.   TCCR1A = _BV(WGM10);
    
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    
42.   TCCR2B = _BV(CS21);
    , O2 `6 A  o% C
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    ! J3 f9 b3 h0 c: y: c! ]
45. }
    
46. 
    
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    9 s- f0 ?0 e& S; |
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    
52.   do
    % t: \  _0 w, X: e& m, L$ c
53.   {
    
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    / u* L$ Q' t- H7 R5 N. W+ k0 e
58.    }
    
59.    if(D6 == 1)
    
60.    {
    # `1 G; v9 x+ t4 L! o, D0 H" o+ {
61.      Left = 0;
    
62.    }
    , Y' F! X8 g. \9 Q( l
63.    if(Err == 99)
    
64.    {
    $ o6 T" T4 @9 I, o
65.      if(Left == 1)
    
66.      {
    3 k3 z6 u# ^- ]% {, b& E; N+ d
67.        Speed -= 5;
    " Y- i, N/ J" l0 F/ w
68.        moto(0,Speed);
    
69.        do
    
70.        {
    
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    ! \% O- M2 ?' M/ \4 W
73.      }
    
74.      else
      }7 ^4 ^! d% c, s3 j& O
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    ) j8 I& D4 ^  k0 D4 n
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    
82.      }
    3 ^% O$ x% d3 ]# T
83.    }
    $ p# v4 g9 Y2 r! A" x
84.    else
    & T+ Y) D1 b: d! h1 R( Z; |
85.    {
    
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    ) [% X. d* l" J% [* ]" j+ N
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    * s' R1 g% Q0 z  r+ T0 E
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    . W3 J2 C7 O8 d5 g& n. Z3 s" }5 o
92.      previous_error = Err;
    7 U! ~9 J9 C+ J- \, R- `; x. T) T
93.    }
    1 f) K: ]! g0 J) s# ?
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    
96.   delay(500);
    
97.   buzzer.tone(262, 500);
    
98. }
    # }3 K2 y: |3 ^- Z- T- o; v
99. 
    / y3 ^$ K5 u5 G1 F4 a" U
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     ; s" a4 h8 a0 k3 l' x
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
     & a% U8 O; k9 n& j
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     / K# P, i: C: K* G1 l4 }
108.    D6 = ~(sData>>5)&1;
     , X* ?* j* Y- A: C* ]
109.    int downD = D1+D2+D3+D4+D5+D6;
     
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     / f# ~: |- r; ^
112.    {
     ( o* y* x$ O( o" A' n1 g: m
113.      return 99;
     
114.    }
     8 g- j8 A% C: h
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     ( K, q* _, A# M
119. }
     4 \  Z6 ?% Y/ m
120. 
     
121. void moto(int Speed_L,int Speed_R)
     ! W- i$ O8 @% F0 ]
122. {
     ) a8 l0 B/ p0 p( A
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp
2 s  X, [7 v; n: g1 ^6 E9 |& H

1. #include "MeLineFollowerArray.h"
   
2. 
   
3. #ifdef ME_PORT_DEFINED
   $ q% y" t3 ?! e2 E* [! d2 G: {3 \
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   0 y" {; F/ U% L3 v
5. {
   3 N" S7 Y5 C8 S7 c8 n2 [. l
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   # j' [7 Q! Q9 F$ Q7 G6 o
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    3 m% s* X% f" R0 X& ^7 j3 D
13. }
    ; m5 U3 z6 U6 \5 f
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    
16. {
    
17.     _DataPin = pin;
    - b5 Q/ B2 K4 \; R# A1 c
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    - J6 Y+ I) H$ ?! x6 w7 K0 R4 |
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    , e1 \- C3 }: e9 L
23. 
    ' N5 f* S  v# c7 l) n: b
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    : `. v1 g$ q% e! d
27.     pinMode(_DataPin, OUTPUT);
    3 m/ [/ ~" q7 X& l+ \1 `
28.     digitalWrite(_DataPin, HIGH);
    6 C( q0 D4 ^3 ]# M% f5 u
29. 
    ! E% h! P6 i, x1 U0 I
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    2 [7 J! Q5 q9 R2 V
32.     #endif
    3 s0 D5 ?! P$ ]4 I4 S/ ^0 Y
33. }
    
34. 
    2 Q( ^1 B9 e# w/ z6 u
35. uint8_t MeLineFollowerArray::getValue()
    
36. {
    , Y$ F' ~6 Q% Z
37.     uint32_t LOW_level_read_time;
    
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    
41.     static uint8_t old_data = 0xff;
    $ L2 g4 u# m" b/ S
42. 
    / n# d3 z6 a9 r1 X: Q- F
43.     pinMode(_DataPin, OUTPUT);
    
44.     digitalWrite(_DataPin, LOW);
    / c, a! O: l  B! x6 ^
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    
47. 
    - r2 g8 K" ^7 V4 R$ A
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    
50. 
    * B- d& W9 d: E; |3 I9 `/ k
51.     time_out_flag = millis();
    ; E; l! [- e  E* Q# Z$ ?
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    8 R( H& Y) Z/ U6 M3 b5 k1 N
54.     LOW_level_read_time = micros();
    
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    1 E8 G5 u) @' Y
56.     {
    
57.         return 0xff;
    
58.     }
    # Q1 S( G# @2 g- b5 @* j
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    4 D6 @  f& y/ b, j
62.     HIGH_level_read_time = micros();
    
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    $ D1 [* q$ U5 J8 q6 l) \; ~
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    : N3 o$ V: W) m7 w5 ]
66.     {
    
67.         return 0xff;
    
68.     }
    
69. 
    
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    . t5 ^+ K, B6 i/ m3 P( [7 `. H& U
71.     {
    5 ?# n9 i8 Z7 p
72.         return 0xff;
    
73.     }
    
74. 
    ) v- k8 u/ B5 S% B" u
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    " m; f% e, m/ o8 ], f
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    9 B8 \& y" I  |; j9 }) c! T' r. V
78. 
    9 j: a3 q6 w7 V9 P
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    $ L! u( [8 P3 g  r
80.     {
    / m, W" @/ D+ c" M
81.         return 0xff;
    
82.     }
    6 x! e+ C; K0 z3 n2 \
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    . x6 a% j' _8 |% {. f
85.     {
    ) u$ U6 _8 E0 s2 I' c% F; P
86.         return 0xff;
    9 P) D; O* `7 U9 e, _% e8 a6 @
87.     }
      W4 O9 `7 ?7 L/ M) A$ H  C( d
88. 
    
89.     for(uint8_t k=0; k<3; k++)
    , L$ g2 {# v3 u
90.     {
    1 q3 j& }: q' Z) V, u2 m( G# d
91.         Sensor_Data[k] = 0x00;
    
92. 
    , X% n5 V% S* a
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;
    2 `" u( n4 [* I( y# B
98. 
    + `7 g/ U' L+ |0 t
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    
100.             {
     
101.                 return 0xff;
     + b- n) f3 h4 K( a* b6 V! G! _/ d
102.             }
     # }( u" u" d: N0 J3 }
103. 
     * X- e  W) W( j& l' p  u
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     $ ~0 d7 l9 n5 Y, k8 x1 }& `
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     7 p, B8 L" t4 j2 l
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     ; @9 _" a5 x2 o+ c
110.                 Sensor_Data[k] |= (0x80 >> i);
     
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     ! Q7 y+ ?, U3 {# V! j/ U4 @, X
113.             {
     
114.                 return 0xff;
     
115.             }
     # f- p0 H  f( I" \9 Z
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     # |0 s0 m  v9 X, W
118.             {
     
119.                 return 0xff;
     
120.             }
     
121.         }
     5 d$ i& {0 y7 O; |0 F: D) n1 K
122.     }
     
123. 
     * A% B# m* t  p" L
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;
     
127. 
     ! {  ?8 ?3 r* k
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     + {9 s1 }2 h5 O# H: c9 |
129.     {
     , s1 Z% q" W) q6 S
130.         return 0xff;
     7 Q8 t- a: Y  D# _( M6 B
131.     }
     
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     + }; o" a; b: {& D
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     ( j0 i7 }+ |& t" Z/ N
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     ( n9 y& j( u4 C4 g0 z- ]
137.     {
     * n% [/ V) H9 e3 d% F: C& f- |
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     8 j: G2 \) q) [% s+ S  {( b
140.     }
     
141.     else
     1 q* \- t# O2 L) L% Z1 H* {
142.     {
     
143.         return old_data;
     
144.     }
     
145. }
     , r# I/ L' Y9 {7 d

複製代碼

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

2018-2-13 01:52 上傳

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

: E  ]+ [1 C$ o) g8 T7 @; ^

強力推廣中

哇...要100 個銅錢

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

2 J7 Q3 f# N0 o1 i8 R認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
能否用到mblock 5 上面呢？

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

无法添加到扩展呀

高手就是高手