|
% K, ~6 \& Z+ D; i1 K6 [
- #include <Arduino.h>
$ ]3 x {) @2 v p0 u - #include <MeAuriga.h>% Z" P+ U' X% m8 M
- #include "MeLineFollowerArray.h"8 m) P8 q8 n0 X; C; T# p9 Y4 [
- % B U$ Q! D2 l: A; w+ q
- MeEncoderOnBoard Encoder_1(SLOT1);
, t: q& p a; f) t: o - MeEncoderOnBoard Encoder_2(SLOT2);$ g$ x, L- k/ D
- MeLightSensor lightsensor_1(12);9 t" h+ u N+ C' I+ E) s
- MeLightSensor lightsensor_2(11);
( k7 u0 x' Y- R3 u, c - MeBuzzer buzzer;0 l0 g: |5 C n$ J
- MeLineFollowerArray linefollower(PORT_6);3 N. i7 a. G8 F( V
- 0 {7 }" H1 O) A
- #define Error1 17 T/ Y% k, b; ^
- #define Error2 21 @ Z& y% \- Z; b
- #define Error3 3) n1 {% T* ]% t4 h6 E# o
- $ \8 A* u: K6 E7 Q( Z+ I) T) J% }5 h
- #define Kp 157 M" `: f0 ?* m. `3 H! w$ V2 N
- #define Ki 0.15* U e7 ^8 S" J9 h3 z# w8 r
- #define Kd 0.03
- [7 R. S! r' R! ]8 i - ( K( ^/ J) A' r% y# @
- uint8_t sData;
5 ~5 m. n2 Q1 B) J: w5 A& K% p - uint8_t D1;9 G. _$ s. ^: m( w
- uint8_t D2;
5 M! S* X" ~9 f8 v/ ] - uint8_t D3;# c g( f8 g9 W& W v9 A
- uint8_t D4;. |! H3 S5 R+ d+ B) S% N
- uint8_t D5;( N( L# j' r9 U- W: T: i6 _
- uint8_t D6;
* |. B6 N8 p5 E2 ^" O, \
2 G+ Y$ Z: h& E: H1 N0 T9 G) j- float previous_error = 0;
9 n$ I4 o4 L* _9 r0 a - float integral = 0;
5 M8 U n3 D( g0 a1 @ - float derivative = 0;
& ?7 d, I" o+ l3 y h - int Speed = 160;
5 z& Q j) r6 f5 \- S6 u3 s' ` - float output;: a! B* Z8 ~8 P+ a& ?
- 0 f% h. a b7 n, S/ s) i
- byte Left;
7 [% |9 V& H7 ]! T3 {5 T3 s! c, n
5 n) ]$ X$ \; J' Y* B) e- void setup()
3 s1 x1 X* Z9 R! s - {
' H0 q) p" ?, i' U$ H- _: L5 _ - //Set PWM 8KHz) i) R$ z! f% N2 t* j( t }/ B
- TCCR1A = _BV(WGM10);
& C1 o8 u/ X3 V& j& N1 Z- `0 G - TCCR1B = _BV(CS11) | _BV(WGM12);) g' W4 @5 E7 ]: H+ I# w* T) D
- TCCR2A = _BV(WGM21) | _BV(WGM20);
2 e5 ?, k# V2 l; k - TCCR2B = _BV(CS21);
% d6 @% I+ l0 V9 C0 Y2 z - Serial.begin(9600); T5 s3 S% Y% P; M9 s! n! a2 o
- buzzer.setpin(45);
# M0 X0 \" i# ^9 \ - }
i; v1 D4 o) u6 T0 b2 ^" t
+ \( x7 u. v( [. W L- @) X- void loop()0 x5 b$ d K- `0 h
- {* [# ^0 f. b4 p2 m6 M! [
- while(!((lightsensor_1.read()) < (10)));1 C/ q% A% _1 c- ^) u* f- K
- buzzer.tone(1047, 500);0 [6 X# R* d9 X" |+ j# a) H
- delay(500);
# I6 \/ a9 E* d' |" r. n1 @ - do; F* W4 c, ?4 o6 c! b5 n$ _
- {
5 x. C; t' S* t1 x) g f6 Q - int Err = getErr();' k8 V3 J, m8 i
- if(D1 == 1)
0 U; N! g$ O( C, Q - {
/ T' A4 Q) n$ J0 c - Left = 1;8 H; V" C/ e$ O# P! j; s$ h7 H. t
- }7 Q9 {: a' L' y3 o& B) Y, c3 G
- if(D6 == 1)
7 z4 L A$ U2 T" y' \ - {. ]) [6 u }' ^) l2 l; t/ D" D
- Left = 0;
6 e+ D! k+ k4 i3 o+ i E - }& N/ b2 t0 Y& W$ m
- if(Err == 99)
3 I5 B* g7 ~" C& ` - {
( }6 r" z6 J7 S% T - if(Left == 1)
9 B0 `* P% E, _ - {
( v6 ]' u/ Q+ K7 Y( o8 y - Speed -= 5;/ k; g. E9 [: g0 l
- moto(0,Speed);
7 W6 ]. }8 A8 U# z2 E - do, p: ? R$ a1 ?
- {4 M, q8 b! A5 G8 L7 w7 J. F! W" ?
- Err = getErr();
2 N8 M' m& P, m% w8 |2 X$ A+ t - }while((D1+D6) == 0);* L8 K/ Z& N: U! t% W4 a) y4 k4 b: t
- }7 H6 t( s+ y3 P/ _$ {& C8 ^4 o
- else
# ?# w4 [7 z6 F" s5 L! T3 j0 b* V - {( ]# ~" r1 n5 n2 w
- Speed -= 5;
7 W8 }2 F0 W" y Y - moto(Speed,0);
* K' ]$ n2 n2 p9 v - do
7 T& L6 S! G* }; l6 K& M* | - {! n9 ` J' ^; q; `- F d/ w
- Err = getErr();; O. H- L( `; A2 N0 G$ Y! H
- }while((D1+D6) == 0);
4 K" ?) `* z4 Q% a. R3 p - }; `+ }. X b1 K. e
- }
9 n2 v& G' m8 n& C, l0 W. Y- m - else
" D4 R- T$ M: n" y# X2 n$ W1 W1 F - {
6 u4 h' S# u& J3 x$ ^, }: N% _# T/ _* y - if((Speed < 160) & (Err < 2)) Speed+=1;
5 Y7 A4 Q) P( W8 q - if((Speed > 100) & (Err > 2)) Speed-=2;
8 h/ `5 C5 E# p) K - integral = integral + Err;
+ @! k5 Q4 f% B5 ^$ o - derivative = Err - previous_error;2 o, b) b, I x2 Z5 K7 S- |
- output = Kp*Err + Ki*integral + Kd*derivative;. Y' R$ j4 R* b" {. r
- moto(int(Speed-output),int(Speed+output));; R3 ?4 w; E' E u' W! W/ ?
- previous_error = Err;
: P/ k* H T" U; u+ F7 V" T - }8 s* Y) B7 n1 P& \* q3 u
- }while(!((lightsensor_2.read()) < (10)));3 f, F( V, o) ]7 ]
- moto(0,0);
5 d" E, z2 @+ q1 ? - delay(500);2 k g2 o- I* n5 R. L# z" ~ f
- buzzer.tone(262, 500);5 Z i) n: C6 p) e6 ^2 |/ v
- }
/ h. \8 ?6 H& m7 X$ g) V* ]' b, K* t
; X- r" f' \2 P& {0 b- int getErr()
0 y5 m1 {& g5 v' g7 o - {
7 N' a R. q3 x7 m - sData = linefollower.getValue();& i# j; o6 l$ w0 B/ L/ E, }2 r
- D1 = ~(sData>>0)&1;
: b O8 \$ j( G' b1 ] - D2 = ~(sData>>1)&1;7 p* I% D1 b5 n) z7 u/ I2 Q
- D3 = ~(sData>>2)&1;
) |# N7 p! N# o - D4 = ~(sData>>3)&1;
( k1 Q4 D% i: Q* j# W5 G6 d - D5 = ~(sData>>4)&1;
! u; F. y. v3 P3 d( V ^ - D6 = ~(sData>>5)&1;" p/ m9 l0 p+ m, r6 W% ?1 _* q5 x
- int downD = D1+D2+D3+D4+D5+D6;
7 d( W! _9 M. I2 W4 x- n3 L - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);0 `+ c3 {/ B. h" R9 Z, K
- if(downD == 0)' g: T6 \* l. D# v$ B1 j! c
- {" k' g+ r5 Z. A6 A n
- return 99;% E# r$ ~2 c' U8 c/ L4 W$ h1 d
- }/ P- W" p! K" [5 K
- else& p0 O& N$ |6 Z$ ]6 f% { _* w
- {
$ |! n. y5 b9 H1 ` - return upD/downD;. D! y$ F$ V/ C0 E: z
- }
2 V) Z/ t2 A) B. F; [ - }
1 O0 a4 r7 q2 g5 \) a( k, | - " y8 y( T2 G+ ? I: t9 }" m7 H ?
- void moto(int Speed_L,int Speed_R)
2 a) n( Y" A, Z/ l# y" q0 e - {8 S+ S+ W# e8 W* u! @8 ?
- Encoder_2.setMotorPwm(Speed_L);* @" ?% q: l3 L5 {! Q: S
- Encoder_1.setMotorPwm(-Speed_R);
4 E1 q d8 L* `5 R - }
複製代碼 0 u, E* y; T1 ^
MeLineFollowerArray.cpp
9 z) `/ [3 C' J$ x- #include "MeLineFollowerArray.h"
5 p, x P5 I. C- N
" K( R' i6 G5 p& Y' \/ T- #ifdef ME_PORT_DEFINED
. N/ ^4 p6 o5 y* r6 t! s2 E# ]( c$ o - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
' d$ k+ M3 q# s0 z+ c# X2 @ - {
+ G% {0 R' @- v- D
/ A9 R& m+ ~- x2 R; o* q) s1 m- `& A5 ?- }. N# d. O: n/ N0 ~ Z. k/ [
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
; G% G8 G& f. R! P [ - {
' o! K4 n- l6 h3 E) T$ y - _DataPin = mePort[port].s2;# w, @! k- E6 _5 u; ?5 a
- pinMode(_DataPin, OUTPUT);
* a: K2 m6 I8 B; y. r - digitalWrite(_DataPin, HIGH);
; [! y3 u' |) A/ ]0 R5 z" Z - }% i* }: K2 t: ]7 t' M5 l" {
- #else // ME_PORT_DEFINED3 ~" Z9 H2 {' m5 d. N& l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
& o! k+ G" v' d6 Z% Q% t: G - {
: d6 d9 Z3 X1 ~: r& f# Y* A. c - _DataPin = pin;. k- N9 y4 w! ~* o1 g3 b! q- [( b
- pinMode(_DataPin, OUTPUT);
" X& I% S+ x* a I8 R' ^ - digitalWrite(_DataPin, HIGH);* E" j5 J1 E: M" C
- }- Y$ t) ~7 d3 S$ E& [, l4 K* I
- #endif // ME_PORT_DEFINED: J/ ~! Y# ^- ?
, t- O) \- T/ n0 X1 [
9 W. T L: R) H( a* j- void MeLineFollowerArray::setpin(uint8_t pin)
% I$ x$ f A$ H; b& L2 [ - {4 S9 V6 p$ x3 j, K& n3 L
- _DataPin = pin;
4 J9 @: S S6 Z1 \ - pinMode(_DataPin, OUTPUT); % E5 Y6 t; b/ f2 h
- digitalWrite(_DataPin, HIGH);6 J6 x" H! A9 h& f0 j
- , I; _7 W s+ C: A% f+ T
- #ifdef ME_PORT_DEFINED
9 a7 P& F7 N% @8 }2 s& q8 f - s2 = pin;0 _ a) f% E# m9 @. B' y( f
- #endif v( S0 }# p+ p+ w( b6 l
- } r7 m F6 A" f, `6 I( a5 y
7 g3 r# Z: Q, x- uint8_t MeLineFollowerArray::getValue() X2 d- \2 F/ e1 r- i/ l
- {% y' |9 D/ j3 y! r4 K
- uint32_t LOW_level_read_time;) j: t0 }0 A- a) C+ b: ?3 |% m+ y' [5 x
- uint32_t HIGH_level_read_time;
9 M; `5 S2 w0 T' }- c - uint32_t time_out_flag;) E) [/ f z& ^ B% Y- a4 F
- uint8_t Sensor_Data[3];
% ^3 j5 Z7 P6 z6 N - static uint8_t old_data = 0xff;
5 L8 Z- w# j! \
6 J8 l: _' K. T9 J9 f) [* m/ i/ g- pinMode(_DataPin, OUTPUT);, \5 z* l5 k. h- p& ?$ z* D2 m a
- digitalWrite(_DataPin, LOW);8 r6 ?& _6 E9 X; A9 U; `; I* |1 u6 y
- delayMicroseconds(980);/ b$ f( k5 g7 M2 o
- digitalWrite(_DataPin, HIGH);
7 i# Z/ p1 S( O" [ - - H& Q( `4 m; G! m
- pinMode(_DataPin, INPUT_PULLUP);+ m: k- v+ h/ b$ i! M- z3 r" |
- delayMicroseconds(10);. x) b6 n4 o+ Y- f+ x3 L5 L
; x* a- Z7 {; u! t9 ]- time_out_flag = millis();- k7 g! X5 h. ?# N4 f
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 f$ q% b6 W S8 Y6 C z. z6 W - 3 w& {" E& N" v* @% e {; y0 ?: @
- LOW_level_read_time = micros();2 @( |& o1 W* J7 b. `+ V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: |: d! P4 k; H0 t' C
- {9 n! Q5 z$ V; l+ B2 z: R0 q7 G
- return 0xff;
3 b. y! B3 a, s - }, C0 s; W9 _. L& Z. T9 h
3 |1 t6 T p2 s* u9 r, w- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
W/ f- }% m& l6 n2 x
, _& S# n+ z0 L) U0 W: j* K- HIGH_level_read_time = micros();
( C9 R- I2 I/ H* x* N - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level+ I6 n% v3 n0 y
; @/ S& |' y4 {% N. I6 n0 f" ~! y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' O6 v/ w8 ~8 y6 l- D k - {" W- f, f- |( K& y4 i' |9 j
- return 0xff;
* u. x/ W3 ^0 C0 e" p/ X: ]) G; g - }! c, Z" S" d# g& B' B: y, y# y
5 s3 S. `& f0 w! ?. p- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
7 U4 Q X; S5 ]" N) @* m/ R - {2 h; g# \! [ m, | A9 W' ~
- return 0xff;
+ m* R. h" T1 Q9 s% E9 J( l) T6 v# g - }2 Y% r& B* u: e: `$ M$ \
; \& z- F2 ^5 p% ^- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- S3 f) _3 L# G7 `; L5 y; H - LOW_level_read_time = micros();+ L' B! E( c# j; \; e
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
L$ w3 V; e: d: \* j( d
0 X7 t, q, F/ W2 o/ t7 a- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 d6 a$ o- W9 [# |9 |% H - {0 ]( G3 s3 b3 k9 v" F8 A
- return 0xff;
+ w3 ^/ n' l- W3 ?3 V - }6 X- }# K( D+ \' @7 x( z+ F
& G% w+ f: k% F" B" x6 T' X: K) h- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
+ Y% K' x2 c" [; x: a( ] - {
, Q1 E. K' w! v' Q - return 0xff;. |) R+ ]/ }$ c ?0 n* A- W
- }
* O# l' n; S _% a - + x- J# s- Z0 F; `$ }: g4 c' M4 ?
- for(uint8_t k=0; k<3; k++)& G6 Q4 l% n4 G" r7 y# o
- {
" j/ f( q: D, d. H2 K# a - Sensor_Data[k] = 0x00;
1 x4 h* B; |2 M: N - ) J5 r2 }& G4 Q. l% Z
- for(uint8_t i=0;i<8;i++). V! P7 u$ r4 `
- {7 {2 K* G& U# s6 W1 N( z( y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level) @& I2 Q+ x# R
- HIGH_level_read_time = micros();) x- {& P" w: D/ X7 a
- LOW_level_read_time = micros() - LOW_level_read_time;4 S2 g' @! ]6 Z+ D
- 2 ^* |; Y0 P: @: q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
5 V& U8 g' u6 V4 c* o - {
8 B- z4 F7 p- H - return 0xff;
, ]4 I) @! Q7 b - }
9 s& b: A$ B4 H6 ?9 a2 S$ x
0 Z" n8 [$ ]" u% M- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* O$ z' g. S* G3 O, o3 H, y
- LOW_level_read_time = micros();7 V" t2 c. n P4 X9 L u
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
6 N* W% m% ?' q: |8 D7 t# Q3 n3 x
/ O: b- `, D: R5 r% L+ n- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1+ M. P3 }. D8 P% m
- {$ S: `5 w/ l) ^& P
- Sensor_Data[k] |= (0x80 >> i);
7 E9 F, Z1 N8 c! y) T - }& m& M9 q. @9 f* a3 F2 y
- else if(HIGH_level_read_time >= 100)/ \4 { {9 ^/ H+ h! l3 F3 \
- {1 S! D1 c9 o' y1 B" ]; P3 X
- return 0xff;
6 K+ T4 x6 }2 G! x5 p - }
, T( t/ o5 p. b& i, R- H! t- ]
3 X2 a/ L) P# {8 p- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
2 y" N! v' ^! r) {. q; | - {6 W: C4 Y0 o2 I" t9 w/ a) I. n
- return 0xff;
" `0 q1 X$ U% {& q# `" k/ Q - }# I, w) O( e5 J1 g1 O% U
- }. b: c% |# t6 Z: N) ^
- }1 ]7 D* ]. [- H' p# U6 l" w: }
0 y q- ?- t; Y7 W) d, N- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level0 F$ p' F2 @# e4 P
- HIGH_level_read_time = micros();
# X7 C5 D% @. ], ?8 p - LOW_level_read_time = micros() - LOW_level_read_time;6 W0 C, y# [* Y! m# I8 [ Y
- ) A Y9 u" X1 d* X' k1 {" p- R! x
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
2 ]- D2 @* `# K! W" Y - {$ B4 V, m1 r, u, F/ o
- return 0xff;: G% ]# `+ {; S z6 Q
- }* D7 e' u3 u4 @. V: ^4 E( y: D
* e( w3 M( K4 Y- K4 V- pinMode(_DataPin, OUTPUT);5 N& N3 a4 f% C5 w) Y: X, o' T
- digitalWrite(_DataPin, HIGH);
4 ]: }: F/ }/ O/ b - 9 Z9 w1 |* F# e8 o* l' M# e
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))3 |* ^& Y! k; H& B" _
- {
. u0 F! O) O: g4 s+ O - old_data = Sensor_Data[0];
, t& B% D' D3 n7 T6 f! B - return Sensor_Data[0];
0 H, q. ?( i9 q# i* U4 I- Q0 y - }: C) C) p" S! s' z) u& W. }
- else
' Y; z, i' q; x; K$ g% E - {
) A1 D5 m* f* y3 ] - return old_data;! L3 ]3 }; M0 \% [! B4 p# e6 h }
- } d: P5 _( n- T; f* s k5 X+ A
- }
4 r$ j; `* C4 G" d+ m' J3 M
複製代碼 % J0 {$ G. R! x# ?: q2 c; b
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, Z# B" V: {, z% a( ]% Z; M- x- P: K' v* H
|
|