|
|
! i8 b- _' J, i$ G: y- #include <Arduino.h>
2 L( R! W( D, I( M2 }: [: X* \ - #include <MeAuriga.h>
* y+ r/ a" E9 L8 F - #include "MeLineFollowerArray.h" ?; U+ N6 q; J; G3 B; j- {. c# m
# h' n: W8 \- o. A6 R e- MeEncoderOnBoard Encoder_1(SLOT1);1 o2 N9 Z3 q! G) N' w1 L; W$ w* U
- MeEncoderOnBoard Encoder_2(SLOT2);
0 Q q! i3 B& T+ S9 r1 |, c - MeLightSensor lightsensor_1(12);
2 R6 y, o8 i$ n! g% c - MeLightSensor lightsensor_2(11);' I' [* }( H0 [/ m
- MeBuzzer buzzer;9 O# C" S) Z# }* L4 O; M
- MeLineFollowerArray linefollower(PORT_6);! U D: c0 A( k" G n- H9 V$ Y" k
- # Z7 q9 M0 B0 w( L8 J
- #define Error1 1
6 c- c/ H" J* O$ f) @ - #define Error2 2
4 X2 n3 [, h6 W* c( E - #define Error3 30 V% [( S/ v, w
6 r7 Q' F2 g _- #define Kp 15 w; e1 y* u# @7 X2 k! l1 |
- #define Ki 0.15( {! b; g; y# D* c+ P% L5 w$ [1 B
- #define Kd 0.03
9 X) F9 @4 v9 R% \# Q% G, j U/ g
* Z( P/ Y, B, q2 Z6 d' s- uint8_t sData;" G! s6 ? i0 V) a
- uint8_t D1;
5 N. C, Z3 S X1 U+ E9 \ - uint8_t D2;
8 A( x) k, o; S+ [$ y) R7 ]5 q - uint8_t D3;
4 e$ b. x7 C$ U0 p _ - uint8_t D4;
' ~* u$ a& [2 V8 @4 |* L - uint8_t D5;8 ~9 n9 z- b( a- ~' D
- uint8_t D6;' n5 E+ K! ^; {1 Q. e
- ' O2 X; {" ~" F' T; v
- float previous_error = 0;' f8 J# i* K% H7 R9 N- I2 v
- float integral = 0;" O! V5 T! @6 k- N5 Q% o, K$ Q
- float derivative = 0;3 V& Y) q2 D+ L# N1 D8 f, A9 G8 j
- int Speed = 160;
5 Q8 n0 m$ N* |: v* W2 e2 Z - float output;
4 [6 J. y* X; l1 \( v& F - 9 c+ o" J/ X2 `( L5 a
- byte Left;
4 S" B- N: K! v; S: K - $ i5 `8 F$ n2 p) d8 A1 F% H
- void setup()
5 K! y ^( e" ?, q - {
, F9 C1 C9 D, O2 V% i8 Y0 H - //Set PWM 8KHz
Z. w3 A3 `! I9 `2 ]0 I5 i - TCCR1A = _BV(WGM10);: T; r; n0 T0 [3 J) S( r0 G0 x) L
- TCCR1B = _BV(CS11) | _BV(WGM12);
1 d, \4 {% S" k* K2 @4 R - TCCR2A = _BV(WGM21) | _BV(WGM20);
) Q, x; N) b2 ^% D1 b - TCCR2B = _BV(CS21);
4 @/ k: \- L! o- z7 ~) A. K+ H - Serial.begin(9600);
$ A6 B1 o) ~, J% ~; t" K - buzzer.setpin(45);
- p ] |0 Y+ p X( E+ f! F' L$ ] - }2 }/ x* j: e( d$ |- _2 e
+ W: p! V5 r3 _8 U- void loop()) T5 X! O' K% @; {' B! g
- {0 |" V* l5 o# a/ e5 v+ Z
- while(!((lightsensor_1.read()) < (10)));8 I/ j0 E4 x! c, X. K
- buzzer.tone(1047, 500);* A; v3 S* n5 I& J! [7 {3 \6 K
- delay(500);& i, b. r3 M8 F9 K: W$ ^" F4 `
- do
$ C3 P6 U8 ?' D$ Q - {
3 c. y9 X% r% l! S( r3 |6 L/ _ G - int Err = getErr();. N5 Z4 r6 F' S! Y' w/ w3 U. ~: p
- if(D1 == 1)2 a" ?( ^# q1 |
- {
2 |6 y6 n' z: |8 }/ v, C - Left = 1;! z, Z! I& T7 v8 ^
- }6 s4 `9 u( r, v. [ F
- if(D6 == 1)
* B& P4 Y: C$ @. @, j! {. l - {
b/ N7 ^1 ]8 O. Z: `5 P) m7 q - Left = 0;, w( [- l& q9 Y, H) R- d+ ^* N
- }
' O- ]9 h- B7 ~2 u - if(Err == 99)
2 L1 A$ H1 X. t# c1 S - {) m& \/ m/ s; I) r( W
- if(Left == 1)$ `1 n6 \; [$ k$ `& d) [* z K4 ?9 j
- {
) L4 u6 W: `1 a5 ], t' D8 |( z - Speed -= 5;
, F! u$ W+ q: y& b - moto(0,Speed);, E Z3 Z: a8 }, \3 o0 }
- do
( j+ Z1 @4 K6 I) V3 w, H! J - {
7 K/ z3 ?$ D* H& E) N" {$ C - Err = getErr();
1 n! ]% d8 _; f - }while((D1+D6) == 0);
p9 m4 Q& P: w. h2 X - }
9 ]" I; @1 Z, T' J - else* N: |& j/ d0 V& B
- {* D& A, H! B# S6 x' {# s5 V
- Speed -= 5;
]# O+ \3 _. v) ]. a5 Y6 s1 w - moto(Speed,0);
( P" ^- w" J' L4 _4 a - do; u0 L/ ?* `" `- l; e8 v1 E
- {- L% O4 e6 F. K! y" t! s& i3 g' W' q
- Err = getErr();
' y- o3 B# _; T8 l1 @ - }while((D1+D6) == 0);
; b) r$ Z$ k% I3 S9 ?8 {, a - }3 f( w5 d' |0 I3 |" K8 J- O) A5 _
- }7 S) a o3 |( j; W0 W
- else
* K/ t# [% A. a - {
8 O/ |4 f5 ]/ q: ` - if((Speed < 160) & (Err < 2)) Speed+=1;
$ A+ c( p+ Z* \9 Q# e - if((Speed > 100) & (Err > 2)) Speed-=2; 6 N5 e/ j' e. u5 _" A) k1 ?; Q, L. T
- integral = integral + Err; [9 `$ K- ]5 s0 @
- derivative = Err - previous_error;
( t9 |8 e; |; B! M4 K3 |4 J1 n - output = Kp*Err + Ki*integral + Kd*derivative;
e, K2 c8 L6 G* u - moto(int(Speed-output),int(Speed+output));1 U3 c; K+ {' y8 M* l8 b# W
- previous_error = Err;
( z. R* j* b) ~ i5 p" f8 A3 v - }
; @7 d$ Y0 {7 {! f3 ? - }while(!((lightsensor_2.read()) < (10)));
) ]. n" ^3 n0 |' P, @ - moto(0,0);
! {# ]# Z& I9 B - delay(500);
: [# Q: {& l4 Q5 R - buzzer.tone(262, 500);
7 G! n- w8 c7 k' t4 e - }! Y+ g* v) A' u
, V; f+ L4 b0 e- {* Q- int getErr()
5 j1 V( |( I6 q, P9 O" a" V7 W - { 8 P/ l, H) e4 J9 B+ K* T/ D
- sData = linefollower.getValue();8 K. f# G9 w* r' G; Z. ^
- D1 = ~(sData>>0)&1;
' J/ [% K/ ]% N - D2 = ~(sData>>1)&1;( m. q3 c6 n/ Y9 @5 |/ b* t
- D3 = ~(sData>>2)&1;2 J2 j; f6 s; H4 f
- D4 = ~(sData>>3)&1;& ?# ] s! [: M* P
- D5 = ~(sData>>4)&1;* w u3 ~9 y1 u. I" R% {8 s( I* h
- D6 = ~(sData>>5)&1;7 v; {- C, [5 d4 }( G2 k+ V
- int downD = D1+D2+D3+D4+D5+D6;/ m r# T( u% f& V! o2 C K9 c
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
5 {3 L* w4 w' f - if(downD == 0)
- q7 l4 U( o% ^% j& U - {" L9 @% \2 L4 Z$ B2 \
- return 99;
! h; g8 V4 b' [; z; d# L& K! f; @ - }
& M8 a+ B! {4 i: y7 `$ x( _ - else* Q/ M, i( O L# ^( ?
- {/ X4 `5 {9 f& c H1 d* {
- return upD/downD;) b) d B* j9 e9 z3 Y8 o$ U0 G
- }
, r: G6 {; ]3 M% K - }2 h+ u# q* f( k- `) m$ L5 k
- , V- p, f" |0 o' s$ n: K3 Q3 Y
- void moto(int Speed_L,int Speed_R)! \/ `. r B! V: O( O+ V: Q5 t
- {
O% r5 [& O2 P9 X& j - Encoder_2.setMotorPwm(Speed_L);
& p9 {& ]! U/ H2 K - Encoder_1.setMotorPwm(-Speed_R);, m- `: q4 H& K2 U
- }
複製代碼
$ y- O; l. q% WMeLineFollowerArray.cpp( x( }' \0 m) e$ e9 }) }9 D4 H* W; ^
- #include "MeLineFollowerArray.h"
3 A" g) l: X7 |; F; G
) |! G/ v/ n# V1 f/ `4 b" `( ~: A- #ifdef ME_PORT_DEFINED6 B3 O* b7 |1 e6 Z
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
( h! J" S; ~' O* V V+ Y - {( L4 p9 k' [$ E; n+ M9 o
- j0 {' c r0 X' _- ~3 B
- }
, T7 c0 ^1 U4 I2 |3 E - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)$ [1 O* H8 k0 C* I R" C, b( M1 A4 v
- {/ Y% }; i0 q; Z u. Q; F
- _DataPin = mePort[port].s2;# q: n* c% p8 Q, L9 Y
- pinMode(_DataPin, OUTPUT);
% p( }. k2 u9 T - digitalWrite(_DataPin, HIGH);) G# J" c; n a4 o$ w
- }- i& t& L2 v Z! }
- #else // ME_PORT_DEFINED
2 p6 C$ y3 N( C X' u; u - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
1 z( N; \8 d' Z0 o - {; C( T+ \0 ^: U4 d' w. l& n
- _DataPin = pin;- F$ b+ v/ G1 }
- pinMode(_DataPin, OUTPUT); . `& ^" q& k8 m) t5 T- ]6 |- l
- digitalWrite(_DataPin, HIGH);
% c6 ?; a2 @" H" Y0 s) C5 [ - }( n# X: s0 Y4 r% s, u* l% L, \
- #endif // ME_PORT_DEFINED
4 E) ]$ l! l M; V - 6 c. V% E0 b( B% B8 W. R& ^
- : d9 ?% n* O8 ~6 M# Q& p
- void MeLineFollowerArray::setpin(uint8_t pin); @( V# {+ F, n0 w" s2 j: V
- {* [: P0 @" R' `% E
- _DataPin = pin;
. S- j4 N O- ]. B& ?7 ] - pinMode(_DataPin, OUTPUT); 8 Q4 Z# q3 ?$ E; @5 f$ K V/ p3 f
- digitalWrite(_DataPin, HIGH);
. p& b6 R {6 R" q9 w8 K - . a A( l" T$ I( h; Y
- #ifdef ME_PORT_DEFINED2 O b' q( _2 w c2 z
- s2 = pin;
4 c/ F4 O2 x" h* o - #endif
, G& l( b1 t; Q) t5 d! c7 h - }. z* D8 I( M% O N$ A
- 9 N" e5 P5 W3 V. X$ {/ |+ v
- uint8_t MeLineFollowerArray::getValue(), Q7 R6 r7 \! p" a
- {: n9 G0 j; p: i! P# l+ D' \ \
- uint32_t LOW_level_read_time;
1 c4 H" m6 { h7 \" N- l - uint32_t HIGH_level_read_time;1 `6 l# p3 C: X' y
- uint32_t time_out_flag;5 n' C- I$ c! X# I# B# X
- uint8_t Sensor_Data[3];4 O G% a2 B: C, c
- static uint8_t old_data = 0xff;2 Q' [8 Z- o, u0 `% ^
, `& ?1 P5 y$ \5 {* G2 g- pinMode(_DataPin, OUTPUT);
4 _1 s& |! A9 b1 d. {' S3 q; [; W - digitalWrite(_DataPin, LOW);% O& q0 c9 w0 J1 n% o
- delayMicroseconds(980);; h- p1 }; i! [: Z1 g7 u
- digitalWrite(_DataPin, HIGH);
+ r5 y( M( u- ]" f* [3 O( z5 i - 2 O5 ?! i, W0 W8 w: o9 p
- pinMode(_DataPin, INPUT_PULLUP);& a' ^& { U6 Q3 Y
- delayMicroseconds(10);5 ^0 }% ^, ]: W8 R2 F
- $ G1 U, u6 Y& F# [* j4 S
- time_out_flag = millis();
& p* q% L$ D- N. @! U+ u$ V - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! m/ T9 F* ^, W! [3 k" h* H
* s& H0 U" W- Q* }- LOW_level_read_time = micros();' u3 _7 @8 T5 G
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ R& P4 i7 o9 v7 V3 s; a; m
- {
5 Z6 |& [* a$ v% [/ p2 m - return 0xff;# q% m( z7 {0 |- E
- }
/ h3 Z# V. w) @+ k( T
4 O$ W8 z1 Q+ L) h* N8 G+ f' m/ G+ V- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 c6 x( D B( d# o: [3 F
1 v* A9 ^ o- f1 i4 D- HIGH_level_read_time = micros();
; W0 q+ B: M# a$ M - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level4 }) _, u" x* }" O, r' T
* E8 d X- f0 H4 K' U- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 ^* j7 f, {9 v
- {4 r1 I i/ a* ^0 h: p
- return 0xff;
0 N5 P7 v! [( q, V: N6 i$ T; T - }& M7 v& g/ Z z9 ?8 z+ e% `( ?
3 {, G3 H1 c& c9 w. T3 G" Z- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
: l! Z1 F' Q* P! k9 J - {7 L. [" G) l4 |9 @! H
- return 0xff;& _ O9 d. r& I/ \; F
- }& `5 N, x1 b5 Q6 s
& l# K8 O( ?- L! ^) U% S9 X- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% ~9 C; v% @, h: y3 Z" w% N/ \ - LOW_level_read_time = micros();
* t) j; F* D8 v - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level- v. a8 B- ^, D, e- V
- $ {' j. y. k3 i0 v
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 o, {. H! F0 K' G& E" ]5 c
- {! Q8 `8 V% ]6 H; `+ u5 V; ]9 l
- return 0xff;+ g" G7 g& [' u3 @$ g1 ^
- }
" }. u% Q, A6 D - + s( \9 P- t1 q0 [, O
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))2 ~& |# E! {# s# O9 i1 V
- {
! E* Z7 r9 S: Z+ ? - return 0xff;: _3 i3 U1 |3 C! N, s
- }- E3 e3 F: j! n U! \; A
- % ?$ }$ q, s: P9 k8 S& P5 z
- for(uint8_t k=0; k<3; k++)
9 Q/ d/ }3 G/ ]" k# w - {, G4 z' m" C: X' F$ Z u H
- Sensor_Data[k] = 0x00;7 z! F" F6 y' P, ^4 e# [+ T
- ) f" G" W* |9 I7 A1 s' Y0 d
- for(uint8_t i=0;i<8;i++)
. u' v0 x3 X7 X. m e1 ~7 A$ f - {6 p! c, { g# r: B
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 Z- Y0 J5 M8 ?" I# G z/ _ - HIGH_level_read_time = micros();
* V* `) T. F. Q - LOW_level_read_time = micros() - LOW_level_read_time;
% g' v$ h' T, y( ?9 B2 n! P: M9 s - 6 Y) }) a: y* e+ O0 x8 L
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" Q y/ t7 k9 d) _9 p i' I
- {
3 _1 c) A$ R' T - return 0xff;
+ c, a' j2 [! g+ j - }
/ H2 P) j; d1 s- g$ V: w$ Z - 5 z5 X! B9 H' X7 y3 f
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; d& i0 n* Y' q: D* R% e - LOW_level_read_time = micros();
4 J; n- ~6 _7 v& ^ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
! t. x- }! R4 I9 F2 S: K! G - - C4 t8 |' E$ @3 _) W
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1. X0 G6 K# D- Z- F1 a. }
- {, g9 Z! |! @8 a: K
- Sensor_Data[k] |= (0x80 >> i);( q' K8 b& U6 G! w# W, J$ m2 u
- }
4 N3 `) `/ ]4 [ e& L - else if(HIGH_level_read_time >= 100)
& H( e7 g# c& r8 K, a - {/ m# c, T1 Z2 v
- return 0xff;
) R P) \2 m- P6 H2 V' G" {" n0 b - }
L9 u: x% ^! B% ~! d - 8 p- Y6 t. P& n8 B- o
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)) }* v2 K3 U$ r) M2 n: i' e
- {
0 P( b0 Z1 P, D3 {+ K2 s, C - return 0xff;
5 f h, X7 I- {0 \ g, T7 b - }
/ r, P0 h5 M# d) q3 g" U6 ? - }
& r8 G) I1 m( h, U' F* m, B - }
) L" {) C7 t0 {, h6 P
: `0 l. s( N( f. L6 l) o5 @/ O- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* \: F: |' O/ {5 N9 H
- HIGH_level_read_time = micros();- a$ D$ [, m4 X b# b
- LOW_level_read_time = micros() - LOW_level_read_time;4 T8 B! I+ H9 U; w* B
7 Q0 _& X' L: ]- ^% ], c- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )1 N: h% S# ]6 K9 t
- {
5 F9 Q) R3 [. } - return 0xff;
3 W, Q7 n9 @2 L" m - }. I% F5 P: I+ ]$ t. ]9 f- j3 Z
! U! f m3 C& L* }" R- pinMode(_DataPin, OUTPUT);! c4 V' \% |0 p. _3 r! W/ _
- digitalWrite(_DataPin, HIGH);
9 I7 M" ?9 @9 p8 [4 d
5 n4 v+ A0 M# U9 D; X, W7 [. U/ D& u- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))& I- j B% T( x
- {" R U; T5 ~$ i
- old_data = Sensor_Data[0];
0 G8 o9 ]: ` p+ N- z6 | - return Sensor_Data[0];( r! {1 Y( {" l
- }
' a; {8 }$ t& @: Z( k) J - else- \* C5 S7 e3 O. s! }6 w
- {6 E! F5 q1 f0 x9 ?
- return old_data;1 a( \' S' f6 w% t% Y" q* D
- }
8 s" P/ ^! Z o% G# z! e - }
* o$ M6 @4 l' I" L& B0 k
複製代碼
- \0 k5 }6 W) M- pMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
?* N( v+ i; P* v4 a6 m, @8 q5 P+ ?) y. P# h
|
|