|
|
0 j$ ~) z3 x; h. K. ~; A3 d! S( E
- #include <Arduino.h>% l+ R3 r5 h+ L
- #include <MeAuriga.h>
9 x5 Y9 K( ?3 f: v0 C8 K4 M6 P - #include "MeLineFollowerArray.h"
- Z% F* ~+ U; \/ y
' K: ?6 H" B- X- MeEncoderOnBoard Encoder_1(SLOT1);6 s! x- ^1 p. \. ~; E6 T3 U
- MeEncoderOnBoard Encoder_2(SLOT2);+ P/ `/ ?: W# b, k9 _7 t8 |
- MeLightSensor lightsensor_1(12);. ~! t6 I/ c: s; T5 G% u% V2 o$ \
- MeLightSensor lightsensor_2(11);
5 ^0 ]/ H3 T1 N5 Q$ G0 s3 J7 b* X - MeBuzzer buzzer;
6 a. k2 O- p& O9 I, w! e - MeLineFollowerArray linefollower(PORT_6);! F0 z v R4 q, }& \ ~
- 5 K5 Q$ _, V5 I, Y5 t7 I$ V, R
- #define Error1 1" D, e2 Z& P. S1 P& o
- #define Error2 2" A$ J" x1 ^" r9 d
- #define Error3 3
( {- k, C; }4 ]/ Z% n5 ~
! |, E3 p3 L0 ]- #define Kp 15
+ k6 ]9 w6 n# G* z3 Y - #define Ki 0.15
* c! E6 @7 b# `" ]9 O - #define Kd 0.03
1 l8 I* @5 f* ? W
: G) o& |- N1 \. w7 i# F$ f- uint8_t sData; _1 h* A& ~$ `3 t9 o8 W" c
- uint8_t D1;
U2 u0 _. h* {. M' ~ - uint8_t D2;# [1 v$ O/ q/ Y2 f( R: C H! r
- uint8_t D3;+ p N! M# u2 [/ N+ R. a" X
- uint8_t D4;$ H& X# B- t; D- j# I- |5 h# q
- uint8_t D5;
) x4 h9 \7 k$ l# g C+ |0 [2 M - uint8_t D6;
, I# L" K/ g) B - : K0 b. Q1 ? M2 f
- float previous_error = 0;1 y) E% f, H2 `: y2 w& R
- float integral = 0;' f: ?, G/ d, G( K' ] H$ H
- float derivative = 0;
3 S/ e* I N1 r- w - int Speed = 160;! y, ~0 v. M/ C$ W
- float output;1 I/ v) J; e4 F% R
4 H2 |- ]& d& j, a- byte Left;
7 v7 F: g$ x$ q: R f - 0 r2 ?; X! ]8 b2 `6 A: |
- void setup() 6 C6 n' f$ `/ o p0 p' J" E
- {& B" [" {: m s; |4 A' j
- //Set PWM 8KHz* C; O. S+ e! H- b
- TCCR1A = _BV(WGM10);
1 {! w0 c3 i2 _! a( _# L2 V - TCCR1B = _BV(CS11) | _BV(WGM12);' n2 Q8 s! Z1 t$ k
- TCCR2A = _BV(WGM21) | _BV(WGM20);
1 {5 b( a5 I( y4 E1 W! V3 x - TCCR2B = _BV(CS21);
$ L9 Y6 V2 e& u8 I U- N - Serial.begin(9600);
; `6 w K$ m# V; ^+ {3 K2 r - buzzer.setpin(45);& _2 c6 Y- W2 b
- }
& I0 o0 v1 ~. G: O0 T( D" ]
' F0 q, D6 e5 O+ d$ X. p5 P1 [- void loop()
' @5 {0 R8 V- ]2 \) T' ?3 b' p - {8 E* ] Y4 c }6 q( N( }
- while(!((lightsensor_1.read()) < (10)));$ l: E+ X* j, D3 ?2 @" Q2 q
- buzzer.tone(1047, 500);; r; e+ n' _: | B: R+ D: ?( F
- delay(500);; M! p/ _* z& W4 ~( |# k# c+ S' y
- do% o2 E% [8 [# J# P$ J3 O
- {
; ] a$ U7 g4 v8 d- @/ L; q - int Err = getErr();4 W( x0 C& j& m y6 O7 c4 J
- if(D1 == 1)
- @( \# _) ^3 E% n8 O: e( U - {
/ @- Z) `2 W8 C: z% S0 Q - Left = 1;& ~( `5 A3 R! A* i$ t- C+ M; Q4 M, l
- }
! J( O5 U4 F7 _$ C1 Y" ^ - if(D6 == 1)& J; y/ R R" `2 I
- {; f* F+ y0 |+ Y0 O: {
- Left = 0;% Y. ^( ?, v1 B
- }( n* Z3 y4 h) S/ I J# I5 X8 p
- if(Err == 99)
" M9 `3 j0 r7 M9 ~ - {
- x( r: q* a" g+ Q* R: V2 s - if(Left == 1)
$ K8 ~& n7 L) t4 o- `, f" ^$ F. X - {
& i- z4 q0 k) `# h1 p- ]5 a - Speed -= 5;
4 {/ _6 n$ T- {0 k$ x - moto(0,Speed);
# r0 ^! W9 J- c( D - do) ^5 ^) X+ v6 N2 c# ~
- {: q. O0 Z# y- R; q
- Err = getErr();6 K5 E/ c6 }+ r) s5 e: V5 b
- }while((D1+D6) == 0);" C) R' V! W; k' \! I
- }, W% T2 `+ S1 l+ Y
- else" c& N* C9 _3 O( H6 M$ V( L
- {
3 n8 ?# J9 B" `. x0 ` - Speed -= 5;2 j6 n, t& l! O X d8 R: H n
- moto(Speed,0);
' }. n% V) o+ I r7 N5 ]0 x - do7 s, A" F2 a3 _) H' D
- {. Y, E A: ?6 y w3 }0 b. _
- Err = getErr();9 p7 D# |% I: a
- }while((D1+D6) == 0);0 E# f/ r7 o) U
- }
( |4 c! _; @7 j, M! Q/ { - }
% H: p/ U m! l4 g {, ^: [ - else% Q/ K* |5 s; j
- {5 ?9 F5 [ O: m
- if((Speed < 160) & (Err < 2)) Speed+=1;
% X. g8 U$ e% ^ - if((Speed > 100) & (Err > 2)) Speed-=2; ' o" D5 N8 \' @9 g- F% g, C3 J& C
- integral = integral + Err;
' Y& o9 H! R" F3 G - derivative = Err - previous_error;: ~2 [* Z# r" y4 Z6 l- n( o
- output = Kp*Err + Ki*integral + Kd*derivative;
0 k* Z- Z# a1 k! Q, } - moto(int(Speed-output),int(Speed+output));# Q8 z$ j/ z( F# v0 Q4 m8 R' w6 C
- previous_error = Err;
7 ]' ^- _# G! g; F - }
' z5 w- ?) J. z$ T0 h/ ~ - }while(!((lightsensor_2.read()) < (10)));
: W5 H0 x* X4 B$ m - moto(0,0);
3 U( K+ [5 ~; Y% _% j - delay(500);# p' K3 f; ~* [# i
- buzzer.tone(262, 500);8 f* D6 X# X, }0 k+ w! `( Z
- }* A6 E) Q+ l3 z1 }2 p; _! H
3 {. @1 n! g% T4 k" O- d0 |- ^4 |% C+ P- int getErr()) d+ i( q6 N: P; h% l
- { # |0 A+ {" I( F! l/ C8 R S
- sData = linefollower.getValue();5 }. y @$ x8 v, K; |7 a# B8 c" u6 A( `
- D1 = ~(sData>>0)&1;- N6 n3 j% q+ N5 l1 B
- D2 = ~(sData>>1)&1;
9 w2 k; s2 F% n1 H- C) J - D3 = ~(sData>>2)&1;
, l3 r8 X$ U* m. ` M% }0 H7 I - D4 = ~(sData>>3)&1;' j+ q9 X& j# F3 w+ {0 T
- D5 = ~(sData>>4)&1;
# @$ [" u' x. ?6 u; ^# s - D6 = ~(sData>>5)&1;0 X7 v5 E% U3 ?0 V& r G
- int downD = D1+D2+D3+D4+D5+D6;. ~6 T( L) ]& d; h
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);2 [, R7 Z* }0 f3 Y# W- @/ x5 R
- if(downD == 0)5 i8 T$ |) B. D7 ~. t0 i
- {& N% f" T/ v# c3 o: [
- return 99;+ G6 w H1 m' M7 m6 X1 w
- }0 h% u4 q/ L! D, v& A% I. g
- else
8 ^( n+ ^% ]9 d5 y: Y5 A! Q7 w2 u - {( f& o6 h' u, I; m. C6 L- z0 j0 b, ?
- return upD/downD;
4 f3 e) q5 L+ P6 C - }
/ M$ W* W, K: C7 }) r( j7 f - }
% Z* o1 b4 @/ b
0 S; O" A8 ]; D- v- void moto(int Speed_L,int Speed_R)
- m' ?- o/ n) a, O9 a' e - {$ \7 h& W: ?& s$ Q) N; [# ^
- Encoder_2.setMotorPwm(Speed_L);
, a$ H5 c3 v& y$ _& |7 W7 K! O - Encoder_1.setMotorPwm(-Speed_R);
2 x# x: j+ t- }" c, ], H: | - }
複製代碼
) t0 ^& ~( e9 v$ i8 ZMeLineFollowerArray.cpp9 P s/ y4 k- j% R! \) F
- #include "MeLineFollowerArray.h"
! H/ g+ W4 d; j) u - & f* T; c" j2 T+ d
- #ifdef ME_PORT_DEFINED
$ }6 q4 ]& V5 ^: @3 |- L( h - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0), ?$ K, q4 }# [
- {
R' n5 u1 S4 e0 r' b - % k) p! u. v/ V
- }8 U3 U- z4 l6 c: O7 S
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)3 @: _. S5 r/ l Q' B
- { x3 w6 [5 F6 X- @# {
- _DataPin = mePort[port].s2;: Q& `* F/ V: k0 _
- pinMode(_DataPin, OUTPUT);
* ~7 i' I. O: X6 S% p3 C( {& b1 k - digitalWrite(_DataPin, HIGH);
5 _5 U* m& f+ H - }$ Y5 y/ T: P! t
- #else // ME_PORT_DEFINED' E( n7 Z0 U; Q6 |
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)# D6 U, M x1 E! C: x- M- V8 s) p
- {
A% W% R+ H& N, ? - _DataPin = pin;) T2 u# c' M6 U& ^ _7 J, e- t
- pinMode(_DataPin, OUTPUT); + K; m' {7 h p
- digitalWrite(_DataPin, HIGH);
9 f+ z- q6 g4 `2 J- q! ~ - }) F" i( x' x: G$ c" c" G
- #endif // ME_PORT_DEFINED
7 R# c! S/ E* k: n2 ^ - * T7 h8 C) q. }! n1 }: L5 U4 p
; o4 i2 f$ U! D$ I3 o- void MeLineFollowerArray::setpin(uint8_t pin)
, ?% H9 {" L/ e) d. a - {, r; P: t! R$ A, x7 m
- _DataPin = pin;
) [" `' j* Y" i7 { - pinMode(_DataPin, OUTPUT); + U6 F2 F2 s: [) e5 A
- digitalWrite(_DataPin, HIGH);1 g; V2 a- v# `" q! a2 l% o* U
4 m0 [( ^2 h" M' X- #ifdef ME_PORT_DEFINED
U* X! A6 ]+ b. h/ r+ g+ @ - s2 = pin;' I* D7 G2 z4 O9 g% V1 ~
- #endif
0 `4 L7 Q; _9 I" O. c - }6 D( ~ _0 x2 R! c6 g; p
- ! L9 ?3 M. L8 h4 H! b2 [! D
- uint8_t MeLineFollowerArray::getValue()8 ]8 S9 m3 ]1 x, }6 k
- {
# U0 [0 f8 O7 e8 v - uint32_t LOW_level_read_time;
* }' }/ k) G# J' }! ]2 C$ [ - uint32_t HIGH_level_read_time;' D0 ~. n/ k3 A1 B8 A! a
- uint32_t time_out_flag;6 W) t' E- T: D5 v$ ~
- uint8_t Sensor_Data[3];" |' B. d, P7 b% `* J3 {8 v. @
- static uint8_t old_data = 0xff;
& P& C8 l6 K3 x3 K- F
; Q: ^- z* f) u' F/ R3 @- pinMode(_DataPin, OUTPUT);
9 D }$ r, a/ ^# [4 Y# | - digitalWrite(_DataPin, LOW);
- E, _0 Y- z( n( B- i4 H - delayMicroseconds(980);
5 Q5 H3 z. |2 G1 G7 I; F - digitalWrite(_DataPin, HIGH);" U9 R4 S# s7 n4 M t
- ' e7 ?: Y1 ?3 C6 ?/ o$ P0 {7 l: {- |+ k
- pinMode(_DataPin, INPUT_PULLUP);
- W* P* ?* H' F$ ~1 |& L1 ?5 m - delayMicroseconds(10);
( i: i0 o1 V* \! {5 o3 Y - 7 x5 o5 V) w T w3 h7 ?$ H
- time_out_flag = millis();+ v+ C* R A5 u: g0 G, B& i
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ J$ T/ ]# K, Y* o8 o& W( v/ \( [
, s; g3 B" d! z6 |; N# E& h- LOW_level_read_time = micros();5 e5 t- B! k# W' L3 U+ R* ?
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ G- t7 u6 B: v; r: j: c# q
- {
/ e: A3 @! s* ~: V' V% X - return 0xff;
' R7 o6 E/ X* ^ P - }
; I+ x) c+ R- W( d - ; Y1 u. {8 ^; `4 m
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% R: D! V3 y) l% \' o+ h
& R) {5 N# ?/ ^( r- HIGH_level_read_time = micros();. o% O/ a1 s# [0 I1 }" F- L
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
$ g, o9 F+ j- \5 n, H0 j" U
: d5 _+ u. B: O6 n7 u0 K: e- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" r1 I- R$ a6 Y. {1 ^
- {, p! W; C# L! }6 U: y4 q
- return 0xff;' H0 I' L/ b# U
- }$ p9 O# h# h* a, \4 _
- & l+ G: U9 k0 a' R1 k
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))+ I! b9 {/ ~. ^9 A- n) ~3 K+ R
- {* Q: {! {$ d o* D4 m
- return 0xff;! A) p2 f9 r& _9 v$ `( y1 w' E
- }
4 l1 Y0 S! X5 V1 j3 Z3 b - 4 s; Q# v# N/ S% P1 V0 j+ e. E
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: B g- ]9 T$ c/ h
- LOW_level_read_time = micros();; _" D9 x7 w/ m/ G- K
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
( I7 ?7 Y* ~1 S* ^1 w; @- ^
, T9 T1 E& A1 e% V3 M- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! Y9 L- C0 l+ n* j. F# R/ s$ U
- {9 l( F# a. t6 n% h& E( {
- return 0xff;' F' P! G/ N0 T2 |# G: g
- }" s! E* u$ M: M( v
- % a6 b# H: |* j8 r8 S2 \( N5 Q8 O$ i
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
1 p; _/ k! ] h/ i1 K+ ?( H - {) M# S- K$ o$ G
- return 0xff;* d& C8 V5 v1 ]- X
- }
' \+ U5 A2 A( ^" r* C - & ~& `- ?) V, g* h2 l5 [
- for(uint8_t k=0; k<3; k++), @/ }9 q: \' u5 \
- {. l5 `) C. v/ e6 M( F! G$ f
- Sensor_Data[k] = 0x00;
6 `0 X. Y' I& F& a* W. _
7 j) M r; ?, i% m7 U% j$ G5 @- for(uint8_t i=0;i<8;i++)% w b) B4 X& p# Q0 M5 _3 ^: T- n
- {, V, R- l2 _# n7 H0 B: C" B
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 Q, s9 g) q4 W. \, L6 S: V' D - HIGH_level_read_time = micros();
: s9 V& s0 U; j8 x7 F" z6 K }% | - LOW_level_read_time = micros() - LOW_level_read_time;
k: _4 Y; S( J% C6 @
# l9 Q5 Y! S6 ?( O/ n& S- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )0 T, M& z1 u( y9 G
- {1 u/ a: E ^8 y: F. A/ Q. v
- return 0xff;" B* a( N! j/ L8 N
- }8 f; x( E' G E5 @0 n. d, M% H
- n" W$ i1 Q' l& k7 C. S- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( s$ `2 y4 q5 G( b" v! Y
- LOW_level_read_time = micros();& S8 Y# `1 ?* @, V3 ]4 R
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level# ^ c+ c8 ~0 o0 l9 o3 x
: u. n0 }: ]6 i' X7 L- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1/ K, ?* b; c( M) h3 G0 e7 r3 P& f
- {/ a' s3 w" ^9 }5 X T$ _8 \. e- R& A
- Sensor_Data[k] |= (0x80 >> i);. `! _! R0 k! |. S+ U& ^1 H$ Z$ f
- }& u0 F0 f1 A* ~2 u, F0 e7 D
- else if(HIGH_level_read_time >= 100)
* z2 F9 n% ~: P9 U+ c& S. w - {* E2 F5 }; b4 n1 r; W6 j
- return 0xff;
+ G1 E6 K. p0 M; n - }9 t2 ^7 y' Z7 b# z' Q3 R% f
- 9 L: y2 y- X1 U( o# f9 [
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
$ G( s" z) @7 i: e e0 t - {& p2 `$ L2 J. { N5 }* ~7 H
- return 0xff;# W1 }3 l4 @# u6 t7 }
- }
* r8 r( q1 o! y$ v' K9 o+ G - }* o! `( `. G/ G4 @# C: y
- }
, @+ ?! ]$ V+ d
/ W! w4 u) {2 w! o9 I3 z, @0 |- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 y- q0 L4 u' A! K# ]5 W5 h - HIGH_level_read_time = micros();
$ h: R+ C3 f5 B - LOW_level_read_time = micros() - LOW_level_read_time;2 N- |: O4 Z$ X' s
- " x" L! p$ G5 f8 x! q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) p: B0 d* l. I- M
- {
, a4 h; T- B! V0 V6 F7 H. K - return 0xff;- i0 o5 C2 k1 r
- }* j0 W' H+ }4 H8 X( w$ W$ r
- 0 z% @0 m3 m$ ?# P6 j& [; h
- pinMode(_DataPin, OUTPUT);
+ P- y7 ~- K; \% K - digitalWrite(_DataPin, HIGH);/ c5 i! o* j" F" R0 ~* e
; O4 q. Q: p) k0 S# {: ~ @+ |- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
9 J& \) `( s" w5 t+ n - {
/ f9 b% z. ?( \1 H& t9 l! J - old_data = Sensor_Data[0];
+ |1 Q r; @8 y - return Sensor_Data[0];
3 R# \ O0 }0 Z9 ` Z - }
2 S( h' T2 ~' v" B, Y5 v - else" t! {# m# @! l- H8 f
- {, Y& ]* q; n1 g: n
- return old_data;
* D) \# N' P( \# I, @- y - }. @$ c* M# T( j* ^6 X& X
- }
! \& g5 U, p0 s. q7 k
複製代碼
2 Y( X! I8 u( B( \ p$ z# [MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 y" N& I$ R- |7 h/ E9 w0 K/ ]1 b6 B2 N
& \' ~+ k( x/ x
|
|