|
|
( h: k& `7 g# e& V+ e2 o- #include <Arduino.h>
9 F- P9 ?. F l# a1 z# L - #include <MeAuriga.h>% `( I+ o+ n4 I- ]
- #include "MeLineFollowerArray.h"
5 n+ b7 n* F2 J$ X - ( Y. ~7 E, [2 ^8 t% X5 \% I5 r
- MeEncoderOnBoard Encoder_1(SLOT1);
8 X* e( Z& x3 s7 n: A - MeEncoderOnBoard Encoder_2(SLOT2);
& ]$ v' N& r2 G3 \' b - MeLightSensor lightsensor_1(12);. C7 m0 \3 x$ s
- MeLightSensor lightsensor_2(11);
* R: t; N3 R5 i7 {4 m - MeBuzzer buzzer;
+ R* \! |* c& E3 p: {) J! ?7 m - MeLineFollowerArray linefollower(PORT_6);
1 q6 a6 ^3 Y, G# E5 a: G
5 F3 e) F1 b9 g6 j' h) A/ R- #define Error1 1
8 j% @* H& s, f - #define Error2 29 W: Y" P& i( J) `% W
- #define Error3 39 K6 ^, e$ t4 U" b
- : {7 e9 n5 w6 F0 s$ `; X
- #define Kp 152 r! W/ ?' e) j; p$ p
- #define Ki 0.155 k+ ~0 J5 m! S2 [
- #define Kd 0.03& ] \9 j$ b( `4 x' @8 w! }. R* v
- / x: U# b' [. z$ S, ~3 `1 W
- uint8_t sData;
2 b" H" N& y" B! |7 `7 |# f, a - uint8_t D1;
5 E* z# s/ T& h8 Z - uint8_t D2;$ V& w, F: n( x9 K* ?# A! g I
- uint8_t D3;
* e) x. v& o: c8 H! q - uint8_t D4;, ~5 N; [5 Y Q$ h3 _6 u5 j
- uint8_t D5;
+ w: S: b- p$ G+ x! r" _ P1 A - uint8_t D6;
4 x1 N* b" v! B: R! h9 p3 j# |
D ~, d# U! f$ @+ x* C ~- float previous_error = 0;5 Y+ ~9 y( Y/ n5 l
- float integral = 0;
1 _# }( q/ m, d s - float derivative = 0;1 b: |* H- B! M1 n6 m; ] R
- int Speed = 160;
& ]$ t3 p& Z, G8 x0 l - float output;1 X( p! s$ o# L W+ ~7 f7 H
- 1 `. t% `, F% f- {& d! A
- byte Left;
! J6 e% A+ z4 }) U! P2 O ?
: ~$ b% W0 A/ b" T' j0 |# I- void setup()
) e. x: z' F8 p6 q2 w% p* X - {
! e: M- g# c2 Z( b2 i0 m6 \" x - //Set PWM 8KHz- ~" S; `7 |4 j8 k7 m
- TCCR1A = _BV(WGM10);- R+ r7 u( E' M( ~* |+ F0 v( S9 q$ Q9 q1 V
- TCCR1B = _BV(CS11) | _BV(WGM12);2 O3 d, G; L/ x% Q5 m y" m
- TCCR2A = _BV(WGM21) | _BV(WGM20);
1 h, x a' ] g0 d6 y' q6 G, ]$ V - TCCR2B = _BV(CS21);: G# o- H; E3 ^8 F8 h6 E) j4 U
- Serial.begin(9600);
6 z( {, l# ?! F' n4 ^- P( Z - buzzer.setpin(45);
" c* J: y9 m5 z8 W - }3 P' l) B$ @( Z
; B& X I5 B" Z/ q, {+ V) b5 }- void loop()2 ]; G) I% D$ g8 }
- {
# ?& H9 W# u* ?2 K8 N - while(!((lightsensor_1.read()) < (10)));2 U) T7 ]' a$ v% O" I* t
- buzzer.tone(1047, 500);
) K9 S2 t. @6 A( ~ - delay(500);
! k. c) B- _# v- v' V3 d! L - do( y$ o8 R, d5 t- T
- {4 E; f2 F9 B8 Q% g
- int Err = getErr();+ ]- S w7 V) P+ \: X+ n! z
- if(D1 == 1)
$ B6 S1 B/ g7 J# G# x. y" X - {
* m! n2 L* W b - Left = 1;
6 C7 H: z' c9 I+ }6 }' F! d - }) e7 u& @* x8 l% ^8 G3 z
- if(D6 == 1)- B% Y$ k$ e, x- |5 l8 {
- {2 z2 ~5 T8 O9 k
- Left = 0;
; b( R& W% B q" Q( { - } F' [# ]2 X* ~4 z+ g
- if(Err == 99)
; d. k1 g$ g5 ~" g/ p7 _' a" Y - {
! ~- W& b/ Q& _% e w - if(Left == 1)! O3 U4 d) N1 h* Q
- {$ V+ k3 H( e8 w) O# i
- Speed -= 5;8 r" V. q, V3 `) l2 T
- moto(0,Speed);
* }' ]! k- o9 X2 Y! H5 p - do& b9 @. M* e/ Y0 s8 {/ _' x& i% J
- {* T7 D6 D" W* R' t, p) w9 U* d2 m/ h
- Err = getErr(); ^; k$ ^" \% S& N! _4 z c5 i
- }while((D1+D6) == 0);/ ?& Z8 M* V0 C2 `: p# e# [
- }6 Q2 _5 o. V- I0 Z' Z9 W
- else8 b; a1 S4 e$ A% ]
- {
- v6 _2 [1 q+ T$ L6 \1 Q - Speed -= 5;. d- I8 Q `1 r# j) T: {; F. W% Z
- moto(Speed,0);
' E$ x* p+ f0 ?2 ]2 _# N; K- i- v+ r - do
2 n, f& w$ W3 P+ X |8 v$ l% t - {
! q! C6 z6 c: E: s - Err = getErr(); \% I9 m5 Y# e( W* q5 O
- }while((D1+D6) == 0);
% K: ]) s' R( J3 h( }0 [ - }, \8 z* c( L" f9 ^" P% s
- }' \, }; I* E4 n4 Q* Y- W
- else H- L0 }' Z7 Y/ w1 ^! ^
- {
+ s9 a# r. r1 F7 M! R - if((Speed < 160) & (Err < 2)) Speed+=1; # B4 d! K! L5 v6 }2 R; U
- if((Speed > 100) & (Err > 2)) Speed-=2; 5 ^/ L% l4 Y; V* t7 n( W0 l4 z
- integral = integral + Err;( t; }; ^$ X2 U2 H
- derivative = Err - previous_error;- Q9 f" O, ^3 B6 S2 }
- output = Kp*Err + Ki*integral + Kd*derivative;6 u- P) S" A; N8 w; J( l) ^2 {* w
- moto(int(Speed-output),int(Speed+output));
+ D! y( k* I: Z( [/ A - previous_error = Err;
& W% z: x+ {, M - }
' }( X! ^+ H; t& Z! i, r6 Y( W - }while(!((lightsensor_2.read()) < (10)));: H9 J6 \7 q% ]7 t z+ |
- moto(0,0);2 M0 {3 s1 a; q' M$ w; W1 |
- delay(500);
5 V( s, r& G1 k+ `6 Q4 p - buzzer.tone(262, 500);* O; i. {+ K: W/ U$ x' m4 V6 x/ B+ ]
- }. ~6 [2 N0 L+ L. c" E
- 5 Q$ [& u1 p! F. Y3 a" ]# ~
- int getErr()0 j! E- b! U2 H5 W9 J N# }
- { " C2 j2 t; c2 q) L9 }1 k
- sData = linefollower.getValue();
+ I+ {# {' T! Q- w& G - D1 = ~(sData>>0)&1;
, Z8 E7 q+ Y' h X! Q# E9 | - D2 = ~(sData>>1)&1;
3 H& t, v4 [; D1 l - D3 = ~(sData>>2)&1;
$ m( X8 V9 ?1 K - D4 = ~(sData>>3)&1;( Z4 A$ K7 p' E
- D5 = ~(sData>>4)&1;
$ t4 i+ d! I1 X0 E- w1 a - D6 = ~(sData>>5)&1;3 n, C/ s) ~; J$ ^
- int downD = D1+D2+D3+D4+D5+D6;
. V: S* I- a a1 F - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
+ ]; {$ A, P8 q2 w: {2 d - if(downD == 0)9 n. l; r& R7 g
- {- j, \6 U/ p# |3 L
- return 99;, W! o. ?+ f9 s9 f3 H0 ~
- }
9 m! f) k" X: V+ v# }* z - else
7 } t3 ^* Y/ N( O - {7 l. n7 v# G$ m/ _$ v- B
- return upD/downD;
. f; z3 o, G7 j% o) B6 T3 t F - }# }! J( r3 B# [! e8 f0 x) l, d
- }# g) F' R5 }+ L
- 9 b$ {9 h2 w' x+ ?+ K: ?2 N8 \: l
- void moto(int Speed_L,int Speed_R)
* P* _6 k5 l3 Y$ x; u - {
$ @, S o- }2 V- y" g; S8 L - Encoder_2.setMotorPwm(Speed_L);
: K# E' t& ^3 m2 ^* E - Encoder_1.setMotorPwm(-Speed_R); H; J& b! ]. Y5 r
- }
複製代碼
- H$ }' X1 V* G( G1 C& R- S& nMeLineFollowerArray.cpp
' D5 x4 i# S: a$ i6 k6 I0 V% Y- #include "MeLineFollowerArray.h"4 ]+ n: C9 p4 c
3 \# D# J+ G4 M: o- #ifdef ME_PORT_DEFINED
4 e4 g7 D* J# ]+ `+ B9 \/ s: K - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)6 p( s% t9 h! E9 Q1 p
- {
P& m7 [7 y+ L8 I3 r1 T - ; k8 a/ X2 m' m
- }
! m9 G3 f4 Z/ d; c3 j - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)1 |* i/ E" p; \0 S4 ~# B$ y
- {$ A3 L) C) V; _, j4 Z
- _DataPin = mePort[port].s2;
2 T( \: ?1 B% E& J! C - pinMode(_DataPin, OUTPUT); : Z, Z+ W0 g: z7 X
- digitalWrite(_DataPin, HIGH);- B% P k" q' g, q3 A0 i5 g
- }
a. ]+ b1 b) m* @ - #else // ME_PORT_DEFINED* R4 k* ?+ L9 J+ y. B8 p& e& p* d
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)- j- W ]; g1 r% X; [
- {
. b: U0 Y* U' q& ^ J - _DataPin = pin;4 t( k, W: |2 p8 E% k" P1 w
- pinMode(_DataPin, OUTPUT);
. y/ x4 x6 K, w1 w. T( B - digitalWrite(_DataPin, HIGH);
6 o' l$ ]* r; c% X - }* b( v& k: g6 b l/ n* E# ~
- #endif // ME_PORT_DEFINED
/ B$ d' H# k' W. o# }& v2 L& a - 5 o0 @$ L& q5 k" g, g% |+ c
6 `' Y% X! W) d! A4 }8 z' v- void MeLineFollowerArray::setpin(uint8_t pin)7 K4 C! r$ F, B9 s8 o
- {
P, |) z$ s2 f! R6 l* w+ D; U - _DataPin = pin;
3 B- N6 ?' L% Q+ N - pinMode(_DataPin, OUTPUT); - \6 }- z* o* w! v6 r
- digitalWrite(_DataPin, HIGH);
( m g; C# Q- O, [' G; p( P - 2 X8 E+ M' R* x
- #ifdef ME_PORT_DEFINED
2 s S7 ]' V8 g* S! l! ~9 Y - s2 = pin;
8 Z9 n, J4 N9 d0 w) o: Z3 T - #endif# D/ [, P) m* w+ F: j+ O
- }
5 z. h$ d$ W' n, Z; u6 H# j - ' c1 p$ A) }+ w& {
- uint8_t MeLineFollowerArray::getValue()/ q e1 `: o. m8 z; B7 g
- {
- W7 h# k# F4 \' @/ |+ r - uint32_t LOW_level_read_time;# Q Z; ]& x7 q# ?! i i- O4 i
- uint32_t HIGH_level_read_time;% ~; @$ f, C G p2 w# j2 Q" e
- uint32_t time_out_flag;
/ _9 A6 {( D( I" `; ~, O - uint8_t Sensor_Data[3];
% ?$ t) W% z6 x) x ?7 m - static uint8_t old_data = 0xff;
; s5 b6 i7 M" M - " n+ V! w1 v6 T p ~
- pinMode(_DataPin, OUTPUT);" I) B* \5 F+ }$ n) d# a1 } C
- digitalWrite(_DataPin, LOW);
0 R4 U: n, v) ~; ]$ t - delayMicroseconds(980);: i7 g7 F3 B4 z
- digitalWrite(_DataPin, HIGH);
' D% e8 I5 M8 u
# e# I0 ~( D$ {2 U* w* I- pinMode(_DataPin, INPUT_PULLUP);
7 x# Z3 j- p& l1 N7 z: u5 u - delayMicroseconds(10);3 S) x. t- A" I
- z& H9 S. K. v0 ~. y- time_out_flag = millis();
$ l D3 ` j7 @! y) G - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); J# P4 P+ h- ]4 N
# B: x! X9 X& x7 b$ h6 C) e) @- LOW_level_read_time = micros();0 F0 m" N% T' C/ }. @- G' A1 a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, H8 O5 P. Y5 e. A' v* V - {% \% s; r* d& j. k) c+ |$ m
- return 0xff;
R3 t" W' l2 ] |* X* H - }
N9 N7 a5 s2 ~7 A9 c; b/ a% {
% }% z/ i8 d0 W0 z! l" t, B9 @/ H- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& q$ [3 [8 ~! `$ Y3 U
- ! N9 `4 i, C7 L1 }5 i r
- HIGH_level_read_time = micros();6 ~( `5 |7 `2 ~) n, T V% `
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level2 @4 a! T& t) y
. T' f; ]2 A: C- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 m+ q: u l2 M$ n* p o: ?. B' n
- {
. g- \5 n* _6 z% l - return 0xff;
2 q/ r* D S% k& f' G/ L - }5 W8 F/ }3 f6 x+ Q
) x) K+ ?1 {9 f! Z! f8 @9 o- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
8 ]8 G2 m4 a& U9 u - {
3 P1 i- ^: Z S. p - return 0xff;
, C/ J8 {1 }4 _9 { - }
! T2 J7 ]' u& y( |' O" x V0 h - 0 M# v0 Z; e5 A- d- @8 g1 G
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* Q) Y" m; C( ~( W9 Y; T1 [
- LOW_level_read_time = micros();
5 D4 J" h+ D& }/ z* C& V - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
, ?7 D9 q F; a0 L9 ]' Z0 K
' y+ a" a7 L1 u0 ?* Z4 `; x" A% o- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ I9 n. v- ?+ |1 | - {
4 P$ c d% M; q* j6 }# ?" x; B6 R - return 0xff;7 b# g0 u% B( K5 k( W( M# L" w8 Q
- }; n6 G$ l# V! x% v
9 M- T( @# o& g5 e8 P- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))7 K& E7 a* K$ b& h" V
- {0 g8 m6 ?1 D1 f
- return 0xff;( m8 T4 d' E9 b% y3 {: M6 _
- }
x5 I. D: b3 F
% D& y5 H4 L3 y3 j: j* u H- for(uint8_t k=0; k<3; k++)
8 x3 b* t Z0 v/ u - {# z6 N$ k# ]! U4 f u! G' J' E/ O$ |
- Sensor_Data[k] = 0x00;
. i! b' [% j( ?! j1 x
3 \5 B* `5 p( Q" M; d9 m- for(uint8_t i=0;i<8;i++)
$ K: e4 h, t4 a7 v; {$ ?' H2 u - {
3 G% ~8 x+ O' Z! z! o. W. F - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level; Y: b( ]4 x2 }8 k2 `3 ^
- HIGH_level_read_time = micros();1 f/ q3 U) Z* Q( R& Z/ U; j
- LOW_level_read_time = micros() - LOW_level_read_time;
3 Y; O0 b4 y. h' k
0 _! J `# O4 }/ Y8 t- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ); f& O- q- C' t3 x7 u% T3 m T1 P g
- {9 v$ U, E* s. t) ?( W Q+ E
- return 0xff;" m: d) K; B0 u/ {
- }
9 r0 u: [5 h, e3 p6 ~6 ]
; Z# A: l" u: O: ^: ?7 }- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ \. S4 X" f: j1 i - LOW_level_read_time = micros();0 L- d ?' i: k6 |
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
9 P0 k, s9 M& c/ t6 j5 L7 @ - . W- Y, D I# A' D% P7 K
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 10 @) R# L, o! }7 s# e, v5 ^0 T
- {7 t) `# G; v1 j+ l& [3 Q
- Sensor_Data[k] |= (0x80 >> i);
: y; F/ X) [% Z5 g# L - }; g- o$ F* H: a' d3 Q, }
- else if(HIGH_level_read_time >= 100) P+ C: p! O1 I( b( G3 U* ~/ G
- {2 q, j( ~) P( c# U
- return 0xff;7 w" \8 s* o& D+ I. D* b. A% F8 B
- }+ a! @, w% @, H: `) e1 R
- ; f/ r6 w( E. |7 l1 M7 m
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
0 n; d' x" f; l: T - {% v5 y, y% o j; x4 B
- return 0xff;9 ]: g$ j1 s* {. o% H5 C9 C
- }0 E: ~; m5 r4 X, ]9 k: ], P
- }
2 J; g8 }2 f0 z) z2 v - }, ?5 n. {; h! a0 V6 p/ z" |3 l
2 C, p/ A3 {$ T6 x6 J1 t- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* t; h, |, q) T; a! m @ - HIGH_level_read_time = micros();
; o) z. Q1 F8 n& @% n - LOW_level_read_time = micros() - LOW_level_read_time;
8 O& ]9 O" I0 Q+ k! X! T" M. u; A4 o
( I, {6 C; l+ ^& q. u- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )9 [/ G) z. n8 l& x
- {
) f. W1 W3 \5 t" E - return 0xff;+ N( R$ U/ |+ [
- }
6 W8 d. D/ W* F6 e' \9 v. [
4 } c5 O) w* [* |# {% f! g) u+ {- pinMode(_DataPin, OUTPUT);* M7 _. G& ^; ?' S" B8 {& U
- digitalWrite(_DataPin, HIGH);
1 I3 j" V1 G4 `0 r' j - % R9 l& s+ G4 [/ R) R: O1 r/ }
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))0 V+ }* w& ?( K0 b3 L5 j
- {
! b' e# U0 h2 K5 G2 D - old_data = Sensor_Data[0];- Q9 Q8 s1 r( h) ^) K9 o
- return Sensor_Data[0];5 h3 N$ r5 {6 B2 [2 a8 Q
- }9 U7 U6 ]( v' c2 |- l8 L* j( k; ^
- else7 v5 C9 D# L( y [' b" J2 {
- {
* G3 Z' h& Y. @* ^7 L( _( n - return old_data;( H9 X4 R- P0 E/ ~' m0 {% r" c
- } m9 I" w6 T6 I7 t8 C
- } |4 N% }( ~% E7 f
複製代碼
- c1 A: N3 C4 L4 s" r1 RMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
0 d, e/ C& M) ^' l) |: f6 Z
% I3 c1 U- Y) }2 i! U |
|