mBot Ranger 高速循線範例＿

1 O1 z5 ?4 R1 ?4 L% g6 G

1. #include <Arduino.h>
   & P  [7 @2 {: c& i/ j: D
2. #include <MeAuriga.h>
   & W' h! E3 W6 e! s
3. #include "MeLineFollowerArray.h"
   ; f) U& h) h7 n: ]  ?/ M
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   " k3 u% U; m" m
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   / b& ~! ?& G3 m) d
10. MeLineFollowerArray linefollower(PORT_6);
    2 @. V. V+ D, S+ A! i
11. 
    ' l9 S, y  o  J( u# j3 Y
12. #define Error1 1
    
13. #define Error2 2
    0 j& P3 f& Z* E& {/ Y
14. #define Error3 3
    
15. 
    ' x: H; L# b6 Y
16. #define Kp 15
    1 v3 H# L& J' P; A5 h
17. #define Ki 0.15
    
18. #define Kd 0.03
    
19. 
    ! D  K( S. ~1 Q' z4 r+ M$ f9 _# X
20. uint8_t sData;
    * k8 f( p% p& d! r
21. uint8_t D1;
    . H, S* w$ t7 w, L& M" ^1 R
22. uint8_t D2;
    : X$ Q) V0 A4 H9 F0 N( J4 b
23. uint8_t D3;
    # Q7 v8 Y$ \* y+ q
24. uint8_t D4;
    + U' U3 y; R+ u8 |, `
25. uint8_t D5;
    9 {' V8 f7 w3 [% e  L( H
26. uint8_t D6;
    
27. 
    
28. float previous_error = 0;
    
29. float integral = 0;
    
30. float derivative = 0;
    9 O- P% c% D! _0 |, u/ h
31. int  Speed  = 160;
    
32. float output;
    . U  Z0 h$ v, t( M5 H: m# {( k
33. 
    
34. byte Left;
    
35. 
    $ C( W7 F9 b3 n
36. void setup()
    ; O: f: {. j! E. V' C9 c
37. {
    
38. //Set PWM 8KHz
    / w, R) ], B9 {. O0 k/ d- ]
39.   TCCR1A = _BV(WGM10);
    # m% L8 u0 ~+ f) ]- J, H! p
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    , I' |! y( |$ v8 k6 [
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    , U5 ~$ x. q3 F- w' f
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    
45. }
    . ?: |$ Z8 v1 ~; }+ }5 N5 d
46. 
    6 i% \% q" X, X+ x  S6 j
47. void loop()
    
48. {
    $ Y' e& e' V( i' U2 I7 R% f* q; P* p# ?8 O% p
49.   while(!((lightsensor_1.read()) < (10)));
    
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    2 r! |5 ]$ i$ ]! q% V
52.   do
    
53.   {
    
54.    int Err = getErr();
    , e  F! k. b8 l' g6 m; G
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    
58.    }
    + ^. ]* ?5 J# K! X" c
59.    if(D6 == 1)
    
60.    {
    
61.      Left = 0;
    1 I6 g* l5 S, i, P+ I9 U, F
62.    }
    ( A2 `$ H$ p- O& x7 }$ I3 F
63.    if(Err == 99)
    $ i3 Y7 W+ P0 k3 u& p
64.    {
    
65.      if(Left == 1)
    
66.      {
    
67.        Speed -= 5;
    6 Q: d. e3 f, G* Q# O8 l  G
68.        moto(0,Speed);
    / n7 E0 W) z- R, F% d7 m
69.        do
    ) l' C! O2 P  `7 ^3 x
70.        {
    - j7 h" a; d7 S, ^
71.          Err = getErr();
    
72.        }while((D1+D6) == 0);
    * K1 n, w  u, v
73.      }
    
74.      else
    4 ]7 I7 E6 z& N) W! x; Y# I7 e3 u
75.      {
    
76.        Speed -= 5;
    ( L$ f9 Z3 U6 g- P* r
77.        moto(Speed,0);
    
78.        do
    
79.        {
    
80.          Err = getErr();
    
81.        }while((D1+D6) == 0);
    7 l+ R6 J8 A. L
82.      }
    3 M2 n. z7 l% i* u
83.    }
    
84.    else
    
85.    {
    ; A) [$ ]5 R0 S  {. R
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    
89.      derivative = Err - previous_error;
    6 S* Y) @) A1 f& k1 u- U
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    
93.    }
    3 P; t0 m- @, }9 }- A
94.   }while(!((lightsensor_2.read()) < (10)));
    , y& k$ b4 l4 L5 n/ U, I9 j! S# j
95.   moto(0,0);
    3 ?- F1 a2 U2 s1 I0 e: ~2 r
96.   delay(500);
    # S1 \* ?7 Q7 Z1 ^- G
97.   buzzer.tone(262, 500);
    . ]( Y* c& R+ P* y# y7 Z
98. }
    4 S/ B0 v( B# b! F6 w' ~
99. 
    
100. int getErr()
     
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     ; K6 X( d7 d$ Y
104.    D2 = ~(sData>>1)&1;
     5 c2 m4 E: {- o
105.    D3 = ~(sData>>2)&1;
     
106.    D4 = ~(sData>>3)&1;
     
107.    D5 = ~(sData>>4)&1;
     , d( H7 }! E6 y& h) }4 [
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);
     ) B  ~) d. p! V3 I% ^7 i
111.    if(downD == 0)
     
112.    {
     , T8 P3 {8 |# J' I
113.      return 99;
     $ x% m. ^- a4 b) K) b" h: d0 `
114.    }
     
115.    else
     
116.    {
     
117.     return upD/downD;
     
118.    }
     
119. }
     6 P1 N) H* L/ Q3 U
120. 
     
121. void moto(int Speed_L,int Speed_R)
     
122. {
     # c$ }) H7 r7 i$ D% d4 i) H
123.   Encoder_2.setMotorPwm(Speed_L);
     ( y% Q9 f* K& E! d
124.   Encoder_1.setMotorPwm(-Speed_R);
     
125. }

複製代碼

MeLineFollowerArray.cpp
3 v* R5 Y4 Y3 U$ |- P1 ?' r3 I$ o

1. #include "MeLineFollowerArray.h"
   
2. 
   5 @) N9 J; v9 E% V  a
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   2 m' v+ O6 S) S( L8 q- l
5. {
   4 Z/ l- V% K& e! r9 _
6. 
   
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   
9. {
   4 L3 [" W+ g) _; O. p+ T
10.     _DataPin = mePort[port].s2;
    6 {$ B% C! w9 h$ g
11.     pinMode(_DataPin, OUTPUT);
    
12.     digitalWrite(_DataPin, HIGH);
    / F$ M' T; ^- }; z; f+ T
13. }
    ! N3 h$ Q  {7 [3 a
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    # N/ Z* U, J3 M+ h( [
16. {
    
17.     _DataPin = pin;
    5 ?" e8 M, e* ^
18.     pinMode(_DataPin, OUTPUT);
    
19.     digitalWrite(_DataPin, HIGH);
    . c! i& v+ d1 O& O; P) o3 Z
20. }
    
21. #endif // ME_PORT_DEFINED
    
22. 
    - g1 p2 E. w- S; C! q1 O
23. 
    . U. V# A6 M1 c' U' z2 E1 E% y
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    . W: l/ u; V3 M: B5 M/ b6 o
29. 
    
30.     #ifdef ME_PORT_DEFINED
    9 R3 D; l+ `7 Z6 n5 N
31.     s2 = pin;
    8 _- l9 X* {# E# x
32.     #endif
    - D/ D4 X6 n9 j( m' \" s
33. }
    4 t4 V2 j7 e& f7 a' C; b, w. W2 T
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    " |8 w# Z. Q. G! x& j0 `8 |
36. {
    / d( |& `+ W# W0 ?
37.     uint32_t LOW_level_read_time;
    % Z* y, p8 W2 A6 o% N
38.     uint32_t HIGH_level_read_time;
    
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    5 y4 p3 B1 C; j# k' _* D$ N
41.     static uint8_t old_data = 0xff;
    ' s0 W8 }" x. J+ y6 A" L
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    ! e. {/ q* l8 s6 T% W' e2 G
44.     digitalWrite(_DataPin, LOW);
    " H1 T) F5 I* O7 t) }
45.     delayMicroseconds(980);
    + A! g, w  r1 l* g4 v9 d3 X
46.     digitalWrite(_DataPin, HIGH);
    % W' P( E1 W# S8 F: ~6 V( h: j
47. 
    / W; {1 ]8 E3 R1 h( d
48.     pinMode(_DataPin, INPUT_PULLUP);
    
49.     delayMicroseconds(10);
    . z5 p! Z5 J" n, q
50. 
    
51.     time_out_flag = millis();
    % n) g. e5 D# Z+ h6 x- {
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    ( B8 Z" ~; f$ j* n
53. 
    
54.     LOW_level_read_time = micros();
    9 V2 ]3 S$ U7 T" |; \2 A
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    0 f/ ^% S, T. R( R9 h0 H8 Y
56.     {
    
57.         return 0xff;
    5 t/ ]( A1 O7 c( e6 U# i$ H- r
58.     }
    5 {# }2 W! v* T% r4 L* A
59. 
    1 P: Z" I. n% U( C" x
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    1 t8 \3 I& p1 W9 J
61. 
    # s; [4 a- v$ d* P
62.     HIGH_level_read_time = micros();
    4 V: F: g4 @2 `! M, y
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    
64. 
    " @6 E& T* k0 c) I, U
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
      N- p7 J* E0 W
67.         return 0xff;
    
68.     }
    2 j" t+ k! f0 z% a
69. 
    + f! K( `  w% f5 q% Q# Z
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    - S4 n! \* j1 s  K: T8 n
71.     {
    0 Y4 K( B5 i) |2 z, r
72.         return 0xff;
    
73.     }
    
74. 
    
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
76.     LOW_level_read_time  = micros();
    . H2 g$ q. [+ N% B% H- ^3 Y
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    ) Z% e8 C6 Y$ {5 x; H
78. 
    ! j6 l3 F% m4 q5 ^% A4 I
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    3 B( ~9 `% x* r3 ~2 n& ^5 F
80.     {
    
81.         return 0xff;
    
82.     }
    
83. 
    , M  A! b3 z3 E4 D. c
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    % Z& x9 @4 w8 _/ F0 o
85.     {
    
86.         return 0xff;
    4 [4 y# m# \, u* `$ `2 b$ ~; b4 U
87.     }
    - k4 f+ f; d# H4 ]' s$ ~' @
88. 
    . P! r6 M0 n( `5 y  [
89.     for(uint8_t k=0; k<3; k++)
    
90.     {
    
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    0 Q$ r6 z. p; r: R- i9 p6 O
94.         {
    
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    
96.             HIGH_level_read_time = micros();
    ( H8 H: h( [6 Y
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    & ]5 L0 w; _# T0 D: S
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    0 j6 c* [6 s: _: z/ `( p
100.             {
     4 X0 u) B: y; z% k  n
101.                 return 0xff;
     
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     2 F/ y; ~5 `) n# n: r+ D" c
105.             LOW_level_read_time  = micros();
     
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     0 r' X( F$ s3 B
107. 
     
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     5 Y) h8 P- ?& E5 `- M" S
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     3 I8 P+ W9 d% B1 k% s: A% ?6 k( r
111.             }
     * O2 j2 p7 D* i8 z. K6 Q2 }$ b
112.             else if(HIGH_level_read_time >= 100)
     
113.             {
     
114.                 return 0xff;
     ; n: w9 p- H( b/ R- h  a
115.             }
     
116. 
     3 b( ?! f) Z, p: @  ~7 [* R" G: y0 Y
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     
118.             {
     6 A2 |1 M) D0 w& z
119.                 return 0xff;
     ( Q  L' w! k$ x! q. F
120.             }
     
121.         }
     9 |0 r* a5 ^% f6 J
122.     }
     
123. 
     
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     / \2 o+ W$ c8 |/ f1 K6 X7 Z) \
125.     HIGH_level_read_time = micros();
     ) v3 M: n& H# a& v0 a# g
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     ' v' P$ z: I6 I! h8 B4 j
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     
129.     {
     9 R( F( }8 o; v6 y9 N4 d4 V
130.         return 0xff;
     - ~. a8 o2 R# s/ |1 x( Z
131.     }
     5 h5 N% o: L- P6 c$ v
132. 
     / P/ p  D# D$ L! e+ w' q/ A
133.     pinMode(_DataPin, OUTPUT);
     ( D7 f$ m. m& G9 Y. i
134.     digitalWrite(_DataPin, HIGH);
     
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     
137.     {
     1 q6 Y& ], F  O
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     
143.         return old_data;
     9 g# W9 }$ F( |" |
144.     }
     2 T( L0 @$ i8 w  `9 D
145. }
     ( e; |, N) i6 [2 R# I: Z

複製代碼

, M8 v6 W; O$ \: [) J0 j3 L6 QMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

3 H! ]- F; c1 N$ h2 I2 t
  q  C9 R+ `3 a

強力推廣中

哇...要100 個銅錢

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

  ?) m. b- |% W' p認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
5 W7 ^, j: ~% j能否用到mblock 5 上面呢？

0 S1 F2 R0 h6 m1 I# J9 {6 s8 N' U7 lMeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手