|
|
/ }4 b) N% d) j# z7 b- T, O- #include <Arduino.h>
$ e5 p+ N$ |/ b' ? - #include <MeAuriga.h>% F; ^5 N" O. v$ S" L( n
- #include "MeLineFollowerArray.h"
! H1 m5 h; v/ r5 P& K( T
( s0 c- n# H# X& u7 o- MeEncoderOnBoard Encoder_1(SLOT1);, O# [/ Z+ Q! `4 w7 F; U
- MeEncoderOnBoard Encoder_2(SLOT2);# S( p/ O A# l! O
- MeLightSensor lightsensor_1(12);
2 w0 {# j c/ K( J3 D Y3 M! D1 M" v - MeLightSensor lightsensor_2(11);
9 c) n$ a" m+ \+ V+ e - MeBuzzer buzzer;8 b) M6 e! [( o8 K: Y
- MeLineFollowerArray linefollower(PORT_6);0 C' B6 S& N) s$ H8 j; k1 R& q! T
- * q7 @ M% Y d9 T; u9 K
- #define Error1 1
( F; d/ U v% Q - #define Error2 2
0 t8 B* X; z9 ~' N! C3 u( C! f - #define Error3 30 O3 P1 T9 s" \- M- ?
- 3 _( W2 z/ Z: `8 ~% v
- #define Kp 15
* g0 v$ l' S. @- I d' a, J - #define Ki 0.15' A3 }/ I+ e% b8 G9 P9 g
- #define Kd 0.03 M: N6 \ [) n) H. L1 h1 c# s# [. E
- 9 s" h; b J, l# j9 C9 x4 C0 f
- uint8_t sData;
$ g& D6 e6 k) i- P3 g0 F - uint8_t D1;# I# ^; W: A0 L' G( H' z
- uint8_t D2;
; B# ?% l# ~) o1 u3 W - uint8_t D3;! x& c4 I% S5 R1 l8 C9 y3 `( H3 ^
- uint8_t D4;
' o5 T% |# R- {$ P) I5 d) v - uint8_t D5;4 g& H9 [1 q1 E: J" [3 y
- uint8_t D6;+ H6 g8 ~3 a2 i2 F/ I6 u x
, Q6 x6 |/ A* }% N: u) Q8 g- float previous_error = 0;
: [: y9 y; {3 m- ^ - float integral = 0;( T& Z( O* a/ M8 Y
- float derivative = 0;
1 q; `) |, y. s( f4 {1 I8 @ - int Speed = 160;5 `5 j* q+ l0 \6 r( r% | m
- float output;
) u9 s* ~) S- A- a& c* L0 a: Z - ! O: s: v- [ ?6 s& L
- byte Left;$ A4 g7 T7 y/ [+ `7 F/ Z, e0 b% d
- . d! u5 N! T; a/ e; P* @
- void setup() & H! f5 Y- d7 S% i" ~ @5 ~
- {1 x) {) L. ^* E7 M: j+ [
- //Set PWM 8KHz5 l. ~. b6 Z6 A: Y9 s
- TCCR1A = _BV(WGM10);7 C8 h/ g |3 v: m8 \. @
- TCCR1B = _BV(CS11) | _BV(WGM12);; e% x* V9 S/ ^0 G! o
- TCCR2A = _BV(WGM21) | _BV(WGM20);! Z3 c& f1 P# q0 m w$ b: _. O4 u
- TCCR2B = _BV(CS21);$ Y. V. o; h4 _1 |
- Serial.begin(9600);- `" C7 e$ ]3 v4 A' E7 Y
- buzzer.setpin(45);. h6 D9 x u2 f, t7 N D
- }. K5 _9 N+ o( @( S7 @1 c
- % M' g. n+ {" x7 i
- void loop()
( Y0 c/ f7 q& q4 W# E$ l - {1 v3 V. b- ] l
- while(!((lightsensor_1.read()) < (10)));. H' q+ y/ L* S
- buzzer.tone(1047, 500);
2 q: I0 a. ^+ E |- Z - delay(500);
! G" ^, I6 m& d( Z; a. ^ - do% t; ? y; d1 k% l
- {
7 V) h# ^6 s. u$ H% Q/ H1 [; _ - int Err = getErr();
- I4 |! G" ?4 G' m6 ^2 C0 ^/ O - if(D1 == 1)! E" F4 t: k ?& e) m( N
- { K2 E5 J8 g/ Z7 N0 k, n
- Left = 1;5 s$ ?& h, _! a7 ]
- }" u6 H/ m5 [! O; z2 t ]+ n
- if(D6 == 1)% }2 q: G' g+ ?1 T' I
- {) V# Q; L3 }" N' z9 L# d
- Left = 0;
- P7 d5 o' [6 A9 m' d; ] - }% j% _6 J9 f9 F5 U# F9 Q \
- if(Err == 99). O/ x- a9 x% [. x
- {
& }- }. f" D% v - if(Left == 1)
. o. \# f4 K% I7 H7 h: X - {
v$ r- a P; C8 K: }3 b - Speed -= 5;4 y- @% L1 S, u) t: k
- moto(0,Speed);
1 G' K/ U# P0 g6 B6 c2 Z9 s7 l - do) _7 A1 A7 j! b* g( `
- { Z) w& ?! T+ K5 }4 _% b- k+ `
- Err = getErr();
; P* k6 b% F- [- _+ l4 E" K G - }while((D1+D6) == 0);( Y5 l @5 y$ j+ V: f8 }
- }/ k ^) m, B+ a' M/ Z5 `
- else
5 [& L; [/ P9 r. _# W - {8 Q I* c% a1 e! B
- Speed -= 5;
) q1 R0 P+ A, [4 C/ G - moto(Speed,0);
3 ^$ G0 q/ E* Z" h9 ~2 f - do% O# S4 _7 g6 B" L5 a8 U
- {
+ k5 o$ h0 U& O - Err = getErr();3 U" {8 l! N8 G* t
- }while((D1+D6) == 0);
8 }( z% `% @: X - }
& D1 G' x+ c1 M) H0 t - }8 G& A' W) E9 w4 A0 I
- else
+ v! g9 K0 _* N( m; p - {/ Y8 T9 r+ G! }4 l$ w9 D y
- if((Speed < 160) & (Err < 2)) Speed+=1;
+ s& X1 |; E5 S% p& ^: f4 @# N - if((Speed > 100) & (Err > 2)) Speed-=2;
" K4 R/ s+ f5 P* }) D - integral = integral + Err;
" r0 A4 }# L6 i1 t - derivative = Err - previous_error;
6 c \; W, @, D - output = Kp*Err + Ki*integral + Kd*derivative;; r: d9 D/ n7 V2 [8 s# J$ j
- moto(int(Speed-output),int(Speed+output));: O6 Q5 a; k! A4 f. @( @- V2 Y
- previous_error = Err;1 V5 J r& }/ J: Y
- }0 @# W; U, o+ T; o
- }while(!((lightsensor_2.read()) < (10)));
. y1 ~# K/ G. C E7 e' B - moto(0,0); `* R/ z1 ?2 ]
- delay(500);: m1 D3 n5 G. @- z
- buzzer.tone(262, 500);
% l& L1 Y. o: h1 r. k - }
# c* n5 W" f9 G8 p4 i; l
/ T7 z0 o) }+ r6 p8 @0 W8 @1 A1 l- int getErr()
5 B4 Z* q0 C: V3 i) i, W* l - { 8 y! r" y; ]% O
- sData = linefollower.getValue();; h+ N7 s! n: C8 Y& R* y
- D1 = ~(sData>>0)&1;
# t% b% W/ B6 p2 ?& x - D2 = ~(sData>>1)&1;9 r! f& [* q: z2 S! G
- D3 = ~(sData>>2)&1;# } B" w- h7 t* K
- D4 = ~(sData>>3)&1;
3 Y; q# y9 f9 ~1 S* W - D5 = ~(sData>>4)&1;6 x& R+ b+ W6 C# p5 s
- D6 = ~(sData>>5)&1;
7 m" W$ ~- w9 k/ N5 k' [+ z - int downD = D1+D2+D3+D4+D5+D6;
' S8 m+ w' Q4 {: v# p0 j" q - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
( G8 w( f( l# |$ f - if(downD == 0)
1 N; e& n9 Y) }9 L7 |, {. p1 [( ` - {
" o/ v" b* \2 j2 F J* P* O - return 99;7 H4 J( Z' f! F, G& E8 E+ U+ H; M
- }. R& e2 _0 A' ^) e* U6 H9 {2 p
- else: U' L1 d3 g9 \8 Y. I) ^
- {
6 `: P* e& K" m& c: c% Y/ B, ^ - return upD/downD;& m1 k% B. N6 n( @6 N
- }
% J; l1 {0 r, O3 l - }
& w- G$ U& J( b! q: a" r% O
* b, N, U6 P" t* P- void moto(int Speed_L,int Speed_R)
& m* h' A, l% M$ @2 {7 L5 ?! D - {0 W* ?7 j9 V4 r! Q9 E9 ^7 {7 R
- Encoder_2.setMotorPwm(Speed_L);
/ g8 O2 s. W$ t - Encoder_1.setMotorPwm(-Speed_R);* G' p7 D. @% p# ?8 e
- }
複製代碼 / l% C% W7 N" [
MeLineFollowerArray.cpp
1 R" h9 v: K, ^" Z4 t) N! W# ?- #include "MeLineFollowerArray.h"( W& T- m7 Z/ Z/ P4 l' e0 ~/ Z
- . e0 d5 w+ t- N# l2 ?
- #ifdef ME_PORT_DEFINED7 S, \: d2 Z- P
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)) A2 }2 L: P4 s$ `5 t/ f
- {2 A/ ~9 x1 j$ D8 d; g! ~
8 Z, e0 v; d8 S7 M2 c- }( a# L4 ~0 P# t
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
' x- ?" _( x5 T* `: y6 S - {
2 k9 A4 j$ v" ^5 { - _DataPin = mePort[port].s2;( `8 G5 {0 ]) n7 n. t, ]& R/ n& r1 [6 n
- pinMode(_DataPin, OUTPUT);
* l8 x) a9 F' h. j: S - digitalWrite(_DataPin, HIGH);/ }' z4 q9 j$ m9 N& j0 s3 J6 g
- }) R7 h6 J) \$ @& t) S) ~: Z1 U
- #else // ME_PORT_DEFINED
4 a: K3 c1 L2 e - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)+ E' Q. Y" q- P/ B
- {. A" _/ U ]# ~" @3 ?+ `
- _DataPin = pin;& V; L) ]0 Y4 `5 z; e7 L
- pinMode(_DataPin, OUTPUT);
# i4 t1 o s3 ]' L" w5 w* @ - digitalWrite(_DataPin, HIGH);2 ?5 G" t9 K. |: [7 {4 W4 B7 {3 ?# N
- }
1 Y0 N9 s' A: ^2 v, U - #endif // ME_PORT_DEFINED
3 d" _& _% F. Y! [6 r: Y U9 r7 s( P - ! o. J9 G) _- r/ U1 q! Y) K# T
- C( | ]( x) K8 O
- void MeLineFollowerArray::setpin(uint8_t pin)( @- l8 e- M' n' K% }7 |
- {
3 v% n& }" c4 C2 D4 v - _DataPin = pin;. l+ ^: u S2 B0 R# D4 d
- pinMode(_DataPin, OUTPUT); 5 [1 U' S( ^. v: C" _* Y! B0 \
- digitalWrite(_DataPin, HIGH);$ y1 H) ?9 X1 ^2 b0 C' G
: d( \ q6 d9 l. O( Q7 P- #ifdef ME_PORT_DEFINED" _" I9 b5 e# H3 g# G4 C8 o6 M4 L$ ^$ B
- s2 = pin;8 ~6 l5 u1 B- m; O* L) a+ {
- #endif( c6 {; \; l; y& p8 o
- }
: J. w7 j$ ~8 l, i - a2 M& P) o' x% F9 H
- uint8_t MeLineFollowerArray::getValue()
1 \$ N/ i P0 K: C H - {9 B5 e0 o" y4 O
- uint32_t LOW_level_read_time;
0 y* h! o" G/ J. f/ ^$ f1 v8 k: y - uint32_t HIGH_level_read_time;
2 [4 K- p8 Z, \3 l% N3 n0 B+ z - uint32_t time_out_flag;; @$ I0 c \9 p/ _/ |" I- |$ T
- uint8_t Sensor_Data[3];* A" e% r( i5 e( R" A% O1 ]) Y
- static uint8_t old_data = 0xff;
- v7 Z9 [2 ]8 [2 y, Z+ N
; n/ y" _* @0 B; j; }% X+ l3 [& A- pinMode(_DataPin, OUTPUT);
% B% l, B) Z* G6 O+ R$ i( v' d+ w - digitalWrite(_DataPin, LOW);
! V" ?: W8 @/ G - delayMicroseconds(980);: Y! t/ ?1 w K5 S
- digitalWrite(_DataPin, HIGH);
. Y: D! \4 c9 R
- S, X& `' \5 U2 l; @! v0 W- pinMode(_DataPin, INPUT_PULLUP);
* ~0 Q5 _' q6 @' `" @2 d8 p - delayMicroseconds(10);5 g; D7 A' v& c/ w% J
" C9 Z9 Q: ~' r" s9 K- g. u/ h- time_out_flag = millis();
, h/ y/ F* C6 ~+ k/ J - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) z! b( B5 J) M* C
~ G6 A$ [* A: o3 `/ w; x0 c; \- LOW_level_read_time = micros();
) o8 _2 ]2 r- S# }- M" Y; S, \/ Q+ f - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 r% _6 g0 J. I# [& N) h/ u7 z0 D
- {7 t/ f; f$ c1 \. |
- return 0xff;! j. V; G ~1 L1 S3 _
- }
8 v- P$ Q+ f- I( i5 \! L( x) P - 0 G4 d ~6 `6 g3 X( c0 m# T
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 F. |3 ^4 p: Z
6 v- a, z, a: p; S3 `2 U; P- HIGH_level_read_time = micros();
! }& p7 T! y0 ^% |3 Q - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level9 X- x f# G# k, a. o9 L. L* k, M
- 5 e. [: u3 [0 A! A8 H3 N' v; q& H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; I9 v! N" E" L' U' X# ^( ?
- {
- _7 U: Z8 J2 w" ?, M& \ - return 0xff;1 d/ }. D2 S) ]: p
- }
; p8 E; L* d/ g6 D, C! g - 0 q: T; s& ^0 m
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
, m6 I+ I0 P2 j# [* k. E8 D - {9 J$ |" S9 L$ u% f: T
- return 0xff;( z9 j! a$ R' |* G' m5 I
- }
/ @% @* t- p1 n) w) e. l3 [
" z# U2 o O+ Q( G% h- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" ?* ?! i T4 t9 t - LOW_level_read_time = micros();7 V4 i3 K; Z! x, ~
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level, k3 Q5 L+ x3 |$ A: T0 _
8 \' p! s/ n; l! [- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! ^) S% o1 @4 v2 T( ]9 d- g
- {! U: C* b* L/ f% z7 _
- return 0xff;. w- M$ D) r5 m: S% \
- }; i2 G' B& N& i0 {- e- Q
- 1 z. | |7 f5 _8 t. j' C6 ]! F D
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; N/ Q; Q6 u5 C# U! l - {
# o1 c. I- o/ f8 g& b/ | - return 0xff;& m3 k/ Z4 u. K* h, z. [
- }! ]9 T6 \3 ~" }; @- Y4 K \. c
- + o6 K) j' l+ |9 h+ h4 W
- for(uint8_t k=0; k<3; k++)
& s2 B( ^! u/ o; C/ l6 B8 | - {
3 D j. z4 }& d1 z - Sensor_Data[k] = 0x00;
~1 Y A1 o) K2 e9 Z
3 Y0 v) [) Q5 l' c- for(uint8_t i=0;i<8;i++)
5 j: o' J7 O. j* H* } - {
P5 t% f' b; v3 G - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" B) P0 f4 n$ @2 z; _5 X" Z* E: ~; a
- HIGH_level_read_time = micros();
" N* \" G6 [) s' |% z2 U: E! e - LOW_level_read_time = micros() - LOW_level_read_time;; P) w: r$ X9 S0 ~% P
/ |0 O# E1 w* ~9 d3 m% m l7 c) |' O- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
/ r/ A! P' ^0 |" R1 n& v! p - {! B9 E/ K, [$ T8 a5 z
- return 0xff;
' s8 _5 Z' l$ N9 S9 M$ o3 B, O4 n/ A - }: j" U5 U4 }5 J0 P8 N
- ' m( E* u$ V1 o, o& ?- D$ V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; g- i4 Z* s, O$ S" t; ?3 y - LOW_level_read_time = micros();$ T) ], M- |0 l8 X
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level- q* d% }* M9 t+ h
a/ L" i$ l2 z( e+ Q( F& d+ m- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
& P% @) A! P' G( \2 ~; y, n6 Y - {
) v) A5 t% S8 C( l" t T; o - Sensor_Data[k] |= (0x80 >> i);! I4 L. X' Z1 r; ^) f" e# A
- }
& t+ B9 P) s& O j' e1 q - else if(HIGH_level_read_time >= 100), ^0 W E+ E0 N# U6 |- p4 Q
- {- H4 \* z& G; M8 X; J& H8 {
- return 0xff;, M: s; m6 U) }% h/ Z1 H9 q5 s
- }
* q; T8 o' y/ U6 C+ A0 q: W# H, o% r
, _5 p8 j8 H' w& w; }6 ?4 M- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
: e* ]- Y# ^ H$ o - {
, m7 V: d9 q0 U3 Z C" z$ A - return 0xff;1 Q$ O: s3 ]) [- z# I" q; p
- }
% n- {# B* \( c - }
& Q# c- G9 }0 A - }
% K9 K6 {/ D. W4 m& L: g% w; X& b - ) U2 J: E* [5 v7 a$ C
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% f$ D) Q0 a7 i- ^' ^: k! P
- HIGH_level_read_time = micros();
! s9 U4 b3 }- W6 c! o - LOW_level_read_time = micros() - LOW_level_read_time;
. V4 H4 |* a% m% z/ K* Y
/ V# ^7 N, Q ?- a1 b- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )9 n% a, i. w- Q8 S( i0 Y
- {
% E4 l0 j/ ~: G3 C6 U( k" g - return 0xff;9 v/ a" i. U' W7 N5 y
- }2 D; a7 @& v: q+ @' T9 h, n
- 6 |0 E) C( Y7 d0 t
- pinMode(_DataPin, OUTPUT);$ h/ U& R H& e: {# `( q: M, \
- digitalWrite(_DataPin, HIGH);5 f( B' [" l% w
8 u1 }+ ?+ \$ f! e/ I; L- y- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
! ^7 d) m# K- a8 |/ C; [ - {! y4 T* t: ~! c
- old_data = Sensor_Data[0];
2 [2 E6 E8 C; B& K3 V - return Sensor_Data[0];/ P4 L G5 E8 @! L
- }
2 V4 V4 x: V2 g0 g+ R - else p+ q, M+ g4 ]0 z1 P' C# X6 l
- {& x, L: H- Y& F9 c+ B/ \) ^- R
- return old_data;
1 ?1 ?: x3 G6 T/ t4 p! Z - }7 r" {0 q% P+ i0 [9 D* p' I
- }) u; F, T6 p% g4 i
複製代碼
" q$ [+ ^' F+ e4 EMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, ?' J7 o( R% x T) ?! A' ?
: N' p" X. A3 {; r, B; q |
|