|
|
7 { e& T2 l' M- [6 ]- #include <Arduino.h>
) y; z2 l, _3 E; z3 \, ~1 L! m - #include <MeAuriga.h>! _2 A) S/ A; f
- #include "MeLineFollowerArray.h"+ f5 w7 h9 `- x2 t- m# h
7 N% j6 N( \9 ?" R) d- MeEncoderOnBoard Encoder_1(SLOT1);
0 d, a) z0 g0 W% N - MeEncoderOnBoard Encoder_2(SLOT2);
/ g( `! ]% z' d8 Y3 b h - MeLightSensor lightsensor_1(12);1 Y2 u8 H4 w( d& L3 s9 p
- MeLightSensor lightsensor_2(11);* j+ I' |( J6 g1 U
- MeBuzzer buzzer;
) F( b2 P+ q5 Y0 a4 [) V2 ^ - MeLineFollowerArray linefollower(PORT_6);& H6 u; j$ v% F$ p
' A; L: r/ |+ t/ S' J! J0 I3 v* [* y- #define Error1 1
9 [* s- {/ D/ a: k: C- I - #define Error2 24 t7 s+ ?$ j. `3 _
- #define Error3 3
' M! p6 f6 B, l! _+ B
5 `$ k- \( X$ @- #define Kp 158 C" S% }' z5 K! P1 c8 t
- #define Ki 0.159 i. J4 y6 ~, F! j5 K
- #define Kd 0.03
# `6 G$ \7 @2 I5 J- T0 a - * `5 U! V6 F% N0 B( O& v) b8 F
- uint8_t sData;
* ~+ r- z% E( @. b, {. j - uint8_t D1;+ s- ]5 v* b9 x3 j7 z
- uint8_t D2;
9 J2 Y/ m4 `( J- M- I) A7 X1 X - uint8_t D3;& y- \# _, w7 w( G, L+ {6 p. v8 N: ~
- uint8_t D4;
5 @6 H9 ?5 t4 l) W% F/ Y( u# Y" j - uint8_t D5;
8 F6 Z! K4 D% q) o: c - uint8_t D6;
$ l0 j+ _5 i) j
; n; o @5 W- ^ ~$ y+ s4 P- float previous_error = 0;
' {0 ?" n9 \6 l8 G7 Y - float integral = 0;) }& x+ d" t ?9 P6 q
- float derivative = 0;) F: q2 X# O& e( ^1 y
- int Speed = 160;
# c" b( c2 L& B( h" M. U - float output;3 _# G) m" m0 [5 W" X8 ]
- - @% I3 w. k; i. D4 B5 L" Q! |; A
- byte Left;* d" i% c) N" f$ x6 |
; z! ~$ T& d* b* e$ p1 g2 g+ O0 U- void setup() " T- u5 F4 |& z! W# V
- {& F3 V y' G- L# W
- //Set PWM 8KHz
; B% T% F. y% E1 D5 p! G - TCCR1A = _BV(WGM10);5 a7 V. d& [7 B
- TCCR1B = _BV(CS11) | _BV(WGM12);
+ h H6 v2 E# K - TCCR2A = _BV(WGM21) | _BV(WGM20);4 e: ~" k$ }& \( q+ Q/ q/ s k
- TCCR2B = _BV(CS21);
! q8 {: v. D4 M7 v* f* d* f - Serial.begin(9600);
2 b7 f! c6 Y1 G' K7 [" N - buzzer.setpin(45);
4 Z* Q* Z& P" |" Y* a1 Z0 U6 k - }
1 I; z; U0 p1 g1 r c6 E6 L - ) f& U! |+ Y3 |# ?: X7 a$ P' t
- void loop()( s( B! K: H( P/ N5 p% _4 w
- {/ p e- b: y5 H+ W
- while(!((lightsensor_1.read()) < (10))); r$ {+ M) {% Q# _
- buzzer.tone(1047, 500);
# _% ^7 G2 Z) b( C* p' r8 E - delay(500);$ F2 c1 p9 y+ N2 f
- do
% B. ?/ b Q9 }2 I - {
! J/ T% Z6 C3 z" ` - int Err = getErr();: K1 ?9 [8 S# V/ W
- if(D1 == 1)9 X" o0 ]% Y" {4 B0 U0 e
- {1 g4 ^& n+ i4 M" ^ T; k
- Left = 1;: s6 r6 w! D& \. A# L
- }
$ \6 _/ ^/ {, t8 Y* y7 l - if(D6 == 1): K- {0 x' T7 l
- {
3 N( {3 f/ r- t2 i, K - Left = 0;/ L) w: G& Q9 e) m
- }6 `/ [- g4 y C
- if(Err == 99)( v9 W& J" C- y: ^
- {8 Q. G- b+ n2 J" H
- if(Left == 1); q$ Z+ K* q9 `! a0 V5 C, b3 J e
- {2 m1 q1 W6 U8 Z
- Speed -= 5;2 H$ X1 x" ]0 H. f) E: x# h1 s
- moto(0,Speed);5 ?! x5 ~2 Z- O, [
- do
! V3 @0 ?5 n) m. R1 u3 A5 [ - {
3 y& s& H6 i8 r: X% F2 x, ~; i/ f - Err = getErr();5 B' N( v! k9 ~8 ?* _
- }while((D1+D6) == 0);. n- G5 Z' e% `9 V+ k* r
- }$ J( d- C& c: e5 m6 x/ X) X3 n
- else
- S8 u4 p0 d; J9 C5 b - {2 N, M: @6 r+ P* ]8 Q" Q
- Speed -= 5;9 Q5 b9 D. ]& z6 L( J) X
- moto(Speed,0); 9 K7 K: l) Q$ }/ R
- do6 e4 _6 }% `0 b$ F3 J2 z6 V7 R
- {8 i& S9 K2 r# V1 L6 e, [5 c# h
- Err = getErr();
U) O) {7 G; g - }while((D1+D6) == 0);
- t( f! _- R, F5 E: B, I4 a - }
9 C& W9 K2 h. M# q+ o# x2 q1 S+ X - }
8 {" k+ |3 ~- p4 e( n/ | - else
1 H5 J6 r3 P* s4 ?. \1 {% P - {( l% o' } H0 Q( U# N/ w
- if((Speed < 160) & (Err < 2)) Speed+=1; , }! H. W0 L. B8 D2 d
- if((Speed > 100) & (Err > 2)) Speed-=2; + P) g) }# E2 |
- integral = integral + Err;( ^ f8 s9 K; y' Q. n
- derivative = Err - previous_error;
$ n7 V3 r' z' n! b - output = Kp*Err + Ki*integral + Kd*derivative;
, O, V! x A0 V8 m4 c' I j: X - moto(int(Speed-output),int(Speed+output));, |$ V; K m: }* i
- previous_error = Err;
; \3 ?$ O a8 C1 Z1 u4 e7 J - }
) O3 y$ N# f! d4 ~" P$ ^* e# g - }while(!((lightsensor_2.read()) < (10)));
1 T5 u5 ]3 S8 r# u& H5 w- X6 i, d - moto(0,0);
; a2 o* N6 x3 k2 O' H - delay(500);
: A' C9 e9 B# {" c9 r8 w - buzzer.tone(262, 500);
# a5 g7 F- m" O - }
; h, h1 I' L- M8 I
0 s2 g5 W. Z+ ~ {3 x- int getErr()
/ g4 u: |+ _( [: d$ ? - { 6 n- \8 T) f) A3 P8 _$ h9 N# Q! Y
- sData = linefollower.getValue();
7 V# W( `8 \! _1 R+ T6 } - D1 = ~(sData>>0)&1;
, ~! b% o5 B- Y - D2 = ~(sData>>1)&1;, i$ Z/ n1 ^4 y5 O0 j- ]7 [; Z, K
- D3 = ~(sData>>2)&1;0 S$ D& d' v. U" ?2 I5 Q! Z
- D4 = ~(sData>>3)&1;& i. l" \, F3 k2 }' `8 g
- D5 = ~(sData>>4)&1;
4 r' @6 @9 q2 v5 M: ?3 I- `$ |0 M - D6 = ~(sData>>5)&1;
7 {! v; T; Q# ~/ s, h - int downD = D1+D2+D3+D4+D5+D6;
% M# v; i' V: R - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
* d: x4 Z3 W0 Y, C - if(downD == 0). I' f) E4 `8 {/ @. r4 a5 N
- {# R9 w- H9 y- C7 q2 r2 [/ P) w
- return 99;3 w5 Z+ |& H. g4 B7 l) g% _- G
- }' [' y' I6 R; V- x K1 A% S
- else
. }4 R7 A( Y2 u& I+ L# ]8 a - {, y/ t( |- L9 t f0 h) T; P
- return upD/downD;
# g7 o% c4 p B# o W - }
1 P# U* P8 a8 {4 g: J; Z - }
$ P n9 Z" p( T4 ?
, r& \6 _6 [6 Z Y- void moto(int Speed_L,int Speed_R)& p7 T/ Q% v& X# R+ c* [/ N. [
- {5 M: H D4 Y' ~ o3 Z8 S% y2 R
- Encoder_2.setMotorPwm(Speed_L);
; p& u; V5 |6 b - Encoder_1.setMotorPwm(-Speed_R);7 ]( y z6 T) l' C
- }
複製代碼
) J, D" k) k. B* f7 k2 k. AMeLineFollowerArray.cpp% [1 [ P1 t/ |. P& u6 u
- #include "MeLineFollowerArray.h"
1 I# I) t* ?, V4 e+ @6 B$ H
/ O+ N# w- m$ o f" G- #ifdef ME_PORT_DEFINED* K# J+ p7 g3 N% }& N/ r
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
. V/ w3 c+ J" B! `$ B% Y9 _ - {" @9 P$ {4 s+ a, H7 n
! ^' A$ X( Z( l+ M6 |- }
! o1 r4 T8 _! q5 \ g( W - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)" {- M. E3 [/ m3 J: C6 K
- {
) d) I$ G5 J4 f0 z9 [ - _DataPin = mePort[port].s2;
- c/ X) c( {& U - pinMode(_DataPin, OUTPUT);
9 u* B: {' _3 E) G - digitalWrite(_DataPin, HIGH);! O$ M+ ?3 l O9 H+ o# H: S2 b
- }
' u# J0 f" S3 p- O8 B3 ^' y( Z - #else // ME_PORT_DEFINED
$ X/ Z8 X) z) N% p* f, T' ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
; }! u7 z4 A0 D" u - {
; G. o2 M( i6 Z2 M - _DataPin = pin;
8 _8 c0 W: V8 ]. ?% K# g3 v - pinMode(_DataPin, OUTPUT); 5 \5 J4 m5 b6 x6 A
- digitalWrite(_DataPin, HIGH);. e$ Q2 B1 ^7 R, }
- }
+ e2 u. |' | R9 V: ?# z. A1 Y' k; O5 I - #endif // ME_PORT_DEFINED- T( B, g" b2 X: c1 g+ F# _
- 4 h2 c" X3 p! Q
- & Y$ F* n; I7 D+ o; u# ]' }
- void MeLineFollowerArray::setpin(uint8_t pin)
$ @) b- K- \# h5 G- k! | - {- @( W l$ C& Z4 s0 f3 v; N7 D
- _DataPin = pin;
0 \" u; ~4 G! s# t9 b - pinMode(_DataPin, OUTPUT); 1 \. V' k, ~# d6 D5 j
- digitalWrite(_DataPin, HIGH);8 K5 \; K, Q. K4 C4 n
; p9 J8 S/ Y e+ a* d- #ifdef ME_PORT_DEFINED3 e# `* F0 C9 H% B& _
- s2 = pin;
5 O m: Y# O- j - #endif* w4 ^$ b1 X% T; K3 k' c
- }
. B, [$ h- @' |3 F
# E7 S! L, m( N" b- uint8_t MeLineFollowerArray::getValue()3 b ?! O$ f" ~+ `6 j/ V) Y
- {' O9 T2 s$ n0 l
- uint32_t LOW_level_read_time;
6 Y1 B) U7 f4 Y1 L - uint32_t HIGH_level_read_time;. c: W4 x9 q9 |. e0 l( d. L! z; ?( u
- uint32_t time_out_flag;
# k0 f3 u6 U& X' u: Y8 h9 C, K - uint8_t Sensor_Data[3];
$ y% Q8 |) Z9 ~7 f! @ - static uint8_t old_data = 0xff;
' ]) }. ]% ~" @
6 @% K K9 X7 i. t2 V6 Y3 p- pinMode(_DataPin, OUTPUT);! S' l8 q' ~: i# ^$ `+ C4 I3 u
- digitalWrite(_DataPin, LOW);
: }+ e' _1 M$ E; B; _2 c | - delayMicroseconds(980);* W! i: `9 N; X1 Q4 z, C+ C
- digitalWrite(_DataPin, HIGH);9 r0 S: x. R2 u% Q7 ?
; F9 |+ ]5 F0 M3 _4 O- pinMode(_DataPin, INPUT_PULLUP);8 ]0 Z9 r2 [# w1 l- ~
- delayMicroseconds(10);
6 z) ^( H( H6 E9 z2 s - " M) a/ ?! n4 e* I( m$ g) E0 s
- time_out_flag = millis();6 d4 d' \8 }4 W* ?4 a& f, a! e: ~+ o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 d' r4 P9 f, ^& Q- l9 a. M$ k
* ]: _4 P. n' Y8 k- LOW_level_read_time = micros();4 N$ [- |1 o; m5 Q+ F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 S. K4 O' M& v, N) n Z1 t
- {
# E! A# o1 l: T, ?2 y - return 0xff;8 g$ r! W% C7 {* s
- }
$ n& i3 O5 }+ N3 v, g" L ?+ ^
# l& A. [) s0 M5 h& u. J0 w- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" M- [! \7 W _* Z$ G
% n! C; F" u& M. f3 S- HIGH_level_read_time = micros();
# Y K4 Q/ F/ W$ R9 N* ~1 T0 H0 ^& n - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
/ W; @1 ?2 d* p9 T4 B9 D* C4 [! h - * |) M" Y# j3 d5 Q/ e1 [) B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* a) {" g# ?3 _5 i
- {
0 v$ u* P. c; K - return 0xff;* V8 D8 C- K. O0 I# U
- }
# \' R/ ^4 N& ?4 i
J. t) i3 V8 W- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))# w' d+ w+ p! V/ p0 i6 A! X) ~
- {
6 j8 _& ]) f0 i1 K - return 0xff;- X5 w( A4 Y3 C: h
- } ]1 N1 T* B1 f% D4 C# G
- 8 S% W9 v: u# A
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 g: d* g# F' {
- LOW_level_read_time = micros();
: a7 V4 z, p& `" C, |9 f4 d - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
8 p% b5 A8 S. }7 c$ P0 K+ T
/ q7 u+ r+ ^' v6 V( g9 L% t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; M4 b* P/ q$ t( ?9 P, f' m9 j
- {
* U* Q+ U1 K. O& U- {6 q# T9 C - return 0xff;6 C: R" D) X$ ] {4 C
- }" Z' K0 u+ J3 Y& c) V
& h+ X# z: Z# Y% e& a- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
. h! P; l+ p K. i1 C: H - {
% R2 I1 f9 s5 Q" z# K+ n - return 0xff;
, w! V6 e3 F8 \+ ~ - }' | b% F$ Z# X
- ' l3 ?, ~' P- X* d- \: W6 L
- for(uint8_t k=0; k<3; k++)
/ `: I3 G; T; Q' V0 u0 @3 l - {
/ ~# D9 T3 g3 T3 C5 { - Sensor_Data[k] = 0x00;
$ s: A& P. o2 W, l1 c - 4 }0 K7 u. K8 V0 U2 Y! s
- for(uint8_t i=0;i<8;i++)- ]( @; w5 s! H* F( q1 m W
- {
( z- P* z" Q2 s. L: S" j# b D - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 q9 K @8 z) X4 Y' M7 U1 G' ~7 ` - HIGH_level_read_time = micros();
+ L0 {+ v- A9 V1 Q& \- \' n - LOW_level_read_time = micros() - LOW_level_read_time;) ~1 S6 \( ], A4 `3 B% y! p1 M
$ h* h' y9 c8 z1 V- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )9 u2 l6 c! V5 K$ h6 {
- {
# \/ |# C# b- r: m G- s) v. E7 s; j; c - return 0xff;
9 q5 e* ?# h2 Q - }5 _& B! A4 G# @
# `: Q+ t; W2 V9 Y1 i# _- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 I) q2 s- t0 I; s6 a6 n" z/ h/ S
- LOW_level_read_time = micros();3 n3 G) e* p1 f2 E. G6 Z; H* ]
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, ]4 d+ Y9 @7 n- `2 A+ U* Q3 V0 V& Z - 7 H9 X8 c Z/ D9 [( z
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 ~4 A& c! R+ L! I' e9 W8 B* x
- {/ I* n: t2 M! c7 f
- Sensor_Data[k] |= (0x80 >> i);2 B7 ~. \4 G8 P: I6 }5 [6 N- v
- }
# N( ]8 K3 M9 M) ~4 t5 I. S4 b& @0 W7 x2 P- ] - else if(HIGH_level_read_time >= 100)
9 h: l k1 R$ \; D. s8 | - {& ^! F* c6 U6 W# h( M
- return 0xff;, d% ?) q5 F* o# h, c, R
- }$ F _0 F0 @1 {5 R N
- . j6 V6 H- Z5 N, Y" q+ ]
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
; `* b4 M5 P; F/ v" J3 E( U - {& C, Z# Z* U5 o* w- e
- return 0xff;( @% ]5 A9 K3 d& H8 {2 @
- }
# w" G( H8 T. w, H7 [ - }2 @: O* w& N4 m8 M' M5 w
- }
2 _0 C/ D* }$ G/ N' |! D
1 k/ `# ^' B4 d8 _& z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" e, p# n8 K$ y3 S
- HIGH_level_read_time = micros();) t0 e2 e: t; y4 y/ I% I
- LOW_level_read_time = micros() - LOW_level_read_time;
q" a" f1 e: L( O% k! D - 6 q1 H: X/ L0 S, i3 S4 N: `- b; Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
# u( w( } h$ e; S) n! |$ x - {
* m! @" s- N1 w) ?2 X - return 0xff;
- z. i4 p, A% b - }4 }) e3 q. U. j( `) ?; [' F
- 9 a9 A3 r: W" E8 D8 [" o, U
- pinMode(_DataPin, OUTPUT);
, O2 }6 H9 p# I$ U - digitalWrite(_DataPin, HIGH);# U; K i, H' v" |" R
- % Z( h0 V* S* y
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
' l0 t' X& O7 V% f6 L- p - {
" D7 R0 Y2 J4 i- c - old_data = Sensor_Data[0];# y* [/ S3 y {- k9 x
- return Sensor_Data[0];. C1 M6 Q# B- G2 e6 J
- }( M/ m0 X' A" U+ Y# _/ S
- else
. d9 \8 d) w. G - {6 o$ G, e6 E3 a, j1 U
- return old_data;
8 Y& D: B; y8 z0 r# r r* Y6 I - }! v- R3 m2 R ]( \ ~2 D* a
- }" I9 l. P: d/ \; w: F( D# w& ], [/ M
複製代碼 ) ]+ r1 K/ f! {$ A3 e8 k3 j
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ K" q4 o7 n/ L2 M0 `
; U' H! X; d; Z' o/ d; Z7 L
|
|