|
5 |& P/ F8 p+ p: q# l8 |7 {1 g g
- #include <Arduino.h>1 |6 v P* ?# ?5 u) T/ Q3 `
- #include <MeAuriga.h>
4 H6 P% B* V" |6 Z- \, l' H; @2 F - #include "MeLineFollowerArray.h"
- g+ T2 S" |! u& L - ! w7 Y7 |, y9 k5 y% K+ g
- MeEncoderOnBoard Encoder_1(SLOT1);
5 D4 i/ |/ n' V) `9 U - MeEncoderOnBoard Encoder_2(SLOT2);
: c l- W( C& I/ c0 J% w+ ?, |& v8 S - MeLightSensor lightsensor_1(12);
: G" f7 W+ Q1 o; O4 p! p - MeLightSensor lightsensor_2(11);
2 A& \7 u' y9 m2 k1 c' G+ I - MeBuzzer buzzer;
. Q+ k$ P! f# x0 d" u - MeLineFollowerArray linefollower(PORT_6);
3 ?: [& ^: y, k W# I& Q - 4 x4 k9 ~5 N5 P0 u' h
- #define Error1 1% \/ U1 h; v6 T/ \8 D
- #define Error2 2
3 c' y. ~) _# I - #define Error3 3/ W ]: e O9 z3 g U
9 i! E5 v9 n# D- \6 p- #define Kp 15* U$ I; s4 f$ q. ]8 U
- #define Ki 0.15
' @( m! R3 P, }+ ?( E7 ^ - #define Kd 0.03: u7 [ W, ]7 |
/ ?; X6 f( n0 ~* x: k+ c3 M1 N/ o- uint8_t sData;7 Q9 ?/ q H$ X( w M( Z1 A
- uint8_t D1;& X R; ? F- C: H' g7 D; I
- uint8_t D2;
; Y4 }) ]2 ~- z+ T8 S7 i* e - uint8_t D3;6 s5 Y. P% d5 K; {
- uint8_t D4;
6 |& O, \" K* x - uint8_t D5;- x, Z- A" ~1 Z
- uint8_t D6;
0 N- k+ z: z0 p4 {' A0 w - - p5 E W* r! z4 |6 n3 }/ r5 G2 O
- float previous_error = 0;3 m4 M$ _( w) o
- float integral = 0;5 q+ N s0 q: t) f: R
- float derivative = 0;
& B7 y$ J! o5 m+ l! x! I9 t - int Speed = 160;
- ?! x$ G$ c9 f2 U; \8 l) L! r4 @' a - float output;2 e6 Q" m7 V7 x3 J1 F
4 [% o$ ~) w- H1 H: l. T- byte Left;
. @$ o6 [2 I" \$ v& K
$ `( S) _% @* I1 J4 a3 p/ i" J' B0 H- void setup() 3 @7 }" z5 ~! S. D. `9 U- y# x
- {
+ `' Y/ z3 L& Q+ u( @, h - //Set PWM 8KHz1 X/ k* ]( S9 m' Q
- TCCR1A = _BV(WGM10);
5 ?/ y7 u% o9 G9 \ - TCCR1B = _BV(CS11) | _BV(WGM12);
' [ F5 r( q( c! T# f5 i - TCCR2A = _BV(WGM21) | _BV(WGM20);
: G6 o/ {! k- F R - TCCR2B = _BV(CS21);
; l* A+ F9 m- A& k' M - Serial.begin(9600);
, h1 w$ I$ y C) M - buzzer.setpin(45);. ^% m! z2 Z% f0 V1 G; f/ z
- }) P p5 H, F! Y9 W' z0 ?9 l) _
- 9 w' Y) z! G* i& J' t# C
- void loop()6 y; x! `. p" ]: J& G2 }& D1 I8 N
- {
4 f# e2 Z; i* J4 @" K! X* H - while(!((lightsensor_1.read()) < (10)));
1 n+ r s( a" I - buzzer.tone(1047, 500);
+ e. ~8 z4 l& r) {2 C - delay(500);2 Z+ @1 v" j' d
- do( w% H3 Z5 I3 H' a
- {) H6 ?6 ^4 I8 C6 B+ G
- int Err = getErr();$ ?4 s' R3 _' s# u
- if(D1 == 1)
; k! a- G- u) L7 V - {
6 x6 }8 o$ }8 u - Left = 1;
$ r, }2 C0 H9 [9 @- C% J2 N2 {! e - }& F% e' l5 F2 `) ~$ f% R1 e
- if(D6 == 1)$ k! R7 h O! s
- {
; v: J0 E5 ]9 @& w# Q - Left = 0;3 H z' H4 ]" ~. ~2 u
- }# n/ H3 Y: q6 J$ \9 E( Y9 V
- if(Err == 99)9 Q; V( e' k j5 v5 v4 w+ |
- {
* t; J* v9 k, N) n% n - if(Left == 1)' W" C J( d' R& ^1 N2 A* z: ]( o
- {
6 X. \5 |* X& S - Speed -= 5;+ B' f9 X" Z5 d
- moto(0,Speed);5 M9 V; A! w2 c) g6 d4 D
- do& n3 o6 O! A. X7 r+ ]' ?! H
- {
7 E; _! w7 a! o - Err = getErr();
0 h* A. A0 I3 P, V" U9 u* @5 r( j8 } - }while((D1+D6) == 0);
8 T* v6 o: X8 R& k% { - }
7 O& |1 p6 {7 U( C1 S - else( w* s* a* j; a
- {! [- i* }% b2 ]( Y' a9 t
- Speed -= 5;
- b( T7 x" j& z C h - moto(Speed,0);
3 H! Y. w' G. c2 d& U9 U* _ - do9 T" v' a, h+ s! d/ T ]' m& u
- {4 C" G6 v% @. s2 A4 j6 b
- Err = getErr();
; G" A6 y# ]8 j) O! P) c - }while((D1+D6) == 0);
( r1 [# T# S' ]# @2 W, @2 P1 i1 } - }
" J# \, Z) x; d5 P' T& M - }' e7 P8 G D& O e# X* K3 Y
- else
4 {& r8 A+ M* y' h; v5 L" V - {% o5 O$ ~/ Z0 E, {' ~4 u. ]2 D$ P
- if((Speed < 160) & (Err < 2)) Speed+=1;
6 @. j' k2 x+ Y, g* i" ~' F2 k - if((Speed > 100) & (Err > 2)) Speed-=2; % P% g9 v7 f( r) v& x
- integral = integral + Err;/ X- ]7 j$ _! d( E
- derivative = Err - previous_error;
( q9 u# t2 K* N, K( k! n- J+ K0 T - output = Kp*Err + Ki*integral + Kd*derivative;& o6 }7 s* a: _7 c% t7 }& U0 L
- moto(int(Speed-output),int(Speed+output));% \+ N- s/ [1 l/ T2 Z" L
- previous_error = Err;' K! ~7 @1 a8 P4 F
- }8 N( o1 P8 [$ V
- }while(!((lightsensor_2.read()) < (10)));, @3 l, w" w+ M) G
- moto(0,0);
5 c: P" X% r( J& D0 W - delay(500);
5 i5 n( v7 E; V$ N8 B3 s2 r F0 I$ L - buzzer.tone(262, 500);
: K4 p& h- p3 H7 |4 ~1 f - }( g2 G7 Q% X/ L, A
- 4 V: D# X& {3 i
- int getErr()
$ O B; f2 W& `5 f6 P - { : v( n7 I( B" k5 H J
- sData = linefollower.getValue();
- D1 o1 w8 Z O- E% v* \ - D1 = ~(sData>>0)&1;
( Y$ c9 `4 I# d2 v; \! a - D2 = ~(sData>>1)&1;& V" C% n9 k& q( C9 Y
- D3 = ~(sData>>2)&1;
) c" @, U2 }8 ]* i. _ - D4 = ~(sData>>3)&1;
+ f* v/ a' M# l& v - D5 = ~(sData>>4)&1;
1 s% j5 Y! U' L" ` - D6 = ~(sData>>5)&1;
- O5 E# { ?" K0 u4 I# ? - int downD = D1+D2+D3+D4+D5+D6;
% k# f$ ?7 \0 g5 A' ] - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
* N& G( }: z+ t I7 k6 r' N4 m - if(downD == 0)
$ V9 w8 f2 ~ B( c6 N% c, B2 ` - {) \% a# C9 x. t
- return 99; K5 E3 Y7 P$ I% g- o
- }. |2 y4 X0 }/ p. D; M
- else8 `# }' S0 Z" x' ]: k
- {! T) N: x$ T( l. `1 n- x
- return upD/downD;0 _! S7 B1 b. R3 ^; i
- }
- u* P d0 M/ Y) N7 p4 V" V- { - }- [2 @6 Q- v9 C8 |
- 0 P" h6 S5 c% n" U
- void moto(int Speed_L,int Speed_R)
8 Y' c1 Y; _5 Y" I7 x3 J - {6 U% G( l' j+ k. c- ?
- Encoder_2.setMotorPwm(Speed_L);3 R$ ^- c& \2 Q
- Encoder_1.setMotorPwm(-Speed_R);
: \$ p2 h" b- ^6 s" S - }
複製代碼 4 ?: B" o% |% d: i1 q# r
MeLineFollowerArray.cpp
# V+ \2 v( B2 z: V a9 \- #include "MeLineFollowerArray.h"
+ U! ]: Y g9 A- ^: N - 0 l" f B/ ~/ K7 x- `$ @8 ]3 g
- #ifdef ME_PORT_DEFINED
+ h9 t) ^. I6 s+ R) g' v# B6 O - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0). Z6 S2 M D r+ c; H T
- {
. N/ o& _( C: e9 E+ s - 7 l3 G$ a2 @& v0 a2 @
- }
3 Y$ G: T1 [. I9 G/ |$ F - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
@8 ?# ~4 X: q$ N! ?3 S- }6 y - {2 }* _1 L- ~1 f: }$ I% u8 q
- _DataPin = mePort[port].s2;
5 k: w) w) ?6 ?. ~8 Z) F - pinMode(_DataPin, OUTPUT);
3 V4 c" b5 p! s* B' F; x - digitalWrite(_DataPin, HIGH);8 S0 @9 e0 O1 h
- }
1 W$ h: |0 Y! o# s - #else // ME_PORT_DEFINED! k7 d0 ~: y+ Z+ q6 n' b
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)1 T7 ~+ \2 G. x
- {+ K$ [1 \3 Z/ f! J
- _DataPin = pin;
( m+ v9 d- e2 l( W/ ~ I8 h2 I - pinMode(_DataPin, OUTPUT); - F2 E, z4 U. U1 c4 q
- digitalWrite(_DataPin, HIGH);3 b5 a+ k% D. W6 N8 d6 g4 t
- }
' ~, }% M$ A4 i; i0 p2 f3 G - #endif // ME_PORT_DEFINED8 y0 b/ X% I! Y8 G+ X
5 g4 f9 a& M! q; X- ' y& Q/ u5 k/ p
- void MeLineFollowerArray::setpin(uint8_t pin)1 Q& v5 x6 g- V3 k0 i
- {
; N8 l% Z: S* z: Y/ I - _DataPin = pin;
$ ~9 V# r3 {3 [8 d9 \ - pinMode(_DataPin, OUTPUT);
; J9 W$ v; ^3 F6 L8 m8 c8 o - digitalWrite(_DataPin, HIGH);
8 l; C3 v( _6 a6 H) C; N0 D1 p
5 N! X+ ]' l2 l$ A8 j; T9 K- #ifdef ME_PORT_DEFINED" i5 w1 ], S# S$ g, ?7 d ~
- s2 = pin;
: @: n; L* a7 a7 b - #endif
" A/ j) u; Y) B" ?/ l* E' {9 ]0 }) i& ? - }& b3 B; x) S" |
- # {& c! H9 J% ^( H
- uint8_t MeLineFollowerArray::getValue()* j+ x# s3 ?! b* _( E+ K2 e
- {
4 r- F% l# v, d2 e k3 f* f) B; B- Q - uint32_t LOW_level_read_time;
4 e: `% w6 ^1 z, @ - uint32_t HIGH_level_read_time;
! o& r9 Y* a; v( L6 h - uint32_t time_out_flag;
5 W" I$ ] l4 B4 T/ I5 `* M - uint8_t Sensor_Data[3];, d0 l" ~, K: `
- static uint8_t old_data = 0xff;2 `- [9 d' o- ^2 Z2 r
9 {7 v- w1 X$ i5 [" o& T, _+ N- pinMode(_DataPin, OUTPUT);5 [0 {2 G1 c4 ~2 u8 b0 r$ }
- digitalWrite(_DataPin, LOW);+ Y3 S) ^0 Y3 W1 Z! |
- delayMicroseconds(980);2 u8 `6 h/ f9 ^* v
- digitalWrite(_DataPin, HIGH);
* u! b7 d4 l' a I - 3 R" k6 K8 w9 G9 I$ U! k# E
- pinMode(_DataPin, INPUT_PULLUP);8 V) @6 u0 X1 J. Y
- delayMicroseconds(10);
}6 J9 e% \/ s: m4 k. M2 r/ X - / m* y1 ?2 d+ p
- time_out_flag = millis();3 ?! K* C4 E& \/ |& u8 ?! x5 ^
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( \6 V% r3 a- n5 W
- ( l& y8 U; Y: g, V: H0 t9 R
- LOW_level_read_time = micros();- d$ r8 J0 k8 @$ \
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& Q, f% g; g* h! j. J* W6 t q - {
" h4 O1 t7 \+ T z - return 0xff;
8 W# t2 ~( T7 W, N2 Z- G - }" h% P/ i- @( Q0 e" p" C
- ' M, F2 u+ A+ m4 {' A
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ e! C, l1 N- v2 M" H( x
- 3 l# t2 ~* G! B* f5 P v
- HIGH_level_read_time = micros();
9 |7 G7 l- O$ l' @, D" s% v" t - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level- t0 R' [) _1 m1 L; C1 e L% P0 V& J
! b$ F0 G) L U# v: J" X: }4 N- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 n1 b4 j2 e9 {+ O* ^6 f! R, Y/ B5 X+ m - {# z" E f9 P3 C @
- return 0xff;3 p8 s5 w8 o) C# [# `
- }& j! V% u# H+ b7 ^. u* K1 g0 A
9 b& x) u$ q P* w( c- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
. S3 ~$ J& e# R8 O - {# a6 D( g" [2 b) y& c
- return 0xff;& j, ~' ?" ?' `" V! K
- }
% `: O& L0 H! V' J! ~ - ) G L/ p3 K8 B' s* o: _
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 t9 Q6 u8 W% u9 T
- LOW_level_read_time = micros();6 X; r$ N) U r# p. `
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
( V) S0 p' n. e$ [
6 T* q# M/ M& e2 n t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% G% N2 c: v [' P6 L1 V1 |
- {
5 R3 [; w8 \3 Z - return 0xff;' ~- t+ m, F' t
- }
# s8 A6 t, z1 s! u( M% ^3 L - * @2 G; d3 _3 }3 b4 }& s# T9 F
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)): E3 R8 {% I' i" C
- {
+ G$ C# C# A- S! k# g- g - return 0xff;
' N* }" y" z" X, w% L - }
0 {2 f) A+ F3 T Z1 [! ^
& [ x6 a$ |. I, y- for(uint8_t k=0; k<3; k++)% w+ R1 _/ [/ e: @
- {' ?) Z2 N i0 o" K
- Sensor_Data[k] = 0x00;
/ v$ y( C' b$ ~0 a
' L. c! s& ~7 C- for(uint8_t i=0;i<8;i++)0 r5 l" ?, D4 x/ W& X
- {
. s8 q# U. a- M8 |$ q- F% s - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level0 T& i7 T4 w7 \" V0 h
- HIGH_level_read_time = micros();
# J9 ^0 q7 Q& G$ I - LOW_level_read_time = micros() - LOW_level_read_time;
) M' ?" r, {, N" E7 \0 \/ L
. H w) s1 F, A, w; w- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
. @' A' e9 _8 u/ N; j% f6 i* I% z/ b - {' Q/ W0 ?. s, J8 w) P
- return 0xff;$ b; P7 ], M% D6 b3 ]& g( d
- }
$ `5 m; Z2 a/ Z2 V. A+ Z* l/ j% ~ - ! d- s; I8 h1 j8 [
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( n% }' g% Z: B5 [2 v3 d5 @. ` - LOW_level_read_time = micros();
) F8 H. a2 B$ I - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
* i' d3 O! I1 S - 9 y# z, R- H* K" y3 Q
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
- I( n! y; F+ P. V1 ~2 S( T) Z( M - {6 t, f" O2 U" a9 q% F( A& z
- Sensor_Data[k] |= (0x80 >> i);) C2 l. w5 r& L& n% y! Y
- }% ^4 f/ K- a/ ~
- else if(HIGH_level_read_time >= 100)
- Q7 _" @# o- m+ q - {0 Z% p, ` Z+ J5 a
- return 0xff;
3 x: [1 n/ {% N7 U: K! z - }
, H `: N* \8 [7 e. \- a+ Z1 n; @
2 Q$ A- c. E2 q) ~- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)/ Q- P: Y+ ~6 q$ c4 M# W9 p
- {
. h Q( R u- c) c b. L9 b - return 0xff;
: N) p' r6 O; i/ l - }
; Q8 J8 i* k2 H. l$ i: q& e4 d - }
- P r+ }8 |# s: Z - }$ P& R. f3 b+ N2 ]' m0 Q( V
- 2 U4 ^4 D1 j( H
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 P, C5 a/ i* z
- HIGH_level_read_time = micros();9 m; [5 D! ~. v$ Q* `4 b- S
- LOW_level_read_time = micros() - LOW_level_read_time;
: k8 k7 B5 c9 [) m6 O/ y8 j8 u: {! E$ s - ( Q. i$ N4 y% s* g7 U
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )& M' Q/ H5 e, l4 v& O! J; w
- {/ a: ~ n$ h' R0 P' ]: b \# Y
- return 0xff;3 [7 f6 i: k4 w- R1 d
- }+ ^9 V# r( W/ l. p( U8 C
6 a( ~6 n4 R/ C) I2 ]- pinMode(_DataPin, OUTPUT);; c' A: }. n% l) Z2 @, }
- digitalWrite(_DataPin, HIGH);
8 p) B5 p- c+ R4 R5 {9 u - - U) k7 }( `3 x# T" E
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
% T6 T+ V9 q9 o! L" c7 U - {
0 V( Q9 s2 [! P; o - old_data = Sensor_Data[0];' | S" h# Q+ F) e/ @! C2 A
- return Sensor_Data[0];
2 |$ b1 r. n. f& w. r- L - }" g8 [# k# @ B, y/ R
- else
3 Z; r; F* G; A7 M( m: ] - {6 ~$ Z1 T5 x+ F0 ~9 g+ V- m: d
- return old_data;" r% {$ a9 G8 L) w8 f: @
- }
; C7 D3 U5 \4 H$ l4 X( R - }1 @* p; Z: r9 h' n4 N& P t1 P) I
複製代碼
6 y. V# ? {- z% v7 j* w+ nMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
( M2 [. A' n) p3 G& g* {9 [
9 D. i: D: r$ e# F |
|