|
|
$ g. {, h. ^3 _; ?
- #include <Arduino.h>
4 s: t; y( q [ - #include <MeAuriga.h>3 q' z" p+ G/ w+ D; F! R; K% R
- #include "MeLineFollowerArray.h"
3 z! m6 q3 [. w d
+ Z$ E0 x4 E P- MeEncoderOnBoard Encoder_1(SLOT1);2 P4 Z* D. o1 B/ O* b+ C1 c
- MeEncoderOnBoard Encoder_2(SLOT2);& h6 H ?/ y: ]: t; b( B/ x# H
- MeLightSensor lightsensor_1(12);
% K! x4 e, W1 ^# V# D' | - MeLightSensor lightsensor_2(11);
O0 A' C+ k ]# E - MeBuzzer buzzer;4 f2 c( r( t8 q- @: x! X' Q0 @2 e4 X
- MeLineFollowerArray linefollower(PORT_6);9 v" B. f9 c' P1 n k/ Q7 x
% o" @) k0 F' O# T5 X% d- #define Error1 1, G0 e" d( ]5 q: w. S; c% g
- #define Error2 2, H" Q# r' J3 B# `9 C
- #define Error3 3$ ^* L) Q' \1 G4 }1 C
- $ Y3 ]4 \! r! w
- #define Kp 152 O. A2 @8 |. Q3 y. m9 N& J
- #define Ki 0.15
/ @& `6 V! `8 B6 ?! J0 d6 o! k1 l - #define Kd 0.038 _; B+ d) T$ @4 S: c
- 3 @( \, f G6 f5 l$ c5 [6 T
- uint8_t sData;9 j/ Q: O/ s5 j3 |1 Y) |0 v
- uint8_t D1;
/ z0 d; H9 {+ C. E- x4 a - uint8_t D2;
2 j8 N/ y6 o: {' \( U A$ T; T - uint8_t D3;
/ {! V$ t/ D7 K - uint8_t D4;
: h5 i. ]" R7 D6 e7 V - uint8_t D5;2 R' _$ A; g, D, K" r3 W4 T
- uint8_t D6;& o% T4 d7 b+ e6 s7 f
6 B9 _( n8 J4 `) y8 ]. r; y- float previous_error = 0;
' o7 ?6 h. H" s7 ]( O, G/ F/ n- l& h - float integral = 0;0 u! B9 H% M; p: F' w' }
- float derivative = 0;
2 Y. t. b& e% P0 o X - int Speed = 160;# W3 t1 V, h) _: m
- float output;
% ]2 b2 b1 N) J. V0 A! z - . t" d: X3 N2 x9 d# s5 n
- byte Left;
+ d' z9 r) N( v- x, l( v7 b# s* u. A
. c& B: P7 Q8 F- void setup()
$ `" L' i5 C9 L# z4 P q+ B - {
( Y* u, J/ f$ c" e! c - //Set PWM 8KHz2 c" d7 \/ f! a% d$ g& z9 y$ f
- TCCR1A = _BV(WGM10);& g1 I { G" k
- TCCR1B = _BV(CS11) | _BV(WGM12);
$ I6 ^" |0 p* R% ^0 ~# I* ?, ` - TCCR2A = _BV(WGM21) | _BV(WGM20);
' u" T/ A6 \5 r - TCCR2B = _BV(CS21);
' s5 p) E7 {9 \ - Serial.begin(9600);8 d+ ]/ ~/ Y3 z5 a) W
- buzzer.setpin(45);1 ~7 h( h/ K% q* I. \1 i- x6 c
- }
, f+ R9 T" {0 z( X4 a c; c$ T
3 }. I7 t0 S+ c1 s8 v7 O7 |* ?- void loop()* S- K4 k$ M) n( w- c J7 V8 ]
- {+ G; W# s( F% {* g8 L1 E
- while(!((lightsensor_1.read()) < (10)));% W! L z/ U4 A2 A! i, S1 a
- buzzer.tone(1047, 500);
: t: x' H$ b2 X: a7 V. |0 Z - delay(500);% z8 m/ @; S( K0 I) Z5 |$ F
- do
4 R+ ~4 e- v+ `3 F3 K - {
, [4 \3 z1 t# O' x - int Err = getErr();: T9 Q; g; l7 E3 _2 i% p; l
- if(D1 == 1)
; U' o/ Z5 w4 [- X5 V" C: p+ u - {% d" q4 q3 y* _5 J- e ?
- Left = 1; t) o. g- n* X% f: [$ H1 F# _
- }+ _; _) Y) g0 S4 T9 W
- if(D6 == 1)4 E0 z" h8 t* a& b1 S* e. Q, w, M
- {
; k6 j- R# u/ N5 A- F - Left = 0;! y2 n* x$ W4 w7 s# _0 D
- }
9 p) M& }8 u7 K0 h+ P - if(Err == 99)/ Z, }/ l* G; {2 u
- { S+ i @/ t' I1 n; o8 a
- if(Left == 1)& D# x6 f8 e l
- {2 h( E" f" R( I5 L7 w0 Z: M
- Speed -= 5;; E/ o' ^& @5 C/ {7 c- F4 V7 X3 ^
- moto(0,Speed);
! U4 D& G3 v, f/ a$ c) J - do
+ W5 l: S. h# @# V' A* z+ e; A - {
, ^4 d& ^& p/ u( }& m - Err = getErr();3 g& @+ S# j* D, P! G: ?
- }while((D1+D6) == 0);
; X f$ B/ t+ w7 x' v - }
5 m+ J; W" `+ X" E$ A - else
5 `% Z8 v" j% ~# p9 u' b8 Z; { - {9 w" Z( Q/ q" u5 Z: o. l! B; L5 L0 y
- Speed -= 5;# k# m. X* c1 M; y
- moto(Speed,0);
) W" Y9 v' `# U% r% p1 f& a) @ - do6 L. P' w* W$ n6 @! N
- {8 e7 p0 j; _+ [+ a$ ?- J
- Err = getErr();# U9 G# U; l ?% m3 ?; r' M
- }while((D1+D6) == 0);; @# ~7 Q9 B% }
- }
$ Y( i0 p1 R6 } - }' k. ]- j# U Q2 H
- else
0 c; a1 y# B3 T' ~3 p# N I0 h4 R - {
6 ~) R2 v" E8 u. f+ e3 t, \% Q, M - if((Speed < 160) & (Err < 2)) Speed+=1;
- ~4 O0 w; [' G* h9 \2 I: b - if((Speed > 100) & (Err > 2)) Speed-=2;
6 f8 \- Y6 q8 u; n - integral = integral + Err;+ c: Z# h5 J+ P. A: D( K9 y) r
- derivative = Err - previous_error;1 t$ F7 m T$ r3 ?7 G; T J* R( N
- output = Kp*Err + Ki*integral + Kd*derivative;) _4 `5 U% B! ^
- moto(int(Speed-output),int(Speed+output));
( f7 \- E" |9 H - previous_error = Err;
% a0 [" g# @( ?' Z: i3 \. r - }
% U' P d6 x s - }while(!((lightsensor_2.read()) < (10))); X$ ~4 V& [! Z
- moto(0,0);" V: ^, w. ]2 t- l
- delay(500);+ q3 g; R {3 X% r3 b
- buzzer.tone(262, 500);
) z: v+ \9 m; ?) w - }& q$ T6 H x5 M$ r3 j
9 X3 [( a, O1 K4 Q: _9 [# j5 c- int getErr()( [7 O0 {5 W) H5 {3 s- x N3 l
- {
6 }* J7 ~' Q* a+ v1 D/ J - sData = linefollower.getValue();
8 j& @1 \3 Q; u: i- l& u - D1 = ~(sData>>0)&1;2 |& u* V+ Y2 p& K+ b5 H' B! Z" _ B
- D2 = ~(sData>>1)&1;, X& Y! d* \3 s+ Q4 K/ ?
- D3 = ~(sData>>2)&1;! b6 ^6 H5 F: u
- D4 = ~(sData>>3)&1;3 g1 P3 F1 e1 _* q) _ ^ H& w
- D5 = ~(sData>>4)&1;( ~1 z/ p" h3 R5 _$ G5 P
- D6 = ~(sData>>5)&1;; f& S; Z7 C6 z: e) Q# f+ N! U
- int downD = D1+D2+D3+D4+D5+D6;+ r1 @( ?$ H% F3 ~, u' P2 T
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);6 g7 I; l; F/ C# _3 M* M1 x2 {
- if(downD == 0)
; z# w5 S7 `5 h5 U - {
. E$ K1 q8 d6 f) N. {# h" S/ Z - return 99;- j: z+ ?3 a6 \% c
- }
( e) O* u: B+ ?+ M( s3 V: h - else6 S0 |- W3 y. U) {1 ]. n
- {
6 }1 y& o. \0 g5 N - return upD/downD;
1 r0 F2 j. P, ?" Q: t0 v! z- e - }. u- O8 H3 z9 W4 z7 ]1 \/ x; @ k/ H! z3 r
- }# {7 s6 A+ U( w7 ?' C+ o/ C
- ) P/ P5 e/ S! a% a4 {4 f
- void moto(int Speed_L,int Speed_R)
0 ]9 D0 n) d6 H7 M9 b - {
3 `/ e# _' w( H( \5 P: z1 `! b# z6 w - Encoder_2.setMotorPwm(Speed_L);* q2 q; H* }8 {" j$ M: ~" i. s
- Encoder_1.setMotorPwm(-Speed_R);- B2 P6 k3 p, v& e
- }
複製代碼
$ K; p% E" u! pMeLineFollowerArray.cpp5 w+ m7 y0 n1 z
- #include "MeLineFollowerArray.h": s# }$ j! V& a0 I% u' P% e8 t) j
: L( G* W, U" Y) ]2 {' A: u- #ifdef ME_PORT_DEFINED
) v) H- q: e' f - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)5 q% O4 H6 C3 I$ v+ T! @
- {
* r9 t/ Y# F1 Z P* i - / l- Z. F# t) N- A1 I/ X
- }, V' ?- b, o: @( o* f3 ^
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)4 s" ~7 o7 w* B* P% i" y) h8 l
- {1 Q2 \, N# h7 r- A* M F
- _DataPin = mePort[port].s2;
0 V+ Z) g* _) d& {9 o - pinMode(_DataPin, OUTPUT); f0 [$ V' g% v8 g, k4 _6 j
- digitalWrite(_DataPin, HIGH);2 p m0 y- o: R4 n7 S: }
- }
7 g1 d) b8 B5 J1 R& b - #else // ME_PORT_DEFINED% D. Y `) Z4 Q$ A; f3 A! E
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)7 b/ k0 P, `# c r3 \! u$ g ~
- {9 W5 j% P& x( j! ]
- _DataPin = pin;7 T: k, c2 K8 Z" |0 X# g# M
- pinMode(_DataPin, OUTPUT);
# e+ C# Q4 z( S j. p" e - digitalWrite(_DataPin, HIGH);
* |' e# W$ q/ ?% p+ V - }# v" o" J0 |" L; M1 B6 h; M3 o
- #endif // ME_PORT_DEFINED
2 I5 x5 t8 y. h r- m. B - j' p; a, F/ e- X
- 3 L. T4 V! w& H; Y! A1 I3 ^
- void MeLineFollowerArray::setpin(uint8_t pin)% y# }, p) T) {0 \4 Z5 w
- {) e% q7 m' p2 G* R5 h% ]+ J8 F& T
- _DataPin = pin;
3 e7 [$ m# e9 X* P; I - pinMode(_DataPin, OUTPUT); ( i z1 P7 c! T+ w1 _
- digitalWrite(_DataPin, HIGH);
; Q4 N3 L9 L; T. U6 U - # G) I& d( }9 F, a6 f( b/ n
- #ifdef ME_PORT_DEFINED
8 [ i3 {1 G, r% ?, G8 c8 g - s2 = pin;" c' l; |5 F5 f- ~' m/ _9 |
- #endif# X; @3 j& z1 T# p+ T) K1 |
- }
$ V& a! H7 _% \0 n. B& c/ Y/ K( Q, e - $ a/ o0 F* M! q
- uint8_t MeLineFollowerArray::getValue()
6 _- y, j8 q/ ]8 Z/ c3 M0 [8 H4 z - {
R1 i3 ?4 Q4 T* o$ o; ~( z: _ - uint32_t LOW_level_read_time;( S1 y; k p$ E/ T- G# H8 B+ d0 }& ~
- uint32_t HIGH_level_read_time;
7 | J6 q* d+ F: m2 {5 q - uint32_t time_out_flag;
1 ]: K: b: C5 X2 C; X( Z - uint8_t Sensor_Data[3];
$ o" V+ g0 a! @3 U3 J1 t - static uint8_t old_data = 0xff;
% m+ v, X$ [4 e# L7 e6 ]
4 A' [, v& p% n8 m0 G3 M. K- pinMode(_DataPin, OUTPUT);
! \% [ n7 w' i1 ` - digitalWrite(_DataPin, LOW); b6 w/ y4 f( }1 ], u
- delayMicroseconds(980);
9 t, L- [' X5 Q( I) O - digitalWrite(_DataPin, HIGH);
; Y( o7 _: Z( G& m- i
, V# ?( p! k Y5 h- @- pinMode(_DataPin, INPUT_PULLUP);
- T5 }2 _7 | e' t - delayMicroseconds(10);0 N# M8 t. o, K0 @
. j9 F! M& f( m( |& X: Z) p- time_out_flag = millis();" l# Q: ]. ]) u1 ]' J8 H* I t1 ~
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 j" N% [4 Y8 V- J8 x# P( D3 j$ b
! n9 A5 t. I. {: b5 O- LOW_level_read_time = micros();
, c' A8 ?0 p s5 d. O% B - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ J7 Q0 f! l. _! b; _" {
- {. j& U9 g6 N6 {* S' ~( Z3 r: R
- return 0xff;5 t3 e+ Q8 U$ ^ \
- }
; k% `+ A/ p5 ]+ h- l+ V
2 h0 c- P$ s# G c- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. [. k* J* { v* m
* q7 Q- D/ K+ d* a* j5 d- HIGH_level_read_time = micros();4 d: ~7 \' a) X) |$ ?
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; J" \- d, p4 _# Z( k) T) r
- , R4 _2 y8 }' l ]1 p; H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, H; E u" p1 o0 Y2 V# a* Z* }
- {
- M; y, c; v& A @ - return 0xff;
0 {5 n1 ? l4 ~ e1 r4 t - }
* X; ^8 R* K8 X# L/ w$ ^ - / i2 i9 V+ `0 z
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))2 R6 S4 R8 X9 B1 y$ Q8 E$ o
- {6 W/ N. S$ S5 w' n- N
- return 0xff;5 P' }" E6 b( w# q
- }% p0 r& G1 _# E' }* s
- " `% u1 W- C# U
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- m }0 R* A |
- LOW_level_read_time = micros();
0 _) ]6 k. Z- n2 v - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level; O/ G, d3 W6 U: Z$ f) n
- % H) S* O- R! Y# ?* ?" A
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% t" R m) {/ _ e5 J( `
- {
% }. J* N9 e4 M8 M6 M6 L6 R6 [) G - return 0xff;
# }. }' W% h) D: c7 a - }
* h2 S& U9 d* Q" A - 7 p9 _4 G$ t/ ?% H* Z+ E+ t
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; |1 s& O1 p3 V: ?: T - {( l! h. f# }$ E( _
- return 0xff; q! c6 u9 a3 U* I
- }2 d# w1 u: B( a F0 a) U
, k T6 | J. i. b; k h9 {- for(uint8_t k=0; k<3; k++)4 m5 Q5 _; d* M! V- C/ t4 u" a, z
- {
, e1 p" i5 W" i+ p9 w/ | - Sensor_Data[k] = 0x00;6 K" b; U& J+ S+ `' K
$ U# n# t2 a) t! @% F- for(uint8_t i=0;i<8;i++)* C9 Y2 s3 m3 p/ E
- {
5 c! G: e2 C* W+ h3 z7 U - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level! K, S2 A! H2 Q' A
- HIGH_level_read_time = micros();2 }9 S- n1 Z- O# Q
- LOW_level_read_time = micros() - LOW_level_read_time;4 T8 {) Y' V$ k1 `$ m
0 I! j/ g- K `. O$ h6 J# K0 C- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
4 d Y q9 D4 U" G- W2 z& S2 v$ z - {
8 `2 {( D+ Y4 F# Q - return 0xff;* p% }9 f3 S. A, s; O3 j5 U1 `
- }
9 B: j h$ d- B t( ? - . H# g" o- z! k8 z$ I+ a
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. m- X* F$ v$ U3 z. U' J8 w0 {! x - LOW_level_read_time = micros();
+ d4 h5 u5 S C& D' P - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
% k; f, L0 l8 }) ] - / Z& P% V2 Q2 d3 i5 ]( ~4 p
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1. ~- n8 x- Q* K* d+ @, X
- {3 X5 b9 w$ [! e% r% i3 x+ b: _$ I
- Sensor_Data[k] |= (0x80 >> i);
) o' W8 i- O4 S7 C - }
7 }. W9 p; ?/ g. A N - else if(HIGH_level_read_time >= 100): o. x9 x2 x& `9 a& O3 l/ @% A
- {- H8 |% P4 L9 ]' n% F5 r# w
- return 0xff;3 W* @$ S' c# U* b& P1 C6 @
- }! k6 S* u6 c9 _# A1 b: G
- ! }7 [# y) P% |0 N
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
3 m5 t9 g% e, B$ m$ ]: X - {- I* P1 N% u/ T, }# f& |
- return 0xff;
# d. E5 p; K* {; K3 T1 @4 b% U - }6 q+ E! ` S( S0 Y5 z
- }# |0 w! A2 l; R
- }$ R+ d) {6 ^/ Q* `1 ?$ R0 ^
- # C% p( ~, @6 _, x$ b7 W% E8 m' x
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level& g8 C% G! l# R5 y
- HIGH_level_read_time = micros();
2 ~' A! N- t4 u( K, Z" N - LOW_level_read_time = micros() - LOW_level_read_time;1 L* o" C; N: D- R O% a
- 2 Y, j8 R; Z3 }: Z1 j. \/ z: G
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )4 C3 f' b l( d/ W8 S# \
- {: E; h4 A! u, Q$ A, j
- return 0xff;
: j! t Z* T4 a2 s) }2 H* O - }
' `7 P4 ~% J5 {$ R
5 m8 a; z, z: X. j: u- pinMode(_DataPin, OUTPUT);7 _9 t. V H+ ~9 y. P% Z
- digitalWrite(_DataPin, HIGH);) t* ]$ ~3 Z9 T
4 ]/ n* k9 @+ {3 J- v- c- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
3 w8 ^ E& \* C9 T - {
' z0 X- g% \) O: O: h/ u - old_data = Sensor_Data[0];! V W0 f2 K7 o. B
- return Sensor_Data[0];
% o+ s6 A$ X8 V' f3 Y l% \ - }
2 T0 c1 J& _4 n; P% k' Y a$ w, f - else4 g2 a& [/ R' ]0 H
- {
1 G. g- M' `. E% q8 P! A8 m: e - return old_data;
: m/ `5 U8 |5 O8 v8 a: {0 I4 ]3 e9 @ - }% D8 |* Q. a! i' V
- }7 F' Z, a3 R, O$ z. G! k
複製代碼 + V9 g$ l% ?% P- Q+ e: {/ H& `, N' O
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' X# a/ W+ V9 w0 I. N7 T* P' Y( ^* b2 Q8 F9 f+ [
|
|