|
|
8 o( v9 l/ F5 c% ?! V7 y- #include <Arduino.h>, D8 I- s3 E1 g$ C9 @
- #include <MeAuriga.h>7 W# O H; M& _; R( w5 g2 S
- #include "MeLineFollowerArray.h"
) v$ s# G) v6 F* V" _% }6 P& L/ `
& o, I$ _& w* y7 d- MeEncoderOnBoard Encoder_1(SLOT1);; f% R7 S) k( Y
- MeEncoderOnBoard Encoder_2(SLOT2);) [8 Z' S0 J8 A8 e* w- _1 k
- MeLightSensor lightsensor_1(12);/ J0 @7 u% x: E% e) w
- MeLightSensor lightsensor_2(11);- f! C" a/ p% V% n5 z
- MeBuzzer buzzer;
0 O8 Z3 u4 m1 X - MeLineFollowerArray linefollower(PORT_6);* j5 W& J+ b6 n. ~: v/ t7 y) C9 ^
( x& S9 D+ B2 H: Z" B- #define Error1 1
% I! x" h8 {( x# m( ? - #define Error2 2
8 @4 g8 T! Z( X& C - #define Error3 37 K3 g% Y" n! k. ^7 @$ {
- 3 J& @: V2 A9 ^
- #define Kp 15
8 q1 x# ~4 x/ _9 `& n - #define Ki 0.15
4 S: b h G2 R0 z3 X& N' l4 u3 I - #define Kd 0.03. ^$ {% k, H5 \! h/ U2 }
- 1 E9 j7 L6 D q+ B0 w0 ~# _. x
- uint8_t sData; n6 j, r- p+ t" w* m
- uint8_t D1;
5 |7 K+ [$ y% j# c: |: ] - uint8_t D2;
3 K9 S0 H: Y) Y - uint8_t D3;
4 ~$ m9 d9 c# g1 ~6 |; S } - uint8_t D4;
" I9 U% R- n! |3 J - uint8_t D5;# X0 {+ b7 E n8 |4 E% V
- uint8_t D6;5 ?1 |. v: N' |7 x( I3 ]* E
0 _; u( O |; S8 l* P- float previous_error = 0;
3 i7 C, p6 E2 A1 v8 i! | - float integral = 0;
$ o" I! [& @; n+ g# K - float derivative = 0;
5 O: q. x) X, a8 o* q+ a - int Speed = 160;! F/ t, a1 q; \- W0 d
- float output;' U% Y) _2 W$ m2 o
! P$ V$ ~! k1 n, }- byte Left;
C6 s5 z" {# _2 ~; s
$ n% n$ o# T n1 n- void setup() ) S/ `" N+ i: v! t- ^; L
- {2 h: h; Q1 Q9 u. e5 B
- //Set PWM 8KHz- Z& S+ E* V5 v L% [) S
- TCCR1A = _BV(WGM10);
\. [- ] a; d7 R0 B# G - TCCR1B = _BV(CS11) | _BV(WGM12);! V) ]: ]' t7 J2 ~5 y# y
- TCCR2A = _BV(WGM21) | _BV(WGM20);
0 ~8 Y! z! V. f% h3 R% G2 M5 w - TCCR2B = _BV(CS21);3 x5 z; H$ ^5 k- m8 Y: |
- Serial.begin(9600);
! b1 K+ A9 N) F - buzzer.setpin(45);5 X A2 d+ H1 ?7 x: t' k
- }( Z, p; q/ k) C# p9 I4 ^) p
- : @. H( {4 ^5 W: c2 d! s/ {8 x
- void loop()
8 c$ M; x7 F$ V+ `* v4 C" N& k; d, c( X - {. X' x( O5 P; y5 H/ {; C* \1 M3 y' n0 N
- while(!((lightsensor_1.read()) < (10)));$ `% t3 f+ K: G$ l. U# M8 b ^" U
- buzzer.tone(1047, 500);! p/ A0 Y) E$ A6 o/ z
- delay(500);, z* G& O6 `$ I! p/ i8 E
- do
( T7 T+ M* T7 d" `1 T - {5 g" ~( I/ `, i- m* G; ]
- int Err = getErr();' b6 j( L4 H9 G; y3 u
- if(D1 == 1)
2 W t" `& M# i - {6 v0 K2 E' z' L& m4 V
- Left = 1;
! ~# Q" E6 g( p% d+ y - }
9 B1 L- S) j E# V4 y' J, P - if(D6 == 1)& `$ V1 r) P7 L. _# m5 o
- {
+ N! x/ `$ O2 p$ | - Left = 0;7 G+ \/ [) a* W+ m
- }% Y! r4 Q$ A) T, y
- if(Err == 99)$ Q1 a% Z2 U# @) L8 x! p8 L
- {' s: m- i9 e3 b" |
- if(Left == 1)" R: a+ `& s, w5 A7 i( E" U+ j
- {
4 @% w; d0 n, ? x8 J - Speed -= 5;
/ b, ^4 K5 W: [- b0 f3 F4 a - moto(0,Speed);- \9 _" [% r s7 \5 U o/ l1 @; T. X
- do
) N) B+ t- k9 ? - {
$ c, F, V7 n3 [4 c0 k, d/ h p1 t - Err = getErr();
* b6 o8 \5 O0 M6 J1 x( m5 d7 j - }while((D1+D6) == 0);
4 g. Q- g$ h6 _. y) ^2 }$ A( y - }; V. z% h, I4 L
- else
% ?* q0 m9 v0 U) X. C- _" t( ~5 G - {8 @6 A! Y5 C+ P& I4 c6 q
- Speed -= 5;
- Y/ b s# o* j) R/ { - moto(Speed,0);
( y" I9 Q+ S$ g- X' j/ g: B5 s& g - do1 Y6 x" a$ }! W3 m: U" u/ H. V
- {* h0 p( E8 I, K6 }3 N
- Err = getErr();! ~4 h! A1 F1 C4 [
- }while((D1+D6) == 0);
6 V* ~4 P# z) i4 q# f, C - }1 @. T+ U! {: ^. q/ x6 D
- }
1 H8 H2 K5 T0 @- c* g - else
- ]8 H; ]7 o0 V1 F - {) c& J1 d# k# C
- if((Speed < 160) & (Err < 2)) Speed+=1;
, A( q4 q4 L6 C- c - if((Speed > 100) & (Err > 2)) Speed-=2; 7 p# X- v. m5 Y/ W) L& ]
- integral = integral + Err;
8 x% Q3 T! z% k; Y$ X" W) s - derivative = Err - previous_error;
' p+ Z: N2 s+ n% k! A; C - output = Kp*Err + Ki*integral + Kd*derivative;
8 \$ m C3 R: E9 l) _ - moto(int(Speed-output),int(Speed+output));. C+ ~5 c7 H( I; F- |, c: b
- previous_error = Err;
9 G9 K8 l6 p- o8 n2 _6 u% \, Q - }
\; U2 O7 ~+ u - }while(!((lightsensor_2.read()) < (10)));
; T3 {" A S- F7 l0 u" ^, ?; i6 } - moto(0,0);* u, B1 c6 [4 W1 I+ ~' }* v# k
- delay(500);8 S5 S% d* `* }- ~/ I5 g* M
- buzzer.tone(262, 500);
9 t2 x1 z& ?3 r' _" Y L - }
6 w$ A' ]2 X# D: ^, _9 U @+ ~ - % c# @/ ~. D, A" v. O, i
- int getErr()
9 a# G0 }, }0 U! \$ N - { ) H2 V/ L' k S' l3 s' n
- sData = linefollower.getValue();, F) x3 r9 A/ B6 h) }8 [
- D1 = ~(sData>>0)&1;/ |/ z. y) S4 T0 }! Z# ~) s5 ^
- D2 = ~(sData>>1)&1;
0 ~& Y- Y, Q9 C+ Z0 X3 ~ - D3 = ~(sData>>2)&1;! E) b1 O/ V1 g' Y* }
- D4 = ~(sData>>3)&1;
1 F, P! F: l- Z1 B: I: e - D5 = ~(sData>>4)&1;
( G3 X: q8 I! F) l4 J - D6 = ~(sData>>5)&1;
4 |, Q! o0 q$ o0 @% \0 M" I$ k; \ - int downD = D1+D2+D3+D4+D5+D6;
7 L7 W% O5 @. J3 d - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);) _! `8 r& T A2 p0 _/ s, R+ [0 }
- if(downD == 0)
3 f1 L& k, w$ e. d7 A6 g - {
: z" D+ a0 Q& R( ~ - return 99;" F1 {' s6 _+ o' A( @9 o
- }9 c& k& B: y, T$ Z
- else
. t- \$ I( Q7 [2 v* e2 o% D - {4 e/ q) g9 b* F) `8 R& ?" V
- return upD/downD;
4 [" Y. }2 o) C2 \3 Q' v7 C - }3 B. g5 y3 G8 |" y
- }6 w ~1 E A, u' Y: a
- : V. f3 X& T2 h' l2 b1 ~1 l
- void moto(int Speed_L,int Speed_R)" J) x# [2 w+ b7 `
- {
( I7 ]) R" f+ z. E. }2 L @0 r - Encoder_2.setMotorPwm(Speed_L);
4 D1 n$ ?+ W# c/ |( y - Encoder_1.setMotorPwm(-Speed_R);
3 m8 c- K K* k$ Y8 C - }
複製代碼 2 |0 r2 ` z) K4 u
MeLineFollowerArray.cpp
+ F+ K) p; [; i' A! t- #include "MeLineFollowerArray.h"
7 }( c6 u5 h. C
- k/ |9 ~$ u2 o5 v" h% ?; ?- #ifdef ME_PORT_DEFINED6 ]7 G Q8 H3 i9 h' R, u( L H0 Y3 ]
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
' f& K1 c: w9 J - {, \ q' T9 x( ?; T4 K7 y
- 4 T+ Q4 n) u" p# _5 @
- }
7 ~& _; Y; Z7 O) s6 @ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)) ?' Y( f. o$ ?! [
- {. ^+ E" x% w2 _' C* s, I
- _DataPin = mePort[port].s2;
# i' [3 r% J, h. R9 P - pinMode(_DataPin, OUTPUT); : j/ j0 X0 f4 @ _% I
- digitalWrite(_DataPin, HIGH);
$ {9 p2 x. P0 [0 ?: o. X3 h! C" M - }
! t0 K: V6 |! `9 l' d+ f( i - #else // ME_PORT_DEFINED
9 | `8 x# Q$ _8 M- I - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)& o: b0 q# y, {* c8 {
- {
' c& }: ~' B% Y. ? - _DataPin = pin;
- b9 D, G3 ~+ G7 Y - pinMode(_DataPin, OUTPUT);
1 U% e) ]2 e1 q! C - digitalWrite(_DataPin, HIGH);+ r" B1 k' u9 }4 Y
- }# G5 u- Z( j5 ]1 M
- #endif // ME_PORT_DEFINED4 n7 Y( T, G V8 W: p$ A
7 O; r' `: F7 ~- 6 g* q4 e- {0 e. ^: U
- void MeLineFollowerArray::setpin(uint8_t pin)% Y" p, \! ?7 s$ o1 |, l
- {' i% }+ [- O! L: D2 f
- _DataPin = pin;# h: t; i8 z _: p
- pinMode(_DataPin, OUTPUT);
6 |( C8 J0 b3 s: \) V$ B- n, y: Z F - digitalWrite(_DataPin, HIGH);3 \9 P+ S' l6 {: M, W+ [+ k' G
- 0 v2 E7 I6 m8 O, L2 I8 s( f" y
- #ifdef ME_PORT_DEFINED8 m( x; `& \. b2 h' z! d
- s2 = pin;
. N, P6 y! m7 v9 N/ E3 l8 B9 f% n - #endif" I8 ?7 ]3 P( q" t0 j& m- V& O5 o9 K
- }
( f6 I z9 E% ^9 ^
$ Q2 i7 D q3 Z9 p- uint8_t MeLineFollowerArray::getValue()
8 q- e0 ?% e% {9 x; m2 z - {" U: q; F: h) K S- Q. H
- uint32_t LOW_level_read_time;
- f4 n$ ?9 X7 r - uint32_t HIGH_level_read_time;" p+ W# f$ y3 ~% K- d, H5 {
- uint32_t time_out_flag;
- E, `; A) _5 q N/ ^% p, ~ - uint8_t Sensor_Data[3];" l( D7 w+ v# r' q/ ?0 Z
- static uint8_t old_data = 0xff;
' Y' g9 c6 i: ]* c - / [ M5 J1 R! M, U0 J) Y( r/ T
- pinMode(_DataPin, OUTPUT);
4 u2 a" z6 N S" w3 A - digitalWrite(_DataPin, LOW);! n4 G9 h- U7 E! C |( o: ~
- delayMicroseconds(980);9 M( L- [8 B1 x
- digitalWrite(_DataPin, HIGH); O- r6 Y) n7 a9 x) W
- . ?6 ?% `3 L7 }# c& e8 c: Z
- pinMode(_DataPin, INPUT_PULLUP);0 `: Q3 I# P& z0 k0 f/ y! f
- delayMicroseconds(10);
; n/ S# Y% K' c$ L8 z
; [' K* k8 J; C- time_out_flag = millis();$ G- W. Y2 Q. c% _$ U* _+ W2 T0 q1 u, Q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* `! j4 ^3 o8 f$ G0 ~5 t
4 P: C% K4 t9 a) g, r3 E- LOW_level_read_time = micros();9 B: q5 p# {4 L, q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 ^* U4 e8 D9 a - {
$ R4 v* |+ `& _+ {# o9 I* u - return 0xff;
6 V5 ~2 d! p& `1 U% I - }2 b3 Y( g$ X4 Z" m0 e0 f
3 T8 ^9 k$ |7 O. D8 L/ H; F- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 X+ {/ I! E! @
- ! s- M* f; B* x+ U% A
- HIGH_level_read_time = micros();3 s/ [, y2 V9 G9 p
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level- p N7 S& D: `
- 5 L3 H1 Z: z- c" s0 G5 ^
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) B) |8 s, P2 a' p; c5 [
- {
1 r$ ]$ l( `% L/ t9 j7 l) Y0 P - return 0xff;
! l; h) B! F+ U- L - }
' F8 \2 A+ c z, z - 5 [ U# X' u/ _& }! U; ?8 Y+ z
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))3 W3 T I. q: E* A% O, O
- {
5 R. `. b! d2 Y; y9 G7 R - return 0xff;9 \* U9 _" s8 ]- j$ ~/ U R- W G
- }9 d4 s A/ k \# j
+ o: X: z2 v* A' \7 K- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 |0 i8 P& ~" B& g3 ?# y - LOW_level_read_time = micros();. N/ \5 j9 L0 k0 e
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level8 s4 C/ Q/ O; Y5 J% X' o* V0 W
- 9 t0 N, {: v6 b6 P. g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& J9 d4 }( Y/ ? i
- {7 ^2 Q3 e. ^; i6 `9 \( n, h
- return 0xff;
4 f7 _& v5 G, m7 H - }
# U8 `4 e1 K' v1 F - # }9 S- P( b% @0 N. I2 q1 D
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
. i6 E% X4 t5 ]& z - {
: ^ Z' z/ M* @4 n- b# X9 _ - return 0xff;
: P5 |9 M$ | B9 @' G7 ] - }
, r$ u3 h4 U$ u! z - , d6 R+ T. ^. `* j# j' l
- for(uint8_t k=0; k<3; k++)
; ~: C% w3 Y6 w5 [ - {
" \8 R7 O, a! Z& j& u - Sensor_Data[k] = 0x00;5 h& G: p4 O' T3 g/ M
- 5 Z0 w) n4 z% {1 l: f s/ q) \
- for(uint8_t i=0;i<8;i++)' b1 Q5 [% W3 J/ E) E& s- F
- {
: ]3 O( p0 t- F1 ]. r: V - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" ~1 B5 f* k8 C& j
- HIGH_level_read_time = micros();0 f7 p8 Y1 k/ l: ^- Z# g
- LOW_level_read_time = micros() - LOW_level_read_time;( X- S# |* Z3 D& R0 U
- ' V; Y0 l# ]! f9 W
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
1 I2 W7 O7 R% a* B- m. ` - {% ]- n. u- a: f- q8 a
- return 0xff;
3 }6 z o" H9 U* x1 ~ - }
' N+ I2 ]1 ^ R O
+ u8 n7 o# Z8 E+ D- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& ?& B" @9 C: h1 g# g2 @; X1 z
- LOW_level_read_time = micros();- V, _% K: V1 d9 l" Q5 M, b4 o3 v
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
6 h2 S+ L7 M- X N( X$ N
$ Z* H0 r: i& G0 L- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 19 }4 }! ~; d+ L+ F# g) n) e
- {3 V$ ]" l" n- M2 R; A$ Y) M
- Sensor_Data[k] |= (0x80 >> i);6 {6 _2 v; i1 ~! a' M& J5 X, W
- }$ {: P. j% t- m7 H8 k1 D. r1 \
- else if(HIGH_level_read_time >= 100)
! Y. ^1 q( o$ s" X - {8 S; X& M ]6 g7 D. [
- return 0xff;! ^+ R0 N: _$ u
- }
' s2 Z) ?- @ J: k3 m- p' ~# B - 6 {* E* |% V! @, X( X
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)% i, v' h9 v: i2 q( R) }
- {
8 D" u5 Y& Q/ r/ O4 Q - return 0xff;
* K3 \5 y" _& x& P - }
5 I( l5 p; _ v - }
! L2 d( L* p- Y5 \9 s - }
& c6 W; I j1 D9 |' h0 Y( B0 h
3 w. j6 L# }0 V s: e; W! g8 }- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level2 L6 n& [# o0 V% Z' Z0 Z1 `
- HIGH_level_read_time = micros();
0 y: D' H9 |( j2 i; q2 X - LOW_level_read_time = micros() - LOW_level_read_time;
}' C- c2 M$ @" m2 `2 o5 D; E5 u1 D - 5 @; u% `1 o! e7 M. ]; a
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' ?" U# L2 O- c9 _7 {1 m - {
% p" F! H0 C/ p* b4 `8 R9 t - return 0xff;+ S3 ]9 ~7 R' s
- }. w$ ~" D% C8 |( o+ F9 U: L. ?
- 4 [( \3 C% m' ^1 r7 |
- pinMode(_DataPin, OUTPUT);
- ^2 c' T" [) u% O, \& F - digitalWrite(_DataPin, HIGH);
" L$ h5 E+ Q% @/ b - * h" s. @4 m3 \( `8 G( ^& O
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
0 s/ G" I( h& K& R - {
, o; l$ ~( n1 M6 j1 m - old_data = Sensor_Data[0];* ~/ n1 z8 w( ]% e4 }7 A8 ~
- return Sensor_Data[0];
* f; Q6 G6 m% L1 I; J - }
6 ~$ A6 v0 u; B$ _7 M v - else
9 A' M0 F: {; g1 B& r1 _' E7 U( W - {4 i3 |+ ^7 s+ ~) ]
- return old_data;
! J" Y; g' u" J& N5 E+ s! A( I - }
7 x: V$ ^) U l0 V% ?0 x - }
, B7 C8 p7 E+ k0 k$ ~- T( h
複製代碼
/ Y" M+ X( _/ ]' L; sMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 h0 _8 D G' o! }9 U) s
! n# V( i( x5 T& U: T) {6 _1 j |
|