|
|
7 n& O$ D4 c/ b& \2 c2 Z- #include <Arduino.h>
: ?: e7 E6 x& U+ ^ - #include <MeAuriga.h>: U( E* } T2 B
- #include "MeLineFollowerArray.h"7 @* |5 M) e' N4 f/ E
) C6 y3 K. t( D) b- MeEncoderOnBoard Encoder_1(SLOT1);# G: I ^4 @- B( o
- MeEncoderOnBoard Encoder_2(SLOT2);
' ?" [; V& {: ~. U& L, n0 v* E - MeLightSensor lightsensor_1(12);
* F* }1 I6 [7 w& {8 [6 i7 j$ f5 i - MeLightSensor lightsensor_2(11);
8 p( U. ]' t* |' A) Z1 ^0 N - MeBuzzer buzzer;
9 t3 @. c* U D: \4 T, _ - MeLineFollowerArray linefollower(PORT_6);# Z2 F9 y" q! X q
: d, J/ o, z; E5 L! f; ?- #define Error1 14 v# P5 ]- w: o
- #define Error2 2! K' y p' ~* z* b" P6 o- e
- #define Error3 3
' I) Z4 `) A% E2 i - + @; q, y% N( s9 w
- #define Kp 15- L# N4 N: |/ m; D4 n6 [0 ^
- #define Ki 0.15
+ d( I7 c. G' j9 {: G: M9 U* [4 ]" O! n - #define Kd 0.03" @6 f. s* _& p, x C" a5 D
0 }" S( V) F! L$ F+ v- uint8_t sData;5 o I6 p! G, T
- uint8_t D1;4 }! `7 V+ H( p( y0 A
- uint8_t D2;* [# _1 n8 h3 r6 Q, Z( z7 T. E
- uint8_t D3;
+ g# R7 \. \+ }+ N7 X( V; u' p; t7 } - uint8_t D4;& `* S& v- o! o* |) R
- uint8_t D5;
4 z! x5 `6 q( s2 B" I' U* | - uint8_t D6;3 e/ K4 O" v4 x/ J
- # W6 L4 M3 [7 V" I2 U; R0 Y
- float previous_error = 0;, D5 o$ T' N, J% h8 V5 y
- float integral = 0;
+ R4 {; T! }7 p - float derivative = 0;
: e$ A: I9 ^8 b5 T4 M) [5 g - int Speed = 160;! w c( p z7 Q3 s
- float output;2 R4 y$ C# J% Y9 O
- ' @1 ^1 S* c" o! r2 H
- byte Left;
* N7 d3 o* e) H& R+ ~, H; J" q( A - % z/ t1 f1 J/ F9 Y
- void setup()
5 ^) ]- R3 ^2 v; U6 Z5 ` z - {+ K: A( t0 g( H9 q
- //Set PWM 8KHz' |/ o; O8 O$ K4 u! d* P' h
- TCCR1A = _BV(WGM10);; k5 N7 e1 b% B. X- t8 U
- TCCR1B = _BV(CS11) | _BV(WGM12);' ]+ D+ i/ X$ U+ ^
- TCCR2A = _BV(WGM21) | _BV(WGM20);
# `) K& _# }/ p2 q - TCCR2B = _BV(CS21);
+ c( e- n: ~3 e3 l - Serial.begin(9600);
: I4 W* [0 C) C# V - buzzer.setpin(45);
' u; j6 Y# h, M8 O8 _" P - }& Q$ W( u a( @5 t- C
- . R: `9 J( o0 ~; o9 G6 v1 q1 \) a
- void loop()
# T" l! m- F7 g+ v5 L2 ` - {6 t, L+ A6 x% a; q1 e
- while(!((lightsensor_1.read()) < (10)));
5 v A3 m. j d8 C - buzzer.tone(1047, 500); b, W$ \( A7 Q! q3 h8 \
- delay(500);! @# d" G0 P! U" K
- do) L0 H1 [9 _5 X" u& P Z, i: z4 b
- {
0 Y! l9 m: C! s# L4 W" T - int Err = getErr();( {4 G& J) S. ~$ d7 Q0 m1 S8 c) c
- if(D1 == 1)
" P( K: {7 n; [4 D9 ~5 F! b - {1 Y2 A5 m3 p/ W% y7 a) ~
- Left = 1;4 s' q# s* i1 i* h2 W5 B
- }
* ?+ s% G, `/ T/ a - if(D6 == 1)
" F8 V/ i% y6 h- @9 m- r; Q) F+ v+ } - {
( j) v/ }8 b) ^( S7 Y% ^ - Left = 0;
0 {8 } _" U5 D7 y4 d9 r! w- K3 H - }
/ F H( J. ?9 Q1 O/ B& F2 T - if(Err == 99)
- {) p% Q8 G( G6 i' o' X# d - {6 N4 }) H' B1 y" h+ G
- if(Left == 1)* y3 } F4 f f4 R! e3 }+ N
- {
/ P! [1 j! Q, w9 y - Speed -= 5;" v( A. e% ~; q. y
- moto(0,Speed);
( ], C8 e$ n1 f0 x - do$ H: W2 O. q. `. i) u
- {
4 N6 @" @2 f6 o: Z5 p) k - Err = getErr();
: |# P5 g. T1 G5 v. b4 [( T6 c% q - }while((D1+D6) == 0);" N. J7 e: o: i3 [. K2 G. H
- }
$ U3 a! J, q6 G( S6 g8 A. ] - else
* V) O" @' {' h: K' k: | - {1 C1 r7 w, u) R
- Speed -= 5;
/ X# h: b" s5 ^: _; a, o - moto(Speed,0); ! u( x/ o: p& t9 g% y) J3 p- q4 p
- do
! S% I- t! j" e& Q8 [( I. N - {
' o, }* p5 ^: |/ q - Err = getErr();
3 L' f' F& v% r7 o& |; b - }while((D1+D6) == 0);/ o: J, N4 o5 Z' Q/ g# T6 \
- }
4 Y+ ]2 N1 H* g - }
8 v( V9 y+ k4 e. Z, q- w, S - else
# p6 ^* i) R0 c7 R5 d! D - {
9 i4 _: K* A/ X) @% C- g* Z - if((Speed < 160) & (Err < 2)) Speed+=1;
& ~: }1 |4 t; E6 Z& V T7 d - if((Speed > 100) & (Err > 2)) Speed-=2; 2 o6 g$ f6 J5 K Z3 s0 l
- integral = integral + Err;9 ^4 M: i; R6 n) u. _' ~% ^
- derivative = Err - previous_error;
2 ]- }, `& ~( C7 T! S; E - output = Kp*Err + Ki*integral + Kd*derivative;. Z" v/ i( T! W+ f. N" n* T- `
- moto(int(Speed-output),int(Speed+output));2 n3 C: y. g/ n5 M) x# w& }
- previous_error = Err;
0 e1 I+ Q! O/ F! c: v - }
) y4 O, A& N1 S - }while(!((lightsensor_2.read()) < (10)));# K8 m5 e( t* t6 Z$ j6 ?7 h0 y
- moto(0,0);# j& Y& j. w/ |/ \/ U( q
- delay(500);
1 O/ ~1 E0 K+ T) j3 z) p p - buzzer.tone(262, 500);
- ^& N, a0 O0 H) w# p - }5 ?0 Z; l1 J7 j- l9 m
- 5 B3 I) M# y- p: X$ z/ Z0 m ]
- int getErr()
1 o& U d, `2 X; c - { # n$ l9 [9 F! W( A2 F* A( _$ U8 K
- sData = linefollower.getValue();3 e5 L' i4 [, R8 P5 @7 [/ X q
- D1 = ~(sData>>0)&1;
0 ^ f; B1 x: M4 x* k9 |: U- q0 n/ e8 b. U - D2 = ~(sData>>1)&1;7 l% P8 c# m1 W4 u1 V3 c
- D3 = ~(sData>>2)&1;) X1 U _0 w$ }/ S8 ^
- D4 = ~(sData>>3)&1;
+ m6 D4 k2 j2 U) k - D5 = ~(sData>>4)&1;& R7 |. }9 @( j4 a k/ k
- D6 = ~(sData>>5)&1;# B! C/ x+ B; X5 p. m' a! A
- int downD = D1+D2+D3+D4+D5+D6;
7 b" A; O/ J: E4 l! U! g - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
( j( t: G5 k; i$ [* N8 [" t5 r. u - if(downD == 0)
1 j5 W; C& ]* O. ^# I3 N- X. K7 ~ - {
6 i6 F7 c5 a3 o0 a6 |" K0 H - return 99; v/ }9 x* @! @
- }' ?0 }: Z# g* s3 p# s
- else0 w2 H, ]; c9 C* [
- {
9 s1 y5 L+ q6 J# e- { - return upD/downD;
. i/ R4 m: o+ i C- Y3 s - }
! u/ w( | G/ W; x+ C7 ?) i& n - }+ G7 I, o0 w l) D" |$ x, d) ?' Z4 ?
; C3 `' ?) L9 p2 V& f) I- void moto(int Speed_L,int Speed_R), V4 n0 B2 x$ d8 ]: U, Q) X* T
- {
+ u5 e" u. N) Z; R* w5 M - Encoder_2.setMotorPwm(Speed_L);
2 ` i5 ~7 x1 _$ m# S3 ]" { C - Encoder_1.setMotorPwm(-Speed_R);
$ \, O7 d6 v# c6 |6 M$ M7 `+ y - }
複製代碼
: y+ W* B& [1 p8 b) M* EMeLineFollowerArray.cpp
. T! U7 X" ^7 D) M! d: t- #include "MeLineFollowerArray.h"
7 `0 m, H0 [% o) X* W. [
~' }4 b8 u+ n$ [- #ifdef ME_PORT_DEFINED
" ? i' B w7 G- C" t& B4 r - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
3 G* `& W4 [ c t8 F - {
! \7 N1 H! E9 W# t: m - 1 B. n/ J. Y9 ?* a( H
- }; @0 X" @8 e! w7 c- H1 U
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
/ a4 l" J: V* t6 T4 k! X& O - {
& Q* f; g% O4 \. i, I1 ?; _ - _DataPin = mePort[port].s2;
, B- ^) B) B; v! D - pinMode(_DataPin, OUTPUT);
% `; v* {! ?" A" @9 P - digitalWrite(_DataPin, HIGH);
3 {: v$ Y7 Y, u. _1 ]# V* |/ f0 E. e - }" l. m$ u' f) V7 r1 l5 X
- #else // ME_PORT_DEFINED
7 p4 p+ W+ q$ Y: ?- h, I2 Y - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
0 h+ ?8 `5 k P: N! s/ K - {
8 |4 T S X7 k& D4 v4 f - _DataPin = pin;! C0 e# F% c+ e$ v: L
- pinMode(_DataPin, OUTPUT); _7 l5 f# Y, z+ ^' }% F
- digitalWrite(_DataPin, HIGH);
, A a; k: i* a3 O# a) I6 z - }
8 V, y2 a5 K- g# g- v" N - #endif // ME_PORT_DEFINED% H" F4 {* h+ E4 ?
/ G, _* L3 v1 _# h3 Q( T0 \3 [- $ C# C4 v* W1 X8 [3 u: d1 ]4 |" x
- void MeLineFollowerArray::setpin(uint8_t pin)
* p0 B$ c& K; S) A* D - {3 p6 s$ C; Q5 g3 `9 t9 X
- _DataPin = pin;
# n7 x3 B5 M# V3 r9 E& \* C* A - pinMode(_DataPin, OUTPUT);
8 K( D+ A( w5 |5 G4 l8 r6 z! @8 w - digitalWrite(_DataPin, HIGH);
; O7 Q( w- k1 E% |" l5 T6 }7 h [
( R+ W& S- R+ ? ^+ Y* f- #ifdef ME_PORT_DEFINED- p6 Z* A* @' b% u% r0 v P
- s2 = pin;
" C# y: E, s8 ^. d) ^0 f4 l: i3 G - #endif
" s( j# \) Q$ r0 p9 Z* m - }, Y3 N \1 A, b, @( Y/ i, |5 y
2 t; @5 d* J. b5 ^- uint8_t MeLineFollowerArray::getValue()
2 w' F/ k" \% z* J0 L( k, Y m - {3 g- e/ U+ }* M: p" e: l3 K
- uint32_t LOW_level_read_time;
$ P- I& o( C! e3 S O4 j - uint32_t HIGH_level_read_time;
! `8 i0 M9 j6 l. M8 V4 f - uint32_t time_out_flag;, v8 G# ]% {3 G- H
- uint8_t Sensor_Data[3];2 [8 [- t) Y+ }1 ~, x* z
- static uint8_t old_data = 0xff;4 I/ [; z/ S5 N0 ^; D3 p
( G, P5 W$ m$ y: U% |- pinMode(_DataPin, OUTPUT);
# X, o7 ]# ^' h+ j& g; j" L - digitalWrite(_DataPin, LOW);
0 ~1 H: ]6 U9 Z' o# _ - delayMicroseconds(980);
2 D& x* l+ n; t; n: } - digitalWrite(_DataPin, HIGH);% B& U9 w2 c* H
- G/ ~ `% b. O- K
- pinMode(_DataPin, INPUT_PULLUP);3 y S/ O# A6 b! x+ l2 L0 B
- delayMicroseconds(10);% {8 f9 ?* n+ P* j% U; t
/ I6 X& x7 H: [' o' D% i6 ?- time_out_flag = millis();7 B8 j4 z t' s( H, T
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' [- U) l, F& }- Q# ?4 p; \8 Q
( Y& Y3 [+ r. y- LOW_level_read_time = micros();) A/ B, Z, \" g# S
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; J4 G1 x3 q0 M* A
- {7 W) \8 p$ } H! L& K1 a
- return 0xff;' i. }' V2 k% s
- }% ?0 y6 \! p( X9 ]
- % E( A5 u" b; l0 l0 D+ w
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ K; U& Y+ v) a4 N/ D) P: {2 I - $ v, _( N2 ~7 e% t
- HIGH_level_read_time = micros();
6 q9 H3 L1 F' ?6 x$ u7 k) e1 @ - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level% D2 M9 d5 g* B/ W$ n/ l. C
- . ?8 X+ m/ u& J9 m4 ]! w: A- c
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 p" J3 Q' O* ? x7 n - {
9 X- w# c0 ?$ O* x! ^* y' C - return 0xff;
6 C2 N( i* {' a' z7 C- n+ _ - }% q) V7 c( B# X" F, E) v
- $ P- q3 e) t w" u
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))8 a% w9 \2 ?: u: ~* G$ }
- {
% G: g& h. o# L" w - return 0xff;. T. W% k+ \; {7 Q
- } Y$ x9 l' M1 I0 k9 z
- ) Q* ^. ]* P0 h6 N9 q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );5 E/ x2 p( O( ~5 y. D6 Y' `3 ^; b
- LOW_level_read_time = micros();7 `; p) W% C5 ~" d8 i
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
/ Q( Y9 n/ x2 W
% f7 n9 A9 S" R- p, L9 v6 }- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ G) g3 i! D+ U9 ~+ o# z# {, e
- {
U' }" {, i {* t+ a - return 0xff;. s; T) F/ F5 J7 \) e
- } o2 B) D- j! j% D6 x- c
- . b0 N& y2 H$ E+ `: a8 z9 Z
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))0 Z6 k& ]: |) `& W' G
- {
8 t. j* B0 m- P/ }. a1 ] - return 0xff;+ J8 w6 O$ B+ F: ~8 ~5 `
- }
+ Z1 V3 j- z* J7 u0 ]$ p - " c, U! R, X' m( T: ~
- for(uint8_t k=0; k<3; k++)6 A! @- Z& F0 A2 Y
- {6 H8 A3 B0 d. T
- Sensor_Data[k] = 0x00;2 b" q* F7 e6 r; G8 K
- 4 k) \0 w6 ? n! k) }
- for(uint8_t i=0;i<8;i++)
6 U; C _% Z& j' l' @) l1 r: ~ - {1 U6 Z' g/ P5 ?1 K. q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( Q" c3 U% q* s% X( s4 x6 q - HIGH_level_read_time = micros();
3 [* @1 h+ Y' J; g0 r1 v - LOW_level_read_time = micros() - LOW_level_read_time;5 m7 o5 ?1 @/ f1 r B
- ; v4 H8 `8 ~2 M' J. ~5 C+ c
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )* |7 E8 o* p8 c) q% t) a t
- {
8 R7 L2 a% C8 b- |/ f: F - return 0xff;4 u/ A5 v" V, n
- }2 O s3 \$ C( U8 b" \: x
/ A3 j% {2 c T- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 u; ]2 d0 T! ~6 j6 K% y9 `4 e( P% S - LOW_level_read_time = micros();
& s5 C- B& j, a. ]) T* w - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, M, ^$ S B4 ?6 H' [+ C - * j: a- Z& J: g- M
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1, J( V# i& \- ^" U+ A, J" d, ^
- {
7 l8 d. r, w( j2 S - Sensor_Data[k] |= (0x80 >> i);2 D5 x' D. ~* q
- }
; U. Q4 `8 i" |/ B' P( H - else if(HIGH_level_read_time >= 100)3 m6 B# N7 h3 b' j3 o
- {
" y2 d9 x+ O% J( C - return 0xff;3 G% r/ p) N; F' z
- }
Z' u4 \- } Y% o5 B5 B# z
4 k8 W' X* j; I( K- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): A/ } P; r. D) m4 z( q& \/ J5 X
- {
. F: } m h( \ - return 0xff;8 G( v5 E* {0 z) D$ O% R
- }( B4 ^, v* ^( t
- }
' `* S8 @- F" E6 E - }
1 F2 K( w5 g# I+ }% `. C - - x+ `: G0 c4 d
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 F: T5 n5 h7 J4 L) o$ `3 B - HIGH_level_read_time = micros();
/ ?8 u- l, Y9 j: H - LOW_level_read_time = micros() - LOW_level_read_time;
1 G6 v$ V$ R/ z2 x* c0 D* w - " O6 r% q% q6 T0 o
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )/ O" _2 a( d0 \
- {9 L; E9 {* E! {( p3 _7 y
- return 0xff;
4 Y# a4 O' N8 F - }% N4 w1 o# n2 u( d6 `7 Y8 N
- . K6 E1 ~6 j' ?* y8 o
- pinMode(_DataPin, OUTPUT);
$ f3 g% q9 w l. _7 V! D; O& ~3 u - digitalWrite(_DataPin, HIGH);
3 `* P" R: O6 R+ o' ?
* w+ Y' h ^# |3 r7 M- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
6 ]% n0 G4 {0 A T( X - {
; z+ v: Q! k+ f! B: U4 a O - old_data = Sensor_Data[0];& O9 F9 H8 R" ~2 W! {7 S v' F! P
- return Sensor_Data[0];
* V3 h1 m" [6 Y& w - }
6 S: B& e1 F( V% G# N5 K5 E( @ - else
& Y* W8 n h2 p, }) R3 h. C' F - {( N% b( b' g' ^2 m Y: J/ b2 M* B0 r
- return old_data;, W7 Z5 g8 z+ U$ Z; ~& s
- }/ D' P# R( x G+ W" E H5 W5 ?8 n& ?1 p
- } n! n% d5 i, `. R* [4 K
複製代碼 8 Y7 @% R9 t; d6 N9 s3 W0 }- I E
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
2 ~% t3 [* R2 V: J" m8 c$ D9 @, z
" e: X& L2 i. c* U% {( B3 l0 @" H |
|