|
|
! _% Z: K' E2 u8 p) c
- #include <Arduino.h>
& ~4 r0 F. `, Y - #include <MeAuriga.h>- E. V% y+ o8 W2 p1 k+ H9 ]
- #include "MeLineFollowerArray.h"/ Z8 _: p( h& k ]. F2 R
- 7 v! W3 l) d1 t" N$ m O
- MeEncoderOnBoard Encoder_1(SLOT1);
% d9 K3 S6 J, I2 q! _7 n& I - MeEncoderOnBoard Encoder_2(SLOT2);, J2 f6 O$ N' o- f M! k
- MeLightSensor lightsensor_1(12);
7 U" B0 i5 w# N0 j: q% n/ ?7 e' @ - MeLightSensor lightsensor_2(11);
' s: v& }7 P) v8 f# P" j+ \) o/ m - MeBuzzer buzzer;3 N1 d& t" P8 F. m K! @2 W k
- MeLineFollowerArray linefollower(PORT_6);" {# \, Q) h$ @2 ?
- 6 } V8 E& z! Q. @* b2 n9 }
- #define Error1 1
2 i8 Z& N4 A+ {8 r - #define Error2 2! v/ W3 L( ~2 _
- #define Error3 3$ ]6 p& q, q0 G5 C3 D) g
- [1 X" Y- F( h! t* r
- #define Kp 15
4 F* a t3 M& S& o8 c. A% k( i - #define Ki 0.158 r. J, C" e! i7 n$ S% a+ S
- #define Kd 0.03" ^" z" O ^' g) Q" T7 f
+ @8 P/ {, F' ~$ i! p! y" `- uint8_t sData;
0 S+ l, I1 N. @2 [/ e9 z - uint8_t D1;
( `1 k: j0 h: z. v3 n# B- M ? - uint8_t D2;
( O4 W4 `" q4 @0 t5 o - uint8_t D3;
/ k9 v4 o u/ a: Y7 { - uint8_t D4;$ Z. u- F5 ?( @0 E7 A$ ^1 [8 h2 z
- uint8_t D5;/ Q* \9 g, q9 D5 _ _
- uint8_t D6;5 G( k% n$ O8 }- ^4 y, q
7 v: I/ q4 N% w- float previous_error = 0;
; Q0 @) D( n/ }& Z - float integral = 0;
- W$ o6 |0 n/ u4 u - float derivative = 0;5 |: r6 u# U. r' D9 e
- int Speed = 160;% z9 H$ K* T8 ^# W. T
- float output;7 [1 f- U, O+ X& M
8 K5 x9 |- {1 ~9 \5 v" ~7 v- byte Left;. O' V ?( g% b
- G e) z* ?3 u$ g- void setup() ; ?" ?0 w- M. j
- {: U8 [0 e/ X- r$ j9 |) d" v( x
- //Set PWM 8KHz ~2 T1 L& a$ G# j9 z3 x+ V( X1 K8 c
- TCCR1A = _BV(WGM10);
6 V* X2 J$ `4 j$ }8 C- D2 | - TCCR1B = _BV(CS11) | _BV(WGM12);7 r6 a, O# |2 G/ c
- TCCR2A = _BV(WGM21) | _BV(WGM20);5 T. T* d" @4 ?; T: }; u
- TCCR2B = _BV(CS21);
+ |* B Y* Y4 \: B5 N. e/ ^- o - Serial.begin(9600);$ E7 F6 j' k" _7 Z6 p. c$ y
- buzzer.setpin(45);8 A- a: e0 k% d* _& ?$ O
- }
2 x/ Q& Z% O' J' o: T; l- @2 [1 H - ( q! y' o6 K) g# e$ d0 O
- void loop()
/ H& _- i8 ^. @4 s - {
$ |1 v# [3 a) U9 J6 z - while(!((lightsensor_1.read()) < (10)));
! V# c+ u7 }% d, F7 O - buzzer.tone(1047, 500);! j2 L! r5 k! ` d
- delay(500);- G* C( Z+ X* o$ L/ K; C. E
- do- J; I' ~( s) C2 l6 ]+ J
- {
- F* W, o. e2 W2 t: Z0 O - int Err = getErr();9 A1 T; O* O% X1 }' _& C; L- l
- if(D1 == 1)
, B' b5 c& O# q: q. H2 B4 S - {4 F1 ^3 ~+ z+ s* r
- Left = 1;, v1 C6 U0 ]) D- \
- }
& j) m; p1 b( g* u ^0 W - if(D6 == 1)3 b. T, J) r! v1 y( f7 l
- {9 n6 \5 e3 {/ C3 l
- Left = 0;
& b7 }! O2 Y1 ~, F - }
: I+ N6 S2 T2 a3 H' {$ r - if(Err == 99)9 {) U. Z% }, Y
- {0 g \' H f* F# O- A8 i8 _
- if(Left == 1)! Q, q2 @8 ~& P2 s1 W. F# Z
- {
4 Y/ T5 U0 i6 q8 ^/ C - Speed -= 5;
: a5 O5 k- F5 A4 m3 c. I7 G+ s - moto(0,Speed);' d5 S* S' c2 c0 F3 X
- do
& a6 S3 y4 Y/ { - {
6 ]2 _2 D; h) ~1 ~% z - Err = getErr();' n6 ]3 w6 n* W' @0 O3 v
- }while((D1+D6) == 0);
' M- a2 d9 `& x. [* ~3 d - }. m* a" y- d9 E0 i, {' ~
- else
4 J. q( C- g7 v/ c }. i0 i! E - {
$ w1 Q* b; S/ ] K+ E4 m/ P - Speed -= 5;& B6 w7 _" Y y" g* T, r2 W, P
- moto(Speed,0);
# N v; z+ J2 ?. [" ^" U - do t- x& A/ f! i2 G/ K2 u: l
- {
: ]% G/ v7 W. W& B& s! I - Err = getErr();5 u- n) V" b5 p, e: F( d
- }while((D1+D6) == 0);1 g4 Q; d4 ~5 @. R
- }
) `' s; r) B* U2 `% o6 W - }
/ v4 ~4 a) q% Q' ~" x: X1 a - else
7 o" i# M6 x6 L9 j' | - {7 Z7 v1 @& T( D* @" b7 I: K0 o! G
- if((Speed < 160) & (Err < 2)) Speed+=1;
( o% X2 w* N1 q* { - if((Speed > 100) & (Err > 2)) Speed-=2; $ C0 c% C- E- \0 O: q$ I& Z6 U, Y
- integral = integral + Err;2 C* y; n7 n& {0 n# R: U
- derivative = Err - previous_error;4 O% k u, k f! T) r! k3 M& A6 T
- output = Kp*Err + Ki*integral + Kd*derivative;
, z. Q1 U" e* o7 D- W - moto(int(Speed-output),int(Speed+output));7 Y! r, X I6 v
- previous_error = Err;/ Z P" o. P' }
- }: K5 E: j3 R/ V" u$ L
- }while(!((lightsensor_2.read()) < (10)));
/ H8 N$ [, V5 d' B& `* M+ D* p - moto(0,0);
- c+ j2 Z' q! u' _; v - delay(500);
* o( v& G; ^1 ?$ r! p K - buzzer.tone(262, 500);* l& H4 X2 i. d9 L4 y9 E; R1 Y
- }
8 H, u! {. r1 f: Y: k8 r- i4 f - 1 {; q# X3 Z0 |) u6 c
- int getErr()( T' l" V: P! s& m* _" h
- { ) I; A2 h* _7 d7 _8 x/ l0 }! {$ n9 ~
- sData = linefollower.getValue();
( b! I% y. Q9 x7 r p7 H- W - D1 = ~(sData>>0)&1;; I. Y4 T- ]' \% i8 K
- D2 = ~(sData>>1)&1;
- L8 Y( R+ @% U' \- D; S - D3 = ~(sData>>2)&1;* k& n( X3 [4 ?" s2 b: [; F5 u
- D4 = ~(sData>>3)&1;
$ x6 K, t4 i+ f9 R, g5 o% Z - D5 = ~(sData>>4)&1;
# f* s3 _2 n7 O5 {# B/ a) u% I - D6 = ~(sData>>5)&1;: V& o6 Y g) B1 c4 [
- int downD = D1+D2+D3+D4+D5+D6;
4 w" A9 S$ s- t1 o2 ` - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: z8 b6 s' M4 j9 e0 ^# A - if(downD == 0)( F/ }$ I) l; E* f
- {
8 j1 h1 V# E+ G9 U - return 99; p4 q7 [4 `+ S$ l2 q
- }
( G) c+ j N& U5 n; \# E - else
5 n" w% L/ M1 i( b) y9 M. C - { ~6 e1 d& k% ?4 L0 O
- return upD/downD;2 h' J" |, ~, b0 j& |/ K/ M
- }1 R' O' \% p6 q% Q8 {9 K% U
- }# k1 W8 d5 Z/ U; O4 V9 P; ?
- / [" }2 J% }- b5 U
- void moto(int Speed_L,int Speed_R); F& h8 ?, i+ O8 J
- {
8 ?+ i' P8 m5 N8 d# W% J. ] - Encoder_2.setMotorPwm(Speed_L);7 E, L0 d; S0 r& Z: }/ j" R( J
- Encoder_1.setMotorPwm(-Speed_R);) {9 a; c- P. \, j' N$ s! w( E9 D
- }
複製代碼
' v b" E4 L/ Q# M$ \MeLineFollowerArray.cpp& ^; N) I8 K( a$ i; |3 F2 H/ F
- #include "MeLineFollowerArray.h"
1 G$ k8 H6 `/ t6 g1 P7 L6 k/ _
9 O2 T, U( @9 B+ l1 t% z- #ifdef ME_PORT_DEFINED
6 O' h' N7 |* {5 i V* f, w - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 x8 O# l( g7 Q0 C7 T: [6 q
- {" T) l" [) d" _# F A+ I% ?/ o
- ! [3 k1 K$ Z, d* a
- }
: ]. } Z; i/ i. R: o4 s0 f: c - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
& v2 V2 Q: h! H/ r/ _ - {; l! i5 T3 h" ~. a5 q7 n( B
- _DataPin = mePort[port].s2;
! e5 L- L# a+ S: P - pinMode(_DataPin, OUTPUT); + Z' p) \/ x9 w. `9 r( N
- digitalWrite(_DataPin, HIGH);
2 X) Q. @5 g3 b9 W: l* A - }# B: {; ?& }2 q: P0 t2 P
- #else // ME_PORT_DEFINED) o) d. U$ ^3 `: n
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)& x6 f& F; C: f$ t) R
- {1 z D$ R, b& [4 _6 o; ~
- _DataPin = pin;
4 m$ s1 s T# U7 e" g - pinMode(_DataPin, OUTPUT); 4 W: {2 ^, B8 E$ T( i
- digitalWrite(_DataPin, HIGH);
9 k" y* e3 g2 P - }' _) \% F5 _8 W& ~2 k
- #endif // ME_PORT_DEFINED
2 c4 V% Y+ j; C& J7 [ i
; ?8 T/ g7 O8 O
+ ^ k. t9 N2 D5 x6 r- void MeLineFollowerArray::setpin(uint8_t pin)
/ ~$ t9 \2 \# c ?+ a; Z - {
5 x5 h, ]6 d6 A - _DataPin = pin;
( R: y6 | o: G5 c" d - pinMode(_DataPin, OUTPUT);
* p1 p! y G# r, X G. O- C# ]. f - digitalWrite(_DataPin, HIGH);& N3 e0 C. g: d9 ^4 U5 Y& A
" z8 S+ a$ \$ e t- #ifdef ME_PORT_DEFINED( H' d& }% m0 o0 g
- s2 = pin;
- b; ^) G5 v* r - #endif
/ m, @" N* F7 L; f; B9 j+ ^ - }
: k% b9 T+ J, ~' s. T* x - / x; T$ `; A( j. R& c6 i
- uint8_t MeLineFollowerArray::getValue()
: q7 |+ Z4 s( S% I; r0 P - {
' e; z: f7 p% p - uint32_t LOW_level_read_time;
$ s+ C4 W7 m* g - uint32_t HIGH_level_read_time;
8 g/ y) z/ Y$ D# r - uint32_t time_out_flag;( b2 `" T7 R5 U& Y2 E
- uint8_t Sensor_Data[3];; _- _% ^& v, x9 x b) r! \+ F
- static uint8_t old_data = 0xff;
5 Q( r7 m/ {$ V, H5 W0 i1 i
. U' P& E; M% W8 g- pinMode(_DataPin, OUTPUT);
8 O# g! N) U; u3 C( U - digitalWrite(_DataPin, LOW);
n& f9 s1 W/ ]% ^6 M4 G5 N1 l$ ]) ^ - delayMicroseconds(980);
- W1 f% k% l, O - digitalWrite(_DataPin, HIGH);( q0 [# I% i, ]6 K3 O
3 o+ C; j, W; o3 v9 Q# y; Z2 ^- pinMode(_DataPin, INPUT_PULLUP);
2 Z8 p' A0 J0 ~9 d# o6 m% g - delayMicroseconds(10);
4 ?9 w2 ^) T9 `' n/ A; R8 ^7 i A - ' b+ M' Z6 p$ \; h: O: V \, H
- time_out_flag = millis();: D, Y2 K6 r' G
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- v7 i& n$ _0 J' s8 y - 7 C) R+ \5 D. ^7 }% [9 s* R, ?
- LOW_level_read_time = micros();
/ L0 A! f4 b! }0 ? - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! E5 l [- i3 T: v3 O+ q- K# z/ {
- {4 K" W& h$ l: h
- return 0xff;! h( i# f! V; d0 Y7 R0 \! p
- }" Z1 A; N- D1 Q8 Q" k \# z" K
+ F9 v5 ^/ T+ A- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 K7 b/ v+ D. {
- 4 c" h2 ~* i+ t) ^& g, t
- HIGH_level_read_time = micros();
' |, u2 _/ }0 f6 s - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; H. E( `/ {9 y* ^
?5 H% h; a# I# \- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: b" Z4 N4 C5 g5 l- e
- {
9 T3 B1 S; H6 s# `' Z - return 0xff;. X: y9 V+ `4 e6 C
- }( j+ K2 [( ^# T4 H! l2 X- P
- a8 L' B1 S5 N
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))8 U' a9 |; ?) V8 i" W2 k: d' e
- {
1 o3 y, _8 B0 B( e - return 0xff;4 \: z' e+ r1 p' g; l- u7 b' p
- }: d# {9 o2 j, X8 S* f9 O2 F5 m
7 L( o5 x& W* ]- O1 A- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* T6 @# v, ^$ U# y" c, t" x
- LOW_level_read_time = micros();5 `' m8 u3 {4 S; f ?2 c2 e$ C
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level9 S% _- c/ q0 X/ F6 [
- & _- p0 A5 [! N. E ^/ S
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. y, [7 a: |' T# E2 i" ^3 C6 n - {* O$ V. G( h q( g" c
- return 0xff;- C$ P+ Z) e1 P* z6 J$ d8 K5 f( q
- }. t! N/ m5 l: d+ ^
\+ f. q( q7 N- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))* l0 f) O0 u0 a1 d# y4 E
- {6 b" G; }! x/ j. P# L9 ^: V
- return 0xff;
+ w: l4 [/ x' }. D' b, I - }
% M+ h1 P- O% }) x
$ O) {! V2 g" O+ F( g- for(uint8_t k=0; k<3; k++)
+ b. V5 q* l& E+ I - {
]2 ^ Z: D0 u! D- Q( ? - Sensor_Data[k] = 0x00;
2 a- t- G+ V. {* I: g6 u - ' }2 W& O# f2 w; R- ^6 `
- for(uint8_t i=0;i<8;i++)/ X& U. t% t+ d
- {
( ]0 H; i2 k" } X G8 X2 ~ - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level9 Y7 V# a9 ^* s- Y) u
- HIGH_level_read_time = micros();# Y4 F" W: p7 d1 U
- LOW_level_read_time = micros() - LOW_level_read_time;
; F2 L5 r& R: h2 |4 A+ Y
5 v7 \/ ]1 v8 J7 _- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% k5 z) ]. j4 M8 a. H$ _* z' V
- {
R8 V! S! O& S: b2 m! W' v! J: K - return 0xff;
: D/ ` x7 h, ~! \ - }* U$ d( N: V' D6 \6 F# X
3 Z! H" h9 J8 E# \' Z7 q; F* G- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- a2 O* Z9 ~ D# x
- LOW_level_read_time = micros();: ]2 U, ^4 O* B( ~0 M E; s
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level! r' u! f1 Q6 c6 ?7 H
- 5 ]2 h0 k# ?8 g) ]- k( R. r
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
3 D z& y% r: j& j& `. x6 r% ?6 Y - {
( ^% Z& r4 o( k( z! c4 R y' y4 _1 I - Sensor_Data[k] |= (0x80 >> i);, V: l; l Q) E
- }
# ~8 K) {) Y* J `$ R - else if(HIGH_level_read_time >= 100)3 @! E. V8 \0 n# a K
- {5 y) [2 t+ A& H: L1 P( h+ ]! ~
- return 0xff;' [' a; c1 D4 @3 u* Z
- }! Q" o/ p! W) z8 P% x/ U/ `% D
1 |3 O7 R' }: k4 q9 `- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)* Z \5 b a" a9 D* U, E2 y9 X
- {
6 Z3 q L4 V0 y* E d - return 0xff;$ A: a9 E7 \' j! H
- }- ~0 m$ L* l+ `' n6 z: |! G4 d) \
- }8 n7 T7 Y# r2 X7 a% X/ J3 P, x& b
- }
8 {, M: j$ @9 u& i - & B6 k7 q% z! B m( j5 \
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level2 ~8 u3 I8 G) {
- HIGH_level_read_time = micros();
* q8 A l# R4 s( o" g - LOW_level_read_time = micros() - LOW_level_read_time;
! ?- j1 c: \# O( b/ o2 G( m - + E: Y% b2 D( L, h$ R1 l
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) x- q. g @8 {/ H- ?7 ^8 S- L - {
, H0 C5 O$ F$ p - return 0xff;6 x- y9 I9 g) I2 J
- }* {' U$ G+ k6 _ L1 Y7 f
3 i& \5 \7 y# `& o6 C* G- pinMode(_DataPin, OUTPUT);9 l" P; a" f; A. z' c2 t8 B
- digitalWrite(_DataPin, HIGH);
F' p' y, ]; V1 ~% u9 n* ?
$ b9 J8 i( x/ o( Q# C# `* T" j- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
3 T: T, t& F3 m1 H/ I6 o' F( i - {
, v1 C0 x2 Y' L/ z- ` - old_data = Sensor_Data[0];0 K- q# f! }$ N1 u9 `( g0 C
- return Sensor_Data[0];
# C. c" h* K# x6 M - }. y. a2 n y2 l" U
- else
6 g) y# H/ R1 h1 ]/ D8 I - {7 |2 E+ f1 X M) J" _. s# d
- return old_data;
3 r+ y- q2 q2 t/ q" e - }# V Q5 S) m$ O3 m+ v
- }
# ^% p1 W0 e) R Y: V ^( c
複製代碼
0 e$ E; @2 a' c, EMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
s6 B# `: A4 R5 b/ ?/ W
+ n! O1 S8 S, C+ ]* @
|
|