mBot Ranger 高速循線範例＿

1. #include <Arduino.h>
   $ ?! L8 F8 J  q3 }
2. #include <MeAuriga.h>
   
3. #include "MeLineFollowerArray.h"
   
4. 
   
5. MeEncoderOnBoard Encoder_1(SLOT1);
   
6. MeEncoderOnBoard Encoder_2(SLOT2);
   
7. MeLightSensor lightsensor_1(12);
   8 C! k# b2 @- ~" A% G2 l
8. MeLightSensor lightsensor_2(11);
     P) }# A" X4 ~! {8 y3 ~3 m
9. MeBuzzer buzzer;
   / n; ?$ R; l! i* X: L" B+ w
10. MeLineFollowerArray linefollower(PORT_6);
    6 B3 M' [' I: ~. l+ [7 g4 w
11. 
    : B7 j2 L7 Y/ U8 [! ~
12. #define Error1 1
    
13. #define Error2 2
    8 v; o: F( w/ K* t" M
14. #define Error3 3
    # i  O$ M+ J2 x( b" O$ Y7 T) [
15. 
    0 B  E: u. P4 T$ ~8 H5 D2 c4 H. W
16. #define Kp 15
    + t9 ~# ]+ n' d
17. #define Ki 0.15
    3 y# ~- Q7 x# z8 V1 A+ }: s
18. #define Kd 0.03
    
19. 
    
20. uint8_t sData;
    # ~  t! v& S$ M" X9 N: Q5 J4 w9 t
21. uint8_t D1;
    
22. uint8_t D2;
    
23. uint8_t D3;
    # f( E: O# y* s% J% Z
24. uint8_t D4;
    
25. uint8_t D5;
    . G, S3 V9 F& M/ T
26. uint8_t D6;
    2 G, ^& q  G5 t  u
27. 
    
28. float previous_error = 0;
    * b3 ^) ]& B* k. B" H: t+ k
29. float integral = 0;
    
30. float derivative = 0;
    
31. int  Speed  = 160;
    
32. float output;
    + D& d7 F- d4 a) m
33. 
    - M# @) A% u$ c, H0 F5 t  A, X
34. byte Left;
    
35. 
    
36. void setup()
    2 g" y( v# r! v7 R' f
37. {
    5 g+ B: n; W- b2 m/ S: p
38. //Set PWM 8KHz
    
39.   TCCR1A = _BV(WGM10);
    $ g9 Y+ q6 m/ O$ p
40.   TCCR1B = _BV(CS11) | _BV(WGM12);
    
41.   TCCR2A = _BV(WGM21) | _BV(WGM20);
    1 C& P8 r  W% D$ g- w" U$ f
42.   TCCR2B = _BV(CS21);
    ) ?* O6 d" g7 v- s  c( V
43.   Serial.begin(9600);
    
44.   buzzer.setpin(45);
    0 u# z/ y5 p: ?1 {
45. }
    
46. 
    & S4 w( \4 i5 O
47. void loop()
    + j# C) f0 X6 f; \! O% w$ x
48. {
    % T4 L3 H  t0 m0 `+ J' E2 s
49.   while(!((lightsensor_1.read()) < (10)));
    $ V2 c( \0 Z* m( q
50.   buzzer.tone(1047, 500);
    " z; _* w# ~7 ~( C; L$ i+ A
51.   delay(500);
    & S/ n, y2 k8 ]( ~4 B: ^0 I0 Q( S
52.   do
    % O. O8 ]8 L( c7 s+ |4 Y& b6 h
53.   {
    ) H5 a" K) l% B! C0 Y0 |: G
54.    int Err = getErr();
    
55.    if(D1 == 1)
    + }. q8 E* U0 s( T
56.    {
    
57.      Left = 1;
    # y6 T: a. B$ i5 G9 y* ]3 S
58.    }
    
59.    if(D6 == 1)
    ) g6 ^! ?: B1 [; D/ Y; i" }
60.    {
    
61.      Left = 0;
    : j4 n* H7 H2 c) j9 J0 n8 a4 @
62.    }
    
63.    if(Err == 99)
    
64.    {
    0 M% n. r  z& \' ?; Q8 n
65.      if(Left == 1)
    " v5 C% a! B# ]! O9 M0 Y8 Y  J. w' `
66.      {
    
67.        Speed -= 5;
    
68.        moto(0,Speed);
    ( p. S. @) T4 X2 `- {# Y
69.        do
    
70.        {
    : [! C, _7 d. E. t& `+ Q: p: x  c
71.          Err = getErr();
    ; f+ U) N# Q# f0 C$ l8 e
72.        }while((D1+D6) == 0);
    
73.      }
    
74.      else
    
75.      {
    
76.        Speed -= 5;
    
77.        moto(Speed,0);
    
78.        do
    
79.        {
    * q3 s& m2 p& t& Y3 B+ T
80.          Err = getErr();
    : f& S$ L+ L' T+ w1 Q) ?0 c
81.        }while((D1+D6) == 0);
    
82.      }
    
83.    }
    & B" u3 o; d0 V2 w% b) I5 a
84.    else
    / M/ B' q- `0 E2 P, c
85.    {
    8 e3 X" f& i# b; ~
86.      if((Speed < 160) & (Err < 2)) Speed+=1;
    " J: u+ x6 Z7 b4 h. |* [+ y0 R
87.      if((Speed > 100) & (Err > 2)) Speed-=2;
    
88.      integral = integral + Err;
    1 i- {; n# `" b) V% K5 @1 l$ k
89.      derivative = Err - previous_error;
    7 q, d0 Y3 E" a3 s  Z: c
90.      output = Kp*Err + Ki*integral + Kd*derivative;
    $ y) {4 R! |1 u) E. l
91.      moto(int(Speed-output),int(Speed+output));
    % H( F! o  x' }# }4 k
92.      previous_error = Err;
    
93.    }
    & }* \& `4 O2 s- k0 ^# A
94.   }while(!((lightsensor_2.read()) < (10)));
    , l  ~- K) Y, p/ v; v( j0 h# L
95.   moto(0,0);
    ; F5 L& F3 Z9 B' \; j
96.   delay(500);
    7 c7 t, Q+ p' B
97.   buzzer.tone(262, 500);
    
98. }
    
99. 
    
100. int getErr()
     : I7 L% [' p: M5 _
101. {  
     
102.    sData = linefollower.getValue();
     
103.    D1 = ~(sData>>0)&1;
     9 Y- Q0 ]* T; V1 K" x% o3 y: z2 K# I
104.    D2 = ~(sData>>1)&1;
     9 B' L' j" n: W7 I3 v0 I, ~
105.    D3 = ~(sData>>2)&1;
     ! I7 E4 d* o3 s$ Z
106.    D4 = ~(sData>>3)&1;
     8 f" S. @1 R: x4 m! g2 c
107.    D5 = ~(sData>>4)&1;
     
108.    D6 = ~(sData>>5)&1;
     
109.    int downD = D1+D2+D3+D4+D5+D6;
     9 a. b, Q/ ~* H* X8 d
110.    int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
     
111.    if(downD == 0)
     # ^! u7 q' c$ q
112.    {
     
113.      return 99;
     6 ?- P% z$ [) Z, [2 C
114.    }
     . T- v, h% D( J/ M2 v0 |" V# q
115.    else
     2 G( L# O- ^/ ]1 h7 N% S
116.    {
     
117.     return upD/downD;
     
118.    }
     : m$ p' T' ^' m2 k% T
119. }
     3 E# E; ~" U8 N1 D3 N: t
120. 
     & L! ^; [% V+ `, X
121. void moto(int Speed_L,int Speed_R)
     / Y# y; ?4 C. O4 F8 S5 y+ ~. C; [
122. {
     4 [+ q! @" m, e4 U, g
123.   Encoder_2.setMotorPwm(Speed_L);
     
124.   Encoder_1.setMotorPwm(-Speed_R);
       l" a8 \: c5 h7 a3 T" x& R
125. }

複製代碼

MeLineFollowerArray.cpp

1. #include "MeLineFollowerArray.h"
   2 n8 A% S  p1 x+ a1 W1 c. j
2. 
   
3. #ifdef ME_PORT_DEFINED
   
4. MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
   + z8 T% a4 Y- H  k0 U. N4 x& F
5. {
   
6. 
     W/ }; A' ?( @( W: h0 x. T4 B
7. }
   
8. MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
   8 C1 K! V/ N# F$ D4 @2 u
9. {
   , m6 x7 E/ N5 x
10.     _DataPin = mePort[port].s2;
    
11.     pinMode(_DataPin, OUTPUT);
    3 ]9 ~) n; ]! M
12.     digitalWrite(_DataPin, HIGH);
    
13. }
    
14. #else // ME_PORT_DEFINED
    
15. MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
    * b" I7 Z0 h* \: Z+ i
16. {
    
17.     _DataPin = pin;
    4 o/ t0 c% i1 b2 T2 a
18.     pinMode(_DataPin, OUTPUT);
    8 X+ w* e5 o* o) F7 G4 j2 H5 D
19.     digitalWrite(_DataPin, HIGH);
    : l% A: @- @' i6 h4 r
20. }
    
21. #endif // ME_PORT_DEFINED
    3 `" m6 a' \6 F1 u* P& d
22. 
    2 S9 k' i8 q5 j
23. 
    
24. void MeLineFollowerArray::setpin(uint8_t pin)
    ( F4 N' D  B- y1 D+ Q+ O: l
25. {
    
26.     _DataPin = pin;
    ; p& V" I7 h0 M( c1 F
27.     pinMode(_DataPin, OUTPUT);
    
28.     digitalWrite(_DataPin, HIGH);
    
29. 
    
30.     #ifdef ME_PORT_DEFINED
    5 n$ v$ y% m, A6 Q
31.     s2 = pin;
    
32.     #endif
    
33. }
    , u* a5 Q8 e  t4 q8 x
34. 
    
35. uint8_t MeLineFollowerArray::getValue()
    0 @2 z$ X% z2 b/ Q* ^3 S& X
36. {
    
37.     uint32_t LOW_level_read_time;
    4 A3 O. x: @6 n) E
38.     uint32_t HIGH_level_read_time;
    2 U- B' a" j  ^  Q9 ~6 v6 _
39.     uint32_t time_out_flag;
    
40.     uint8_t Sensor_Data[3];
    4 _4 u$ w8 K% ?7 I& m2 `
41.     static uint8_t old_data = 0xff;
      m6 @1 J( P. @+ o5 A, I
42. 
    0 t" b4 c8 ]2 _" Q' f
43.     pinMode(_DataPin, OUTPUT);
    & i6 m- k$ q  Q1 j, X6 Q
44.     digitalWrite(_DataPin, LOW);
    
45.     delayMicroseconds(980);
    ( y" r  m& o3 Z% h
46.     digitalWrite(_DataPin, HIGH);
    # z+ ?1 d* [7 {9 K) e5 ^
47. 
    
48.     pinMode(_DataPin, INPUT_PULLUP);
    ' l1 A! P9 Z( Y5 V, w  \
49.     delayMicroseconds(10);
    7 J) m! l& u$ J
50. 
    
51.     time_out_flag = millis();
    / l9 h. Z8 e' g# W. s0 N8 o, P
52.     while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    
53. 
    
54.     LOW_level_read_time = micros();
    ( L3 G1 s$ s# H3 f) U: M
55.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    ; l; {* M; b# ~" E, T$ P
56.     {
    7 B& A7 x" F$ B% m# Q- i, z
57.         return 0xff;
    
58.     }
    ( K! ]+ m; b0 Y  Q; g1 K) @- q9 {  d
59. 
    # F: U# k" `; t8 ?0 u$ s- _7 }# r
60.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
    + Q! o. N' {0 N! s8 d2 D2 |+ c
61. 
    
62.     HIGH_level_read_time = micros();
    9 D. ~& L& H9 c
63.     LOW_level_read_time  = micros() - LOW_level_read_time;    //read 100us LOW level
    : U- W/ V9 v0 `
64. 
    
65.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    
66.     {
    
67.         return 0xff;
    % `# G0 J" z- a7 i8 q
68.     }
    . e; o( W, n* j% e. L  j1 N
69. 
    & O1 S9 j8 y2 L% e- I: |0 g
70.     if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
    
71.     {
    * }1 b( f! e% {$ _) K
72.         return 0xff;
    
73.     }
    % v; H: B  Z, J" X6 D6 u0 _
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
    
78. 
    
79.     if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)   //time out
    1 M. w$ c. H# T! _3 O
80.     {
    + ]0 f( r* L9 Z; c
81.         return 0xff;
    
82.     }
    
83. 
    
84.     if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
    
85.     {
    6 k( M8 q2 v: }  r. z
86.         return 0xff;
    " l& a$ c0 X' n! K3 Z. B& v
87.     }
    # `9 E+ Y7 q  h
88. 
    3 ~" j( Q4 ?3 K; O2 A
89.     for(uint8_t k=0; k<3; k++)
    2 M; s) }5 }) J* T
90.     {
    
91.         Sensor_Data[k] = 0x00;
    3 v6 c! c1 f8 t/ m" P4 O
92. 
    0 q1 l2 `4 C  `4 h3 l4 A
93.         for(uint8_t i=0;i<8;i++)
    
94.         {
    ' u  t+ B* i9 L1 n. j5 l/ q
95.             while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
    9 P9 @9 C. a1 n6 s$ b( Z& ^4 i
96.             HIGH_level_read_time = micros();
    + O" Q2 [6 v' n6 I
97.             LOW_level_read_time  = micros() - LOW_level_read_time;
    ' P! Z+ i3 y) q
98. 
    
99.             if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
    ! h) G3 t7 o6 i9 v! ?0 A
100.             {
     
101.                 return 0xff;
     8 k8 j- d5 ?* p* Y+ m/ f  @+ y, Q
102.             }
     & K/ R/ P( o+ L/ {
103. 
     
104.             while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
     / L. F! t# R2 r8 d
105.             LOW_level_read_time  = micros();
     , u$ F6 B/ `% j
106.             HIGH_level_read_time = micros() - HIGH_level_read_time;    //read HIGH level
     
107. 
     0 i, @5 p% [+ Z3 b  a1 J
108.             if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100)   //bit 1
     
109.             {
     % X8 t( U& A' C1 r+ i( P* w' B9 f
110.                 Sensor_Data[k] |= (0x80 >> i);
     ( N! j% y$ `5 W2 d' Q1 R/ j& z
111.             }
     . m: E- S# j: ~0 P
112.             else if(HIGH_level_read_time >= 100)
     5 o' o9 _4 e% Q  [$ u* R1 U
113.             {
     
114.                 return 0xff;
     ) b7 i. F( U8 D9 J1 E
115.             }
     
116. 
     
117.             if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
     ' s3 t, w: Y- a1 {$ j8 ^4 V
118.             {
     2 E' a  c' y4 {! ], X# l  z
119.                 return 0xff;
     2 c/ W( L( a; a" ^8 t' f
120.             }
     : Q* ~; s* d. ]8 m$ Y$ _% r8 @/ s
121.         }
     
122.     }
     : [7 ^- w# B- R0 Z" ]' ]. H6 i
123. 
     6 |- y2 x) Q! G8 Y/ L8 c, \! ]
124.     while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );   //read 50us LOW level
     
125.     HIGH_level_read_time = micros();
     2 M4 ]: V1 E0 U* C, V9 i& t
126.     LOW_level_read_time  = micros() - LOW_level_read_time;
     3 K; a1 O+ o: d8 ^
127. 
     8 K* W: G3 y. E; ~+ d; k; S
128.     if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
     / G7 D6 A! O6 p$ L8 X" O( F1 z
129.     {
     
130.         return 0xff;
     ) ^/ l2 z+ r; C0 U: r9 {3 `3 Y7 ~2 a
131.     }
     . m1 d, t& ?* t6 I
132. 
     
133.     pinMode(_DataPin, OUTPUT);
     8 a3 J6 `1 f5 E
134.     digitalWrite(_DataPin, HIGH);
     * R4 Z/ h4 m6 d0 x2 A! {$ `. _8 [7 \; G
135. 
     
136.     if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
     6 J. K9 H0 V* v6 g/ D: k
137.     {
     
138.         old_data = Sensor_Data[0];
     0 W) Y$ Q- p& l& P7 J8 S
139.         return Sensor_Data[0];
     ; p" ]* J5 _! h. w/ M/ ?
140.     }
     0 U7 `( w+ K. K4 s4 y8 h. r
141.     else
     $ G1 c; N3 Q& X1 O2 g5 I5 K. g
142.     {
     8 u4 p9 @. G' |+ W' ]. U
143.         return old_data;
     . X3 \9 f6 e* m1 i5 V1 O/ M
144.     }
     
145. }
     ; n) `+ c2 M9 O9 i$ V6 }7 L# L- A

複製代碼

. R* X, Y- B1 ~1 Y7 e% pMeLineFollowerArray.h   MeLineFollowerArray.h (731 Bytes, 下載次數: 10, 售價: 100 個銅錢)

2018-2-13 01:52 上傳

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

強力推廣中

哇...要100 個銅錢

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

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

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

感謝分享Thanks

能否用到mblock 5 上面呢？

roboger 發表於 2020-11-13 15:43
+ c& b, K4 |0 t" [6 @2 K能否用到mblock 5 上面呢？

. o" {# O0 Z7 |& d/ o/ \MeLineFollowerArray 在 mBlock5 中, 只有支援 megaPi 控制板, Ranger 無法使用, 如有需求, 我再寫一個擴展上架吧!

无法添加到扩展呀

高手就是高手