|
|
* M2 o& [, p) |1 t9 D# Z- #include <Arduino.h>, w5 i8 k' ]7 p$ @0 R
- #include <MeAuriga.h>) a! O" A7 [: C( b- B5 Q& [
- #include "MeLineFollowerArray.h"
1 r8 U% n5 T3 g, J% P% w; c! e
. Z4 G: ?0 r: J9 Y: ~+ E/ Y! l- MeEncoderOnBoard Encoder_1(SLOT1);
1 S3 A+ M# {( X - MeEncoderOnBoard Encoder_2(SLOT2);5 {' n2 j7 f$ \" V n3 E( f
- MeLightSensor lightsensor_1(12);
* w! B0 n; q: k! I7 V. r+ a - MeLightSensor lightsensor_2(11);
6 S0 p# o# X6 r- Q - MeBuzzer buzzer;- a. T7 }9 A% m' p
- MeLineFollowerArray linefollower(PORT_6); V. R/ D) T' r5 y, A
# I4 x8 ?, b, ]' w4 P; d, p2 a- #define Error1 1& o+ q- F6 H- R- p, C8 ?
- #define Error2 26 }! @9 O; X9 X( F; }- `7 A* r
- #define Error3 31 T' o; c" J4 w+ R! C2 ?
8 Q B: u' l8 E0 ` t2 H- #define Kp 15% q2 m5 q( \1 e' d2 N, L- n
- #define Ki 0.15
" u* Y0 N" q; [! f: C - #define Kd 0.03+ {+ A5 f% `* ~7 K( k; T
- - I+ Z# d# \( A2 m W& d& z0 q
- uint8_t sData;
$ o7 k4 Q; R5 X: y4 E: @ - uint8_t D1;
1 Q' ~; J( W: f8 V* d - uint8_t D2;1 h+ p9 y" Z+ H' i
- uint8_t D3;
4 u+ N6 I1 c) b - uint8_t D4;' m+ T) R3 M r; H
- uint8_t D5;
: ]: G- O' ^6 B2 J* C) d1 Y1 K, m - uint8_t D6;1 y2 Q% f. _4 s; M* h" P0 R9 [6 I- v
- # O4 C* R7 L, L
- float previous_error = 0;1 s' I# i6 O( B; _- g: S
- float integral = 0; H6 D i2 l& A5 X
- float derivative = 0;; b0 ^! `* _$ I. }6 \: u. y+ g
- int Speed = 160;
9 h: p; _! _( X' b) Y+ a - float output;' n4 ]. G7 e1 `! y# l+ I8 i
- : S# U5 l D, F9 ~7 J7 B l+ F( I
- byte Left;
2 j, {! k: L5 ~5 e- y - & _$ G+ S/ {5 `/ J' [" d5 s$ s0 p" h, {
- void setup() ! {- ?) ^1 @. K& g( T. @1 O8 ~# Y; h) F
- {
F* K. C5 l% t" F - //Set PWM 8KHz
' m; Q6 o* o2 j: ` - TCCR1A = _BV(WGM10);
1 x! s- L. r, ? - TCCR1B = _BV(CS11) | _BV(WGM12);
f$ v! {: B: F3 u0 T5 u - TCCR2A = _BV(WGM21) | _BV(WGM20);, G* p0 r- G) C* K; b
- TCCR2B = _BV(CS21);+ M6 q# }+ e! [$ x4 l4 `/ I3 x. e7 k
- Serial.begin(9600);) G3 c& r4 _% q5 I5 ~) \" z" Z
- buzzer.setpin(45);
: F; Z9 P0 K o* u8 k - }
3 l+ Y- r5 U! |( Y9 V/ c
1 y) H0 `# h: ^1 [6 m: W- void loop()- L& C2 i+ g* ^& h0 l9 l4 K
- {
4 l4 y' K$ B- }" Y! s# r - while(!((lightsensor_1.read()) < (10)));9 j. i0 B/ t L, P; `$ f' ]
- buzzer.tone(1047, 500);$ j( g$ L7 f" o: ^/ K1 [
- delay(500);3 ~; r- F n& Q
- do
9 `9 o6 g% Y9 x: e/ I& p2 } - {
0 z7 X, [$ [" q1 u' m. K0 c- k - int Err = getErr();2 W$ T6 ?3 o, y( o, t2 O% I
- if(D1 == 1)+ [" L% `5 c2 r! a! t9 D/ a% K
- {
, u. t" p6 v& P% `/ i/ t - Left = 1;
F- `( Y& M& V/ w" W- T - } x$ r% P1 u! J) j) x7 `
- if(D6 == 1), ?) f4 \3 i3 s$ ^3 \
- {
2 V* u- ?, ?8 d9 @5 w; g2 g9 G - Left = 0;
/ P1 l" |. V6 F- q - }; ~7 `0 Z6 {# E& ?4 G
- if(Err == 99)# H$ T2 Z- ]% L8 J/ q1 B0 w* q
- {
?. p: q5 k1 c - if(Left == 1)
% Y# U. J3 ?. }7 ~ \5 V - {
- @! Z) H- n7 L; ~ - Speed -= 5;
% A4 Y- c d( A/ k - moto(0,Speed);
& u! |4 W2 `- S - do# p6 |* {: d0 l3 z! L
- {* J; i! Y( O) T9 I1 X
- Err = getErr();* y. _2 ?* D. [! b7 V+ C! v* [
- }while((D1+D6) == 0);
8 y7 w/ A/ k/ k7 S - }
% C4 S6 a, j8 j* M4 u" [8 V* A I/ P - else! B4 E" |) b# M% }/ ~& H
- {# s0 S5 ]! m6 |, [6 w }+ ^
- Speed -= 5;) H3 @1 a5 Z2 X3 m
- moto(Speed,0);
* @7 ~4 ~3 t% q3 ]9 h6 H3 v) b - do
) g$ Z( j5 c- a# I - {
8 i- H& |# J+ \& u, F# d8 }0 H - Err = getErr();7 O' `9 k/ t+ V* d+ z% N4 ]
- }while((D1+D6) == 0);
# Q! [& O9 u0 {0 p - }4 K% |: g# v3 G6 T; E) @
- }) s8 s; K& L3 a M2 i2 q; j% a
- else
' e! \( n; e& [( p0 q/ w' ? - {
' z& S" Z, u! a$ m. ~( {/ @8 G( Z - if((Speed < 160) & (Err < 2)) Speed+=1; 9 ~7 ]! I0 g4 U6 a& v
- if((Speed > 100) & (Err > 2)) Speed-=2;
5 u% y3 @0 @) n2 e - integral = integral + Err;+ z# J+ D+ M- Y" @, F
- derivative = Err - previous_error;
8 D+ d: H' ^) B - output = Kp*Err + Ki*integral + Kd*derivative;+ s& F: p, x+ c6 l9 ^9 ^; X
- moto(int(Speed-output),int(Speed+output));
, R' G2 K' G; F# l6 o - previous_error = Err;- j/ F2 J. z6 t! `( t, _+ h6 t+ f
- }- g0 q4 M, |. H/ Z! g x* C
- }while(!((lightsensor_2.read()) < (10)));# u/ k. j8 S' {' U P* N) L7 \, v
- moto(0,0);
5 d7 v, G, H# Z- _1 \- z4 \& A( s, A - delay(500);
; ?) ?6 Z6 @$ |/ l0 v - buzzer.tone(262, 500);# J9 K5 l3 `3 Z& ?- C+ k8 n, c$ J, ]
- }+ @- u! a% c# s6 d3 F. `4 o6 H
- " R/ c/ J0 ?/ P! e' a% ~
- int getErr()
' l- q( w) T) T& ] - {
; z" T" E5 |# P6 E8 I - sData = linefollower.getValue();
+ o+ E2 f0 @4 m - D1 = ~(sData>>0)&1;
) D+ W. r. P* J7 c; d+ e5 D4 b - D2 = ~(sData>>1)&1;6 y7 O( D( `1 V' \. N$ w
- D3 = ~(sData>>2)&1;: M" x' g: s" ^5 k7 F) N0 _
- D4 = ~(sData>>3)&1;+ p& d. g4 B7 n6 @7 D2 ]. {
- D5 = ~(sData>>4)&1;
9 e9 j0 M7 L# L' Q3 y$ } - D6 = ~(sData>>5)&1;$ c% ^; C' a& C: z
- int downD = D1+D2+D3+D4+D5+D6;
6 w9 y4 P, R; [! d/ w& A/ c - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
; \" O7 y: K/ o - if(downD == 0). l$ d) m- j% A# E) V& ?7 |' V
- {
2 N8 [( z) I+ ~5 O - return 99;- G* \5 W9 E) R: @# O* E
- }. @ c# P- u. {) ?) h1 u
- else
6 l0 F/ k' b* e2 |4 J( b# F - {
+ Q8 n1 X: |* `: T7 B3 t - return upD/downD;
- `( O6 G5 V% D - }& C2 s6 a) D3 Q9 X6 v# _6 r! D
- }
+ N8 F* E5 R$ a5 b5 a - 6 A* j6 ^2 n4 e! Z2 W
- void moto(int Speed_L,int Speed_R)) I1 ^9 V z- R0 D7 {5 C) E3 T
- {
! {0 U- ?6 X% X3 o8 `7 D. m' ?3 u! f - Encoder_2.setMotorPwm(Speed_L);
& t: }1 n6 d) u5 _" B" I" Z6 W - Encoder_1.setMotorPwm(-Speed_R);
4 W' x" i: s; X6 h9 g7 `3 m - }
複製代碼 ) r4 d2 ^& L$ p- p* p0 h d
MeLineFollowerArray.cpp* ~# X6 {6 W, o- u$ {
- #include "MeLineFollowerArray.h", Z' R' N1 ^2 @$ r
- " L( Z: [3 C) o, g3 [
- #ifdef ME_PORT_DEFINED3 L" _+ _! p0 y! N' @) p- S4 I
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0), @9 z$ P3 b! `! j8 G3 V; F* O
- {
; ]: y- O1 d0 x9 q$ E, h+ o
# v. t/ {# f. g8 U& \0 s. _, y- }
* M8 ` l: H' p' ? E - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
! G* N8 u+ t. ]3 ~ - {
, O% K5 \5 T) d! g! l, d" D - _DataPin = mePort[port].s2;, |: H! I! n% ^, O1 q
- pinMode(_DataPin, OUTPUT); ) e, f- k A; n# ?4 Q+ G
- digitalWrite(_DataPin, HIGH);& ~% P, H% w2 ` _: \
- }5 {8 E" E4 W$ |% t% s' B! M
- #else // ME_PORT_DEFINED; S& m3 _6 a( b* @. t
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)4 x# w7 Q1 P9 p# X
- {
# T# G( p+ N; {2 ] - _DataPin = pin;+ J5 @) i' \$ k: R5 @
- pinMode(_DataPin, OUTPUT);
; |0 l L7 z/ l; | - digitalWrite(_DataPin, HIGH);
6 v/ Z6 [& d5 _& j6 N - }7 T0 G' ^2 l8 z1 \9 r+ ^. U
- #endif // ME_PORT_DEFINED# b( U: n5 @/ b g5 S- P
9 x, h4 O; ?8 l% b1 u# F9 f% x
$ I: C, }- }& O) C- void MeLineFollowerArray::setpin(uint8_t pin)
2 c7 g3 K: b" i - {
* [" P% x7 f8 U: s" K - _DataPin = pin;
) k- f3 s I# Y7 P/ }0 c+ ] - pinMode(_DataPin, OUTPUT); 8 ^3 b) U8 M9 T8 L
- digitalWrite(_DataPin, HIGH);: D2 E) f2 n, E3 x! ~7 o* H
/ Z) L3 N" N, J- W9 d: C0 a- #ifdef ME_PORT_DEFINED
6 W& \* i/ X) J, r/ f - s2 = pin;3 v& E ~3 u' ~" M, e
- #endif& v; A; k7 a0 Q- W; c. S1 `/ y
- }
; m* h$ F& g% u5 \2 u+ E6 K" o+ P
! {; e B1 j( M" `9 K1 ^2 M% x- uint8_t MeLineFollowerArray::getValue()3 b" d, R n8 S, k& s
- {
( A, Z0 b1 G5 E2 {( r - uint32_t LOW_level_read_time;+ k6 j( k" l( p; D, {
- uint32_t HIGH_level_read_time;: `3 C# R- J. z1 D7 U; o
- uint32_t time_out_flag;
6 b4 W' g) Y/ \6 k - uint8_t Sensor_Data[3];
# ~: s9 F. @) J - static uint8_t old_data = 0xff;
/ v4 g& K- R. E. B7 C. t
9 p/ U b4 f! L- pinMode(_DataPin, OUTPUT);
1 k7 R! J8 r9 P1 i0 ` - digitalWrite(_DataPin, LOW);$ Q# G4 T" N. B d
- delayMicroseconds(980);
2 m, J& ~# D Q4 d. d! l9 Z - digitalWrite(_DataPin, HIGH);3 _6 l) ]: a3 T) K
9 R4 e) ?4 |. T8 \3 |. ~- pinMode(_DataPin, INPUT_PULLUP);) h! d( k! S. e; T4 h5 {) f( U
- delayMicroseconds(10);
" I% h) @% t& _1 \ - " c* {; v9 s2 z
- time_out_flag = millis();- N. U8 m/ ^7 p( x
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! |& e, H" D4 b& I* Q4 h - 5 A# @9 @: L3 v
- LOW_level_read_time = micros();
) J x) Z' U' O: O+ H" b - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 X; _$ r* B$ C1 @5 b - {
, y3 N+ Y5 _& u1 X' y) ?* B, A9 g - return 0xff;& L- o+ G! _" n% h2 x
- }0 \* N; |$ Q+ r( g9 E
- ) Y; t1 W5 t! B+ A
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. Y% K6 t" x, [" j
' o( v9 k. I: C8 K: T& ?- HIGH_level_read_time = micros();
: }0 \; f9 Z" S3 i7 l7 g& k, S - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level" ]/ R2 o) C& L1 G9 \) S+ l
- ' x# X- p5 C+ V. ]; `
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ V9 V# f; V5 Y9 ]+ d
- {% P7 S2 l5 ^4 |& l
- return 0xff;
, u4 V* T; c" C+ \" A/ q3 h0 G - }
* G. A' h! D- Y& L$ e5 a - 6 |/ V1 x6 X+ s, @
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
' w6 b0 p, x: G& L! F1 v8 i$ W - {
2 y& y2 x2 R, _; {9 n( q: D - return 0xff;7 h6 P* C& D, O9 L, b& M* J! C
- }
2 K n8 F6 N( a! X: ]0 S
- ^0 l! s& G9 j ~' f* L8 D- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) B2 M( P5 L, s$ i/ _& O - LOW_level_read_time = micros();3 G9 h \4 C+ F2 U |3 x" g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
; @" g; u$ \. L" B1 m2 ~4 M" v% `
! D/ C" d/ e+ @- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" B* ]3 \4 B9 p
- {# @1 u1 o! X# g) Y
- return 0xff;: i3 u, U! L- ~, E8 T8 R& S) G
- }, y& j" Y; W1 O2 \: G# j
& f" e/ J/ @1 e6 A- \! s- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))! X7 ? Z( ^ H6 v8 u' ^8 ]
- {
4 i+ C* ?' G2 u! d, W5 s - return 0xff;8 h3 F v& F& a
- }& Y4 C) d2 j9 W# B" u' M+ H% b; d5 |
- 6 B4 {- k5 U- C4 D: R# W
- for(uint8_t k=0; k<3; k++)
; y& B4 Q t! J; w2 O6 t" _ - {4 L0 ~/ T3 [9 g3 P7 A1 t( ]) D
- Sensor_Data[k] = 0x00;
( N$ b. @% R/ I7 u. {
, Q' E6 ~7 t% g. {& o/ k- for(uint8_t i=0;i<8;i++)9 v: L* y8 H$ u% |
- {
* p3 [! |: J# W6 h( h' x1 { - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
+ d I z# e- v P! G - HIGH_level_read_time = micros();+ O5 s0 ~0 e3 M
- LOW_level_read_time = micros() - LOW_level_read_time;% k* [1 ` q0 n; H# s
- 7 L& |/ S: K0 S4 L+ {
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 }( M: ?7 {! g6 A6 s% |+ f; W3 _
- {
1 r; `2 F4 N0 ?$ F$ i3 ^) e - return 0xff;
: ^+ G+ B, t6 l) \" w( s, \ - }' z, h5 y5 { O9 U- b, D7 ]
- $ z" o+ k" B5 d: l$ Y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% P, }; q; J) b* l
- LOW_level_read_time = micros();
. K$ M" ?8 i6 R" p z, ]) H - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level# j+ A! T% J6 ]% n4 a* Z
- # ^: C! F9 M% @4 r5 k5 p
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1$ {. R' |' P7 @6 b% p: b4 N
- {4 O+ h4 t3 a( s5 j
- Sensor_Data[k] |= (0x80 >> i);% z9 j9 N) Q8 a3 Y6 k* H% K
- }
7 }7 N5 |( r6 W! l8 c - else if(HIGH_level_read_time >= 100)
) g9 d! E- p8 o - {3 \, P' ?5 n. G/ ]$ ]( r
- return 0xff;7 H8 s8 y" [$ r, i' o
- }
( F" u1 U" i0 W - # D6 ?( \: t2 W4 m+ \+ L4 b
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)" @# n. t& A) t) F
- {
, \: z: m8 m. r; E, X P - return 0xff;0 B5 N& D" j& P2 g
- }
1 A) a7 }2 I i! M( g8 x/ Q. } - }# O( k/ C" U5 l% G$ O+ M& r! ^
- }3 M* [* u2 S# `5 \- R5 k6 g* a
5 o3 x* R, X `- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, P i- v: I. a, | - HIGH_level_read_time = micros();
. U5 l i& p& o/ q: ` - LOW_level_read_time = micros() - LOW_level_read_time;. x9 z3 Z6 ~8 }# d( J
# X* E0 F" W1 u' R- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
$ u {) P0 p1 Q2 \3 R7 N( I+ A - {
2 p# `0 F% c+ N( r& H+ Q2 }8 s4 ^ - return 0xff;
J2 O; ^$ `/ Y# V' H3 S/ n/ E* Z - }* ~) O. [, B& _1 l9 \
6 V! Q2 y3 _ Y! {- pinMode(_DataPin, OUTPUT);
+ Y7 @/ G9 d9 W7 j7 W6 c - digitalWrite(_DataPin, HIGH);) K5 y5 g" S4 t! c+ C" t+ p; s# m
7 U: w+ D- I+ u# p- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ v/ b$ Y: ]+ ]! S0 f - {
9 p, @9 m2 v9 e& s( g! f. B - old_data = Sensor_Data[0];6 K/ E9 |' E, _/ p9 T. T
- return Sensor_Data[0];- Y; p8 l* m) J3 u) ~
- }/ [9 a3 m3 v* j! [
- else
! ?( i! Y! {( i; ` - {2 u1 j* R! H: T
- return old_data;! G3 @% Y6 |# q* c9 q
- }
6 \6 C4 R2 O Y - }$ c' W8 W8 g, p7 d7 O! \ o
複製代碼 0 @$ ?( [% B( K0 @: A+ J
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' l- D8 }3 s0 s; S* @
" D; }( p; y" S4 z2 i
|
|