mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   : @4 Y5 K$ s( l4 u( w& ^
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   - ~6 ^6 P& d/ t: w" ?, G
4. 
   ) M+ n. H4 P/ t* ~5 E( ^# b7 I; T" I
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   9 b/ D7 b* R% k' |
7. MeLightSensor lightsensor_1(12);
   
8. MeLightSensor lightsensor_2(11);
   
9. MeBuzzer buzzer;
   
10. MeLineFollowerArray linefollower(PORT_6);
    
11. 
    # l7 U7 U4 H. D
12. #define Error1 1
    
13. #define Error2 2
    ! c, v' `9 O2 Q6 p
14. #define Error3 3
    ) q! s3 M2 q: G. B6 ~1 U
15. 
    # m3 c4 O6 t! l! u' Y
16. #define Kp 15
    
17. #define Ki 0.15
    * N" Q5 Q7 b6 }6 e0 V. D1 \1 s
18. #define Kd 0.03
    / g5 ~* \( w4 {" c% ]* Z
19. 
    $ ?& r: |! J- X, p( i, P8 v
20. uint8_t sData;
    
21. uint8_t D1;
    0 }$ ?, s2 c2 w9 b1 Q
22. uint8_t D2;
    
23. uint8_t D3;
    ! B  v2 h" g' i- K! H+ A% C. {
24. uint8_t D4;
    
25. uint8_t D5;
    
26. uint8_t D6;
    
27. 
    * R* R- ?2 Y+ A" g7 V0 a
28. float previous_error = 0;
    
29. float integral = 0;
    5 J; j! e) z8 ~! T( C% J
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    ! n1 L6 A/ S0 N  |5 R7 W+ S
33. 
    6 p  ]0 t6 \  l/ |: i& b
34. byte Left;
    
35. 
    
36. void setup()
    
37. {
    * `$ r2 I  E; g/ I. L- P
38. //Set PWM 8KHz
    ! v. K3 V. v' s% u5 v5 ~
39.   TCCR1A = _BV(WGM10);
    6 y7 L3 Z5 m3 T7 d
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    . a9 W. \. g( b4 b
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    + ~& C' U; I. ^3 g
42.   TCCR2B = _BV(CS21);
    
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    9 R( o# B8 F! Q0 j. y% o3 [7 Y, U
45. }
    
46. 
    
47. void loop()
    
48. {
    
49.   while(!((lightsensor_1.read()) < (10)));
    / _+ m6 a9 ]* e4 L& R
50.   buzzer.tone(1047, 500);
    
51.   delay(500);
    7 H  D1 C2 h4 e1 u. l2 v8 h
52.   do
    2 _4 i5 Y1 Q; P0 z
53.   {
    , X1 f+ ]% U, V) U
54.    int Err = getErr();
    
55.    if(D1 == 1)
    
56.    {
    
57.      Left = 1;
    ( b5 d  A: C7 `
58.    }
    * L) P/ V" d0 h/ P' H1 c, b
59.    if(D6 == 1)
    , }9 Z2 g" B5 u4 P) ^/ Z
60.    {
    ' m" p5 u4 H- p2 u$ P6 a7 |! \
61.      Left = 0;
    4 A  S% l7 X! w
62.    }
    , D$ S6 R' @; n# Z5 u0 G) G
63.    if(Err == 99)
    
64.    {
    
65.      if(Left == 1)
    ' Z8 z+ A& z0 u; [$ Z- `9 K
66.      {
    ' x8 |4 {& U1 E- V
67.        Speed -= 5;
    . s1 M$ W8 E  S2 S, T9 ]; M4 Z
68.        moto(0,Speed);
    
69.        do
    
70.        {
    
71.          Err = getErr();
    3 W, n: s, l( r
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    9 X: p3 R: R8 y. H+ x
75.      {
      |3 c& W, V" a9 e
76.        Speed -= 5;
    9 \  M7 P8 k% _. o
77.        moto(Speed,0);
    , D2 f5 z5 q1 s  v3 z
78.        do
    
79.        {
    
80.          Err = getErr();
    % R* T, L: s4 g0 }
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    
84.    else
    
85.    {
    0 d; e) a* r, }( a
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    " E9 X. L3 V4 s" E) d
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    ( B- B2 [0 n& L, o% d0 R
88.      integral = integral + Err;
    # V  W7 O0 ~+ P
89.      derivative = Err - previous_error;
    ( x- u" Y# }( B. M6 M5 ^) G
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    + {. `; Z2 v0 d, r2 r! g
91.      moto(int(Speed-output),int(Speed+output));
    
92.      previous_error = Err;
    ( u# k! D# }2 ~0 V7 e. G2 n* f' W
93.    }
    
94.   }while(!((lightsensor_2.read()) < (10)));
    
95.   moto(0,0);
    $ C0 _: R7 B7 i
96.   delay(500);
    : S, r7 ~* n! C
97.   buzzer.tone(262, 500);
    
98. }
    * }9 D/ S. l7 g) N* k4 t+ Y
99. 
    
100. int getErr()
     9 N+ s! {. [, C, p. b
101. {  
     , R4 D; m$ J7 s' f. K5 M2 [6 m" {
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     
104.    D2 = ~(sData>>1)&1;
     
105.    D3 = ~(sData>>2)&1;
       L, b' ?5 ], C- `' ~
106.    D4 = ~(sData>>3)&1;
     & S9 X# \* z- }: `+ a
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     " z1 w$ ^' f" l7 H6 d/ y* E" {
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)
     : V: o; {: f" `0 b& d
112.    {
     
113.      return 99;
     / [* c, y4 ]2 W& w6 o
114.    }
     & e; R# Y/ _* a# J; Q
115.    else
     
116.    {
     * _8 w' f% }  O9 \
117.     return upD/downD;
     % Z# f- M! J7 N: s$ ?) S! z
118.    }
     
119. }
     
120. 
       x( i$ L' O/ N  c* d; Q2 d
121. void moto(int Speed_L,int Speed_R)
     
122. {
     : h2 e" B. c# O
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
     8 i" s/ B  O9 x
125. }

複製代碼

MeLineFollowerArray.cpp
8 d7 C& B8 H9 l. z

1. #include "MeLineFollowerArray.h"
   + H3 ^% Y+ L/ [
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   6 s4 G* e0 j7 h+ \/ d- C- e/ v9 O4 @* E
5. {
   7 ?! `% Z8 \. v' ~$ q5 k
6. 
   - v5 r. ^% `. {) ]
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   6 c& h8 B  \4 @% q* ]
9. {
   
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    3 H$ J/ ]8 ?$ m) E. O' O
12.     digitalWrite(_DataPin, HIGH);
    7 ^4 c$ p7 K: r4 b
13. }
    8 k) ~& W- [! K# c. g8 U1 ]7 n7 U, l, n
14. #else // ME_PORT_DEFINED
    : a( M- r6 R, O. N. j' \  c
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    4 D& v8 S$ Z* x; a
16. {
    
17.     _DataPin = pin;
    
18.     pinMode(_DataPin, OUTPUT);
    2 y0 w+ X, v+ U1 N* ~7 V+ n
19.     digitalWrite(_DataPin, HIGH);
    
20. }
    4 [: A. m) e( X2 T8 b+ m  u3 W, j1 A
21. #endif // ME_PORT_DEFINED
    ; G$ d  {$ j# R/ n0 ~+ R
22. 
    
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    
25. {
    
26.     _DataPin = pin;
    
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    
31.     s2 = pin;
    
32.     #endif
    
33. }
    ) }/ b" H1 y; `# S: K" ~' Q' P
34. 
    5 }8 a, U4 i5 a
35. uint8_t MeLineFollowerArray::getValue()
    , W' t  t; F4 d" ?. w+ Q$ |
36. {
    * w* M+ S2 P5 u
37.     uint32_t LOW_level_read_time;
    + ?% s& w5 E" Z- a% \
38.     uint32_t HIGH_level_read_time;
    " S% j4 F4 P' X0 R6 }# h) ^
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    ) O' H7 F! y- [% `3 }
41.     static uint8_t old_data = 0xff;
    
42. 
    
43.     pinMode(_DataPin, OUTPUT);
    0 X7 S* A0 g% ^8 {# r0 H
44.     digitalWrite(_DataPin, LOW);
    : D( m" ?8 I, ?+ Y6 J) [
45.     delayMicroseconds(980);
    
46.     digitalWrite(_DataPin, HIGH);
    $ |% Q: A  B* h6 [( z0 a
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    2 G; x% s/ W5 Z% _% v
49.     delayMicroseconds(10);
    
50. 
    
51.     time_out_flag = millis();
    0 H% }% K9 L' D, o# n% p0 h
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    ) F2 G* O) Z7 E  W6 y! `0 v( H
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
56.     {
    
57.         return 0xff;
    8 ~$ M& F* T% [) o( @
58.     }
    
59. 
    
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
61. 
    
62.     HIGH_level_read_time = micros();
    * g6 n2 k% W( N; }* B' q- R
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    : o6 [, p: ?1 K
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    0 N6 a% Q$ f+ Y6 ?; U" [
66.     {
    ! {& R: `; G$ v0 E2 D$ e
67.         return 0xff;
    
68.     }
    
69. 
    , n! Z! X" k' W$ q
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    6 k  K2 w* T9 p& B" }# M
72.         return 0xff;
      R$ @( x4 e: A* q
73.     }
    # P- n8 y' t. A/ y4 g, T
74. 
    7 O5 u% q" @! i: c: ^) ?
75.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    $ B; v$ j/ h7 i4 p( F. k* ?# M: T
76.     LOW_level_read_time  = micros();
    ' x( i' F7 k+ d/ m+ {" T
77.     HIGH_level_read_time = micros() - HIGH_level_read_time;    //read 50us HIGH level
    3 I: _  U* A: Q4 W' l* `
78. 
    # J' o, g  b: M2 m% F" @
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    9 n/ ^, y8 M0 D6 X, u
80.     {
    $ q0 X! S) J4 U  p
81.         return 0xff;
    
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    : P) P8 a- v5 ^, m$ Z, A, a
86.         return 0xff;
    
87.     }
    - g0 }9 s! z  Y9 \* C
88. 
    , Q7 q+ X# `. I& I* ]" Q
89.     for(uint8_t k=0; k<3; k++)
    ; {" ]$ o8 c2 D$ j3 i; J- \- I
90.     {
    ( V8 T/ H  f" {
91.         Sensor_Data[k] = 0x00;
    
92. 
    
93.         for(uint8_t i=0;i<8;i++)
    " i% q' o/ i7 r; Q5 E; b. F1 w
94.         {
    ' G# Z5 n( ?2 [! x6 C
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    $ q) p) m/ }/ y; s4 V
96.             HIGH_level_read_time = micros();
    7 o" u  r; b. D) F8 k! Q5 _
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    
98. 
    7 V; g) P$ g# p2 C' C( ~- z2 {5 y4 B
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ' f& s( e0 C; ~! P, p: Z
100.             {
     4 M0 B" R) E5 I5 K0 x  S# H
101.                 return 0xff;
     
102.             }
     
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     
105.             LOW_level_read_time  = micros();
     ' M' F+ A& g3 ^( c
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     " @: t9 c- J" K# U7 D
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     
110.                 Sensor_Data[k] |= (0x80 >> i);
     # w2 \0 p- ~" i$ J
111.             }
     
112.             else if(HIGH_level_read_time >= 100)
     1 H; V& @) D! T' M" E$ e9 H
113.             {
     
114.                 return 0xff;
     
115.             }
     6 x# c* f' N5 ^7 N
116. 
     . {6 j4 K, T, X4 n4 l/ x
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     , E, G% o+ _$ O* t7 }" c
118.             {
     5 I+ ~2 k, T" D' X- ~9 C3 T0 V7 d6 l
119.                 return 0xff;
     
120.             }
     
121.         }
     5 Y# }* ?& l: w8 L( s
122.     }
     
123. 
       \  B7 B1 w& h0 Z$ K/ @) |
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     6 Q& G3 [, r$ U! t/ {) M
125.     HIGH_level_read_time = micros();
     # r. B( `, g- Z7 N2 g' }5 ?
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     
127. 
     
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     % y0 |% o' O. f, }) k
129.     {
     
130.         return 0xff;
     
131.     }
     " _3 a! e( J& n+ l0 e, ?9 J
132. 
     . l, C6 u& C. D. e( O( J- L+ ~5 G
133.     pinMode(_DataPin, OUTPUT);
     9 O5 q3 d" x, o; [. C& q* R. U
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.     {
     5 x2 A- B% \7 F2 S
138.         old_data = Sensor_Data[0];
     
139.         return Sensor_Data[0];
     
140.     }
     
141.     else
     
142.     {
     , [) y) a$ F! y& X. H( l
143.         return old_data;
     
144.     }
     1 z4 @9 L1 Q$ e2 z
145. }
     ; X4 w8 P% Q) M# m0 X$ W& R" N

複製代碼

. h5 S& e" d* Q$ J- N$ wMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

# i$ [7 Y# h) h: s; g
' x2 ?5 Z0 v: G  x# O  V& }4 G" g

強力推廣中

哇...要100 個銅錢

daven 發表於 2019-3-6 00:09
( a, C3 l) C- n% I: F; h* U$ T: g哇...要100 個銅錢

4 `" J3 m, W  e; l. O" O3 h: c認真的回饋本站一些內容, 很容易達成的!

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

感謝分享Thanks

能否用到mblock 5 上面呢？

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

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

无法添加到扩展呀

高手就是高手