|
|
* ~1 F) Q4 I" Y$ |- #include <Arduino.h>
1 F" A/ Y! {+ m$ o! b( q - #include <MeAuriga.h>1 x" A$ h; [/ F9 m! r0 c
- #include "MeLineFollowerArray.h"
) h: M6 I: i9 v$ y# V4 Z+ E
- J+ j' a( }$ O1 V3 O- MeEncoderOnBoard Encoder_1(SLOT1);
9 T ]5 L, ]9 H - MeEncoderOnBoard Encoder_2(SLOT2);
2 u+ Z7 n6 u$ l5 s+ y - MeLightSensor lightsensor_1(12);
3 i; q6 m9 a# M9 q - MeLightSensor lightsensor_2(11);# \3 M" j0 F4 i7 m* u. A
- MeBuzzer buzzer;
6 A6 _: t& N8 S( M9 O9 Y - MeLineFollowerArray linefollower(PORT_6);8 B1 Z0 L( p5 ~2 d# y
- 3 G+ e# D7 [) k& ]" L% \
- #define Error1 1( d* F( _# c% l6 b$ c v
- #define Error2 2, p3 v! d& q3 n9 H9 a
- #define Error3 3+ k0 h" Z) P0 R! H5 \
- ; J) m$ ^% n* o4 p0 L
- #define Kp 15; ~+ b# |2 v+ @! ~' Q' W
- #define Ki 0.15+ X1 }5 @9 }. B) c5 R/ S' N
- #define Kd 0.03
/ {, a- M) K3 X - ( [, D. K9 v! q. O
- uint8_t sData;
2 k- Z# x* O+ t+ d( f - uint8_t D1;
! s6 {: o9 G+ n6 } - uint8_t D2;* y: p' j" X. H, e6 C, F9 l1 r
- uint8_t D3;
4 w% L- f; B1 @7 O: E* g - uint8_t D4;1 R( z1 R o! A- e# B& D, v) c
- uint8_t D5;) r) I% s% u* w6 S
- uint8_t D6;! T( a) K' j3 k7 F' e
7 X g6 D+ W( s. B- float previous_error = 0;1 V0 {! `: m* d2 K9 s5 z" |0 A
- float integral = 0;) C/ N, R6 O6 {2 a
- float derivative = 0;/ n1 R# b, G2 a) a! p$ A) V
- int Speed = 160;0 S8 @0 W2 \& `+ P8 \
- float output;
$ {, a0 f8 _: Y2 Z - ! ~% D6 z- Z& b& s- c) l' D; @
- byte Left;& Z$ [# H$ ?: ]# k2 q6 ?1 ^. p# Q
- $ X1 O; d) Q* Q$ E
- void setup() 9 M N& `% b$ w, W
- {( G9 h; `, f6 @: \3 z7 R. @% }$ k
- //Set PWM 8KHz. U8 V6 }4 C2 U2 |2 f" c3 Z
- TCCR1A = _BV(WGM10);
' L" \+ ]/ l% B: m9 r6 r - TCCR1B = _BV(CS11) | _BV(WGM12);8 D9 |& y' r) i" r: m* ]- r% \- I
- TCCR2A = _BV(WGM21) | _BV(WGM20);
& ~" _# r7 |: H) L+ e, a - TCCR2B = _BV(CS21);
& C3 K f4 k V) R! e9 j - Serial.begin(9600);% d5 I% s" \8 q( {# N
- buzzer.setpin(45);+ ~* p( w* C) p/ D8 `
- }2 s8 R6 r" r7 o1 h
- - i3 q' Z0 x8 c# ~5 h! Y! W
- void loop()! U' W2 G/ N& ?, {/ Z* a
- {
7 j7 K9 t0 X0 q - while(!((lightsensor_1.read()) < (10)));9 r; V+ T0 G6 t, U5 C5 u v
- buzzer.tone(1047, 500);
9 t* {' I0 ^5 l0 Z - delay(500);) `. |' t/ b7 @+ j5 N. \
- do
7 |& `& D' S l& m7 V- n- u# C - {8 y/ y+ u" z8 D
- int Err = getErr();; h: ^$ B( t2 W0 p
- if(D1 == 1)
# J' _3 }& Y7 h6 h& H! a) c - { {% Y- Q! g8 c! V& }
- Left = 1;
( F. d, j+ a y' D - }
1 u) m5 i3 E+ L2 v8 o# H6 X; p - if(D6 == 1)& I6 @7 a. }* q* I
- {
; o6 ~. A o2 U7 [ - Left = 0;
' i. h% i" F9 f, V6 t; v - }5 D5 {5 D. x1 v' r
- if(Err == 99)3 o0 _0 g* k" b5 O0 G
- {6 F# n: U C# J
- if(Left == 1)
2 C b4 X' k3 F( b v3 D - {
) a" K3 T6 F5 b) y7 z5 X: G - Speed -= 5;
2 R$ v( R# Y7 d0 e - moto(0,Speed);$ ]( V) G3 I8 b$ s8 A3 q" L- A5 H
- do
2 k- A2 Z$ I) \& _ - {
0 B5 s A6 z6 u/ i' N6 @1 G - Err = getErr();
% Z( q, R4 h' z$ t+ |5 N - }while((D1+D6) == 0);
5 @1 @5 ]: v) o- u - }
b6 X, ~0 d& a - else! S% g- a" ]1 c
- {( T! e' ~. s+ e% G
- Speed -= 5;
( y/ C6 p) X7 q- D0 h+ F - moto(Speed,0); # e4 D3 X' g" B: H
- do8 [5 k. j; S5 o' Z
- {% g1 I+ Y# h$ u8 o6 m8 `
- Err = getErr();" i, D3 j- F8 E: I. {
- }while((D1+D6) == 0);8 G- G# W3 o4 B: X1 }' u7 b
- }
1 {( p% v# o: q - }
! p) X3 a, N5 r# i7 \& X - else
* l* ~, |& P8 T8 O7 I9 j - {
1 c2 @, E% O* a: m; c2 l7 } - if((Speed < 160) & (Err < 2)) Speed+=1; T' R. F7 j( q* E7 k
- if((Speed > 100) & (Err > 2)) Speed-=2;
" p- j9 f: P# ?6 u4 W# d/ R: n - integral = integral + Err;/ t3 L! I! J8 j' H
- derivative = Err - previous_error;
; u) v3 F8 ^7 i# l - output = Kp*Err + Ki*integral + Kd*derivative;
! r! p& `2 I5 U8 ]) I - moto(int(Speed-output),int(Speed+output));. \# e( d L* k
- previous_error = Err;6 @9 y6 k3 E ` \4 F
- }
9 F r: S6 B; t8 I# @7 P; C, @7 }& U - }while(!((lightsensor_2.read()) < (10)));0 ^8 q$ |% r* p' s
- moto(0,0);
3 p; `; m; I( b2 }6 C, s; m/ T - delay(500);8 b! q/ ^' V0 [4 @
- buzzer.tone(262, 500);
# M3 Z- w4 ^+ H- u" }' M! i - }0 W, W9 i* L- |+ d1 S I5 ?+ r
5 p/ h* M/ g) V- f- int getErr(); m# o; \; @' |- `% d1 E
- { ! n( \1 I: I; f! E3 t" I* A4 F/ V
- sData = linefollower.getValue();
- E9 ]' j3 o6 K: w/ W- ^ - D1 = ~(sData>>0)&1;
& D% f! g* O8 |- e - D2 = ~(sData>>1)&1;
6 ?# Q- I) |# M0 \7 w6 G4 p - D3 = ~(sData>>2)&1;
! }0 ]9 @5 }1 c9 c; w4 ] - D4 = ~(sData>>3)&1;
, [& f: k" s7 @ - D5 = ~(sData>>4)&1;4 a; T4 _, ~# j' [4 \8 ^
- D6 = ~(sData>>5)&1;" r) _& l) t. |( f( h+ y$ a
- int downD = D1+D2+D3+D4+D5+D6;9 m+ D; p" u, T; s
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
# R/ k* ]* A7 f1 x; p( H: C - if(downD == 0)
* h( z+ i4 V8 B2 v3 o0 f - {" S# I/ M; |8 h6 o% i
- return 99;
7 k8 ~( P8 Q$ s, ^, r# L# X! ~+ _ - }9 p3 k( ~, N9 j* y5 ], `8 A4 }; m
- else
* e3 U" j+ x9 |# X. O) _ L4 U* G - {
- |: ?4 Y+ I" C) B* y* t5 K - return upD/downD;& \- o( e% Q1 ]9 O) l& n9 |
- }# y" W% U! t1 a5 j* l3 B
- }- @: N' |5 Z9 Z6 J7 }* w
- 1 [' D7 ^2 F e# b/ J
- void moto(int Speed_L,int Speed_R)0 p- h$ V+ y, w! `, |
- {$ U' n3 R! n B; ]+ ~
- Encoder_2.setMotorPwm(Speed_L);
9 ~6 p& F& K/ C& z5 Q - Encoder_1.setMotorPwm(-Speed_R);. v D( N3 \, |4 h+ D. G/ |' F
- }
複製代碼 0 Z% U3 `6 x" J7 d; ~* M* K
MeLineFollowerArray.cpp% j) Y7 G6 K$ n# C8 K" ^% _0 ?) m: Z
- #include "MeLineFollowerArray.h"7 d4 P" |: t5 q! r( F4 o6 U
3 I1 _( N8 s9 e5 t* l2 T- #ifdef ME_PORT_DEFINED
3 C% z5 C0 V9 `3 V - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
( e# u) j) l; B+ z% q- E - {( G9 p* a7 R1 {
2 a* }* _. C* G+ W( ^- }
2 o4 U* |; d. s6 b% {3 Y( g - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port): n) W4 ^4 v) o* n
- {+ p9 T8 V% ~" A
- _DataPin = mePort[port].s2;0 Q/ ?' t, I9 M1 @ o
- pinMode(_DataPin, OUTPUT); 9 L2 i% t$ U& H4 E2 s
- digitalWrite(_DataPin, HIGH);/ o, q" v& g; \, f) }0 b a
- }, g- {4 A8 f1 W8 R2 k
- #else // ME_PORT_DEFINED* E& B' V9 u, ?% R5 O
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
0 c2 I' i& V" Z6 v - {* S- O4 } w1 |; i
- _DataPin = pin;9 {, f- O7 C' `9 c7 r, N
- pinMode(_DataPin, OUTPUT); % R+ {! I/ ~9 y, K, e
- digitalWrite(_DataPin, HIGH);3 ?- w/ U& r7 ^
- }8 X. C5 j4 V0 p( f
- #endif // ME_PORT_DEFINED
4 g, d+ U* l" O$ p& L - . K1 T+ Y8 v0 Y! y ~4 d4 z, j. M! J
$ l6 P, p& ^( C" o3 c+ l8 ]- void MeLineFollowerArray::setpin(uint8_t pin); X' w/ l' R2 G [3 K' c/ k; L
- {
5 i3 B' [8 n) l" }. {! E; |6 d: ^ - _DataPin = pin;+ c2 a8 l" A$ }8 Y; ~$ @8 }
- pinMode(_DataPin, OUTPUT); 5 c( G) T. u3 Y4 W4 `8 |. B0 B
- digitalWrite(_DataPin, HIGH); O9 i2 k. L9 \- |0 p# K+ d& Y
4 Y9 L6 O4 a1 `" j! Z2 R- #ifdef ME_PORT_DEFINED7 |7 Z- }% U+ G$ L5 e0 `
- s2 = pin;
% i3 _7 ^" g: Y4 @- G7 z - #endif% T+ {6 w4 \4 C/ F( A a
- } Y! `6 u3 \% ]& f+ w0 ]
- , U, ]1 J/ a# f$ ^4 y
- uint8_t MeLineFollowerArray::getValue()
, n4 c3 M( B6 h: o - {# d, T( k* k! Y3 O9 N
- uint32_t LOW_level_read_time;
. d& ?! w4 [( w+ z7 h' ?* G - uint32_t HIGH_level_read_time;0 P1 D( ]" s3 p& q
- uint32_t time_out_flag;: E7 T0 _# g h
- uint8_t Sensor_Data[3];- N4 d' ?/ C. ?% j4 ^. [
- static uint8_t old_data = 0xff;
) V" ?4 M0 ^- n/ B- ?
3 V1 g' e& f, C; n1 v- pinMode(_DataPin, OUTPUT);
+ Y. G; H) N* l/ s - digitalWrite(_DataPin, LOW);
. [" C% C/ w. h( F# \5 S" T; `/ e - delayMicroseconds(980);3 n% Q' o/ k' ?6 t% T. j% g
- digitalWrite(_DataPin, HIGH);( c9 p! l9 M. V7 D4 {2 _& P
. Y$ z" ^3 p7 U, v3 @$ b- pinMode(_DataPin, INPUT_PULLUP);
: A7 q S6 E# [& J7 F, z7 ~8 y' A, n - delayMicroseconds(10);
1 D2 o, i, S+ n8 e5 t/ T# e* ~
8 _( q4 C7 R% B1 g- time_out_flag = millis();3 J3 a# F" @9 a% y5 }7 O0 g1 n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) z1 G, [: K( J& E( b9 i
4 ^, T% n/ H3 {# L0 \ x- LOW_level_read_time = micros();
3 { y+ Q6 w0 f) c$ z - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out- w+ h% a- Q( b( b
- {; I! p- ]3 ]* ~$ i2 e3 J3 O2 k- }1 e
- return 0xff;
4 I4 o. }/ L* _ - }
: l8 P( g; _; F: O
2 i' H! s5 y6 K- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; N; f% v7 j; `$ X; y
- * I3 `3 A/ X2 ]$ `" V
- HIGH_level_read_time = micros();& ~4 L* f; r N
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
" x! M8 ]0 u# |* i/ u - ' \" a5 Y# n; v% U0 ?# J
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 \# n( `- v& }8 s7 @4 L
- {
" [, i' o; d2 z6 E6 v - return 0xff;( g! q) v: U4 K5 F
- }
# o- F3 P7 x3 t" e - ) g, e% j9 O! }2 H' i% k: k$ w* ^
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
; W. m& ?6 i+ S) n! a - { e3 e3 B7 }# @) y7 \7 \. ~. h3 S7 U
- return 0xff;' V- e2 C7 }* D( W: u) s5 y3 z" J% y
- }
$ o, X- Y3 ]/ C' K } - ! r: Y2 O. g: b) {: @
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 I5 O2 P2 J5 X
- LOW_level_read_time = micros();
! o/ `, ^! |+ \: u - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level$ V) m# M% z/ `. l2 N) J
5 x+ G G9 h/ H3 l- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 s% {$ c1 s; S+ R! O4 W; p
- {
0 ~( F! f: ~% i2 |$ o - return 0xff;
4 H, {0 X$ J3 a6 e% D( J& _ - }7 M, _8 ^+ M0 e0 g J
" p$ K# X& |! S- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
7 N3 x: M) t4 t+ z( v - {0 x/ n |" t' p" A7 x
- return 0xff;
! J, y" l: T* c% g+ Y9 B5 ?" h - }! A# F) [1 w7 h3 ]
- ! w$ I( F9 c, R& Y8 @9 f
- for(uint8_t k=0; k<3; k++)
3 ?0 w( l3 a) Y* H* d- X: U* m - {
7 M5 V7 h. U8 j/ g+ [( J - Sensor_Data[k] = 0x00;
9 S* r/ e0 W: M7 K4 p% Q - 7 N6 R4 W0 |# r7 v/ X1 W) ~
- for(uint8_t i=0;i<8;i++)
- A9 t% P5 |- O4 C2 h5 U - {
0 ~8 ^+ k6 K, j; R - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level1 Y5 x+ |! ]! ^9 |% n& V+ D. V+ A
- HIGH_level_read_time = micros();
/ M( t& X4 v: p, H) m6 U - LOW_level_read_time = micros() - LOW_level_read_time;( ? o2 N9 j% j# _7 O" Q
- / c( \# D' _( I9 [$ m% [8 b, B$ o4 e1 A
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 p6 i( c: f' v/ t- A - {
( v, M8 f3 R& S' ^# @1 m' K# j - return 0xff;& o4 Q# P8 E4 c; z4 K) A
- }
' d. f8 o. u& K% O, x
2 c' A7 _/ ~8 G7 k- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ y* x: m" H) Q0 A
- LOW_level_read_time = micros();/ K$ K) G. L2 k7 k! K
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level2 t9 d+ u+ O; T3 D
- 2 X2 K3 l4 Z' F$ q4 V5 E$ y
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1& O7 i* ^, D6 S7 E
- {! K9 c( `7 Q. z( V5 Z: Q1 {1 p
- Sensor_Data[k] |= (0x80 >> i);1 P: [( K( `: _( O5 y5 e
- }
3 ?* [ B" }7 S# v+ L1 }2 o; A - else if(HIGH_level_read_time >= 100)8 B# k$ H: b& n; O
- {
' ]6 }5 [2 u) ^! [9 @3 a - return 0xff;& z0 b, u) U5 D" k
- }
d+ S# \" F7 t3 r! ~
3 G( G& ?, F! I% w% Q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
: D" `$ f* v; X$ `! j - {5 X7 A) K& t2 S, a: j- J7 h( V _7 T
- return 0xff;
& u+ P; C8 E( ]6 E7 @ - }
0 P1 w# C0 U' M8 t4 S3 {9 B - }. d6 l$ G' }+ v
- }
/ L; V% C! z' F8 z
3 s+ W8 Q+ o5 Y2 a- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! f4 K3 \* I4 `9 v% R - HIGH_level_read_time = micros();9 y0 D) s( m! M& ^/ t
- LOW_level_read_time = micros() - LOW_level_read_time;6 b* o' b$ ]* j3 g6 \) J
- ) m6 s9 b+ w& h9 V/ t
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )* o; p8 \ c m; ~+ k
- {) h7 H, h' g9 _7 D
- return 0xff;0 g6 {' g7 J9 x5 j, N
- }- s* E. N, I7 ]) J
# O& w3 S2 k, h' E* o- pinMode(_DataPin, OUTPUT);
" T5 D$ I. c' t3 ~ - digitalWrite(_DataPin, HIGH);4 K9 C* f. @ ]+ u) W) n( y1 z0 H
- Y. |" x7 ?# a. e9 }# D8 @- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))! ~8 `4 o8 k8 c( H' S% |% z6 v1 B& Z+ l
- {& O7 n1 `, W. K7 X! y
- old_data = Sensor_Data[0];' Q+ m4 G2 N _2 ~. k& t/ [
- return Sensor_Data[0];7 Q2 D& v' x, b. a( [/ k
- }3 X9 c, z/ ]' _3 e2 x5 v! ^
- else
# M2 s/ g; F; V( G) _/ {; Y - {
; T/ U" x/ K* d/ c) K; m0 k - return old_data;7 O) O) @2 X2 [" _
- }, g/ L' \$ T6 g' U% k! x, D& f
- }
! _) W9 n* q7 h2 r7 }4 n
複製代碼 8 { l5 |: X! M) H
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
( H/ a* z- ?" ?, U! x' V; J* |# e. V
|
|