|
|
0 K4 v/ J; x0 |- #include <Arduino.h>+ X, A! O0 e" X1 `( p' Y; z+ K/ u
- #include <MeAuriga.h>
' M7 {0 n8 T- c9 L( w, P- R/ N - #include "MeLineFollowerArray.h"
. M0 Y, b! f8 j. z1 h
4 {0 W- u) t9 `5 n ^ k- MeEncoderOnBoard Encoder_1(SLOT1);
. i+ i$ Z2 o$ u+ @+ m - MeEncoderOnBoard Encoder_2(SLOT2);
/ f0 D$ n, ^* { - MeLightSensor lightsensor_1(12);! g# ~6 `. X' o0 g, n
- MeLightSensor lightsensor_2(11);
0 O/ E8 e' ?; v% L - MeBuzzer buzzer;6 M5 h1 E* d. X* |
- MeLineFollowerArray linefollower(PORT_6);
/ D8 x, @" f' @) f9 e7 m! b
3 d3 y6 y% Z& i0 W: W# [; Z- #define Error1 1$ J, ~: U8 _% g' ~3 B- W. `
- #define Error2 2& V+ ^- c- K- i2 B9 P) ~& A" z
- #define Error3 3
0 s, C, u+ e4 @) ^# ?. Q: f - 7 P9 _' }2 r; K- }4 F$ @
- #define Kp 15
1 c0 w; L+ h9 H0 I/ h5 c2 O& _ - #define Ki 0.15. S% B" [8 @, F9 z( v4 g0 Q
- #define Kd 0.03, I3 z& Q9 e! C6 u, d. V
2 u! w; d% P6 b& n8 [- uint8_t sData;
2 J: F& m; C* v) Q7 \' O - uint8_t D1;) M3 s1 E( T) Z0 J
- uint8_t D2;
, H4 s2 i9 `) i+ m - uint8_t D3;$ m# i7 A ]8 U
- uint8_t D4;+ z# z/ O8 a- G/ _: J- Z
- uint8_t D5;1 F9 f% `- C0 O8 b
- uint8_t D6;7 v, r* n. K4 d' E2 e
- * @/ o3 d2 T) T1 \. X* I
- float previous_error = 0;% z8 |7 k5 a( r8 [/ L$ O
- float integral = 0;
4 x1 n/ r% p: S/ K# x" R - float derivative = 0;
" G9 `& @9 c/ j3 |& [9 A - int Speed = 160;. {6 A+ Y& s7 l( H
- float output;, N" C4 h# {8 t. j* i: n
- 6 j8 ?+ m# p* v! v1 G9 P: {6 h; l& V
- byte Left;
+ r! @/ `4 {, d - & @- p, H+ ]2 v& a) j3 d" t1 w( b
- void setup()
`0 w( u6 Y( @. I6 v1 l - {$ s& w$ _% A% V8 x; N' u7 E
- //Set PWM 8KHz
5 [ m0 _# C* M2 V" y8 c1 f - TCCR1A = _BV(WGM10);4 H- t9 k5 ]% X6 k
- TCCR1B = _BV(CS11) | _BV(WGM12);0 u Z( q2 K+ m+ t( q
- TCCR2A = _BV(WGM21) | _BV(WGM20);
: C$ o4 b+ ^5 O) l) ^$ _ - TCCR2B = _BV(CS21);
0 d0 P. m0 O! ]6 g3 s - Serial.begin(9600);) _9 B+ H6 C1 @8 D: \9 Y _+ J
- buzzer.setpin(45);) r. s: o5 Q, l- M
- }
' _3 ]9 }4 ]9 F9 ~& S- \- Z
4 g, f* z( W& a& E- void loop()( T- `( J% y* N O
- {0 k- R" h X0 c" V* o
- while(!((lightsensor_1.read()) < (10)));. t( u4 T, a9 ~
- buzzer.tone(1047, 500);
8 U2 s7 \+ [; u# x1 T - delay(500);
- q! A3 I7 q( s3 d! R - do
+ y: q2 C/ Q8 i. S$ z - {
% Y( ?% W, b+ ` m0 K - int Err = getErr();
" x" {- Q! p4 L' ^! f; ]8 } - if(D1 == 1)
; m4 ^) M% z! X6 F" A2 A1 h. f - {$ F% C6 [$ t- E; W% u9 }+ i2 ~
- Left = 1;/ a, Y4 C3 C2 p
- }
1 G% b/ M3 }' _/ O" t" t6 q+ Z - if(D6 == 1)$ x1 I, W- _6 r- c) L
- {
- z R `" |2 h! q2 w( w - Left = 0;
0 b5 J. i0 N% s5 R3 |2 b0 f& U/ t - }
Z/ V4 k& ?9 w1 l6 Z7 b& @) c - if(Err == 99)/ s! _7 S _. [, e) [9 S
- {9 q7 S8 X. l' ?3 O: d( G8 i6 t7 [6 N5 `
- if(Left == 1)3 W/ M3 @ A1 o. N# c3 B: {, s$ a. Q, o
- {
& h# F5 @- e$ z3 o* d - Speed -= 5;, k1 T, W8 V7 [& s# A; A0 \! b
- moto(0,Speed);# A# d9 Y/ k, i& B7 ]" V1 i
- do
- m. t( }3 E% F! r: w" C6 w - {' p: t* w3 S w6 L# t& D( M* A
- Err = getErr();# g) V( j' K2 r
- }while((D1+D6) == 0);
8 S3 Z+ s) b* V- n' y0 V! ^ - }
3 N, {0 @) p0 F - else
( K) }% ^0 \+ ~5 j - {
& `7 s* O3 G( w% J& B: H - Speed -= 5;
( D. m( F4 Z; A - moto(Speed,0);
8 d+ k; j. W0 b2 o# q, w! k+ } - do) }& U" d% k) G3 f8 o) o# d& e- t
- {9 A5 u$ O5 K) P( r$ k+ C
- Err = getErr();! V7 j/ D% ]( d f- q$ u
- }while((D1+D6) == 0);
0 R2 r& [' R, \! Y! |$ V( ~ - }
5 E, g; J( t: q0 G- l - }4 \6 S) T+ \, e% ~) J
- else
* H1 n* a/ S9 x/ D! M - {6 ~0 D; q0 R0 N" j+ {" t' `
- if((Speed < 160) & (Err < 2)) Speed+=1; , K1 r& ?1 Q, E* r- S+ Y
- if((Speed > 100) & (Err > 2)) Speed-=2; # o+ l) i; _) @1 C1 A, u
- integral = integral + Err;
, u1 S! c" z3 I* O/ k9 Y' q1 q$ O - derivative = Err - previous_error;
$ y, w" m/ K: Q3 q" l - output = Kp*Err + Ki*integral + Kd*derivative;* d# d% P8 x9 a6 v
- moto(int(Speed-output),int(Speed+output));
- X/ b5 s0 B' i5 V - previous_error = Err;8 v8 F9 | @ z# t8 M0 F' I/ U$ M
- }
4 @5 h0 \0 g, ^9 G E - }while(!((lightsensor_2.read()) < (10)));# C6 ^9 D2 p4 F$ z9 R
- moto(0,0);
: W [& }5 M3 M' ]( x! A0 m4 F - delay(500);; J& r ~4 a. d! L; X
- buzzer.tone(262, 500);
+ A0 s4 |& C# V/ e6 z& S - }
' @' r8 V, p ]/ Q - 5 M3 ~& E+ @# j! T, m$ h
- int getErr() t! y3 H v' p4 N; D
- {
) u4 U E! c, _" b6 C# @+ M - sData = linefollower.getValue();
4 q5 U6 X. i1 k. V - D1 = ~(sData>>0)&1;
5 j2 `& Z ]# a) O: O5 ?& o - D2 = ~(sData>>1)&1;
0 Q$ ?% M" x( i0 W; B' p$ ~ - D3 = ~(sData>>2)&1;% C3 J* R1 W: w
- D4 = ~(sData>>3)&1;3 d4 I" M6 s0 @
- D5 = ~(sData>>4)&1;
3 G4 m% q% t2 a1 [! r9 U$ c1 b4 Y0 q - D6 = ~(sData>>5)&1;
' ]8 w; V" {- T/ I# w" i% O2 Q3 P - int downD = D1+D2+D3+D4+D5+D6;
* v7 y" r2 F+ X: i; j# M - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);0 U& W% h, O$ r3 M9 c6 b; P
- if(downD == 0)2 \/ U y- z* E" H% f" Y
- {* E9 X- \" D2 N
- return 99;
6 L- }6 n3 Z3 v9 i9 l7 [ - }/ E/ r' g4 Y2 ^, h. F
- else
' I8 x. O! {% k) c( E) h - {
% i f+ \% ]$ `+ U. A: r8 C# a. c5 b - return upD/downD;
% ]7 n( d" a; b+ H# q! h3 @3 H- G1 m - }) I# H' }8 @) s& _$ z b6 {; `! v w
- }
& S' e5 h% ^/ x/ J5 @
* _/ B4 k" k4 y" x/ q/ ~( Q- void moto(int Speed_L,int Speed_R)- {9 ?' [6 x$ Z, A; O$ C
- {
7 E7 N& o: j. F - Encoder_2.setMotorPwm(Speed_L);( W2 ?% Q! B3 ?. S! ?4 P9 Z! z5 |
- Encoder_1.setMotorPwm(-Speed_R);2 d5 g% W/ x8 f/ G( r1 w; T# W, I
- }
複製代碼
) x# y% x: ^' n- J1 CMeLineFollowerArray.cpp
* I7 i+ N9 d v2 j" ]0 l0 F- #include "MeLineFollowerArray.h"1 _+ v( W) ?% w
- ! \ R2 f/ G8 W9 k$ V- E
- #ifdef ME_PORT_DEFINED. M7 C) J) y6 Q! p. a
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' O5 ?$ K7 k' R7 d; r+ s& Q9 F
- {, W2 L* I6 E, s8 \4 v
- 3 i% [; w# {4 h z' j/ P
- }
6 j1 e; d4 s* \- K - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
( b; m0 ^% V0 h5 S1 p! z% Y - {
* `( w% W2 ~' e# J Z& {6 m - _DataPin = mePort[port].s2;; u; Y/ \5 g$ q: u+ o
- pinMode(_DataPin, OUTPUT);
. p7 r9 I( y1 b9 Q. K* j - digitalWrite(_DataPin, HIGH);
8 ?' o1 [' D! p' O# `6 z - }+ H$ t! `, u+ r4 b$ R9 `" J
- #else // ME_PORT_DEFINED* e) R: W: o. @( J i3 e
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 e7 O8 W' S( E
- {5 k0 P, F" t, y! q/ m L/ W" A
- _DataPin = pin;& i) h9 e$ o/ t+ o% W
- pinMode(_DataPin, OUTPUT);
" j' R( ^9 ^ }; X i - digitalWrite(_DataPin, HIGH);
$ |( Y3 R7 Z0 l3 F - }
# K: A7 z! i& m+ U q - #endif // ME_PORT_DEFINED/ Y+ t" V8 M0 t6 W8 q9 Q" H% [
- 5 e: z- W3 ~% H% ]. I3 _5 d
% \6 O5 [& n1 H, N) E- void MeLineFollowerArray::setpin(uint8_t pin): I, |7 g( Q5 P: w$ _4 S
- {% w8 \$ _' F8 w$ f
- _DataPin = pin;8 y) g1 ]7 J" m& I* D8 T% a7 @! p( G
- pinMode(_DataPin, OUTPUT); * M/ A' y; N% Y. [. I% N
- digitalWrite(_DataPin, HIGH);/ o! \) E2 z% y* h% l/ ~% H$ T, Z
8 i6 ]. j, z' P& K0 {- #ifdef ME_PORT_DEFINED$ h/ J# g- D8 ~: z% [
- s2 = pin;( u( A3 r1 f# j2 b) S: E* D$ O) J
- #endif
! {4 J$ X" Z$ \) {0 W! q" C; J- B - }$ Z+ K b6 A' o. r6 X
4 [4 {0 K& A2 T- uint8_t MeLineFollowerArray::getValue(); R1 U' l4 G$ ?6 b
- {
) X4 n$ ?; c! f) A) A! [2 p8 ? - uint32_t LOW_level_read_time;0 |9 _# ~( r& G4 v# w! e7 ^
- uint32_t HIGH_level_read_time;( w4 k0 t: d! [. {
- uint32_t time_out_flag;
! X1 @5 t& L! a8 @& \7 ? - uint8_t Sensor_Data[3];, C9 R, n$ q0 R) ?! L; C9 h
- static uint8_t old_data = 0xff;
$ v8 e, z0 o+ N7 L6 R
. e" C' Q6 J0 R# W- A: {7 l- pinMode(_DataPin, OUTPUT);
) y+ |9 _# t3 w, ^! y3 f& A( | - digitalWrite(_DataPin, LOW);2 e9 X- J+ e; Q. ^" B
- delayMicroseconds(980);
6 r6 U. `. l* }* } - digitalWrite(_DataPin, HIGH);
7 J4 E" Q2 p; p' A0 b
$ H: q7 ^2 l3 o3 y* w* E4 r3 q- pinMode(_DataPin, INPUT_PULLUP);
' c7 G, x/ V) ^1 q - delayMicroseconds(10);
6 @' l2 _* y* V B - # N2 t' y K- |( M. a
- time_out_flag = millis();
, x$ l5 Y( @) F3 L - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: V6 `, ]( u5 ]4 E. b$ u
6 ]% I0 A2 q! d* B% [% r7 f( ^: z- LOW_level_read_time = micros();
2 W6 y& f8 @! Z' g - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
# |- R; P& x$ z, o+ g% F& h M - {
# K- o/ C( z( n4 T; y - return 0xff;
2 A! N- [) E3 l - }, j. ?+ p; m, X6 F# E
- - _9 G9 X- v9 ]9 F- G1 [1 `; l
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 k. S6 w7 Y, ~, E/ O
% v& N! N1 u8 w3 |% z1 N- HIGH_level_read_time = micros();8 @1 M) n' Z7 d" \
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level/ i% F, A. [3 v: G1 F# t
8 b' Z! B) P4 F* Q5 d# ^' x- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 i1 T V9 C6 l# z: T4 h8 q0 O o
- {
( c& `6 S/ s8 M: t - return 0xff;
; r" [ I7 t5 y! Z/ u9 y, f/ h - }
$ H. v) ^ z; K; U/ E/ ?( z - 6 o% B/ L) O9 w/ N6 @7 A3 } j; e6 Y
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))+ k- u; X" ~) B
- {9 p+ y' y" o4 M4 ?% W
- return 0xff;( v! a# }% {, w' w
- }
% a" x: c g: l1 H( p3 j6 k/ N - % Q% Y) S8 T* P2 w" o) q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. o" l3 T* b$ l- [ - LOW_level_read_time = micros();
- |( C' S4 y& x& Z - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level: P4 K; S$ Y4 K/ q g$ {+ y
7 p3 H- \2 @4 u- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 x( f- b) Q7 _ - {
3 M6 a) X( g! ^- y4 W5 u/ S5 c - return 0xff;
. g7 y: H. E; | - }
7 Z9 _1 P; H2 K3 V" v2 V - % T' ]1 C8 q6 ^0 p
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
( e* K& c- n+ Y - {* b3 w" A3 X' B: r
- return 0xff;
) n0 x5 b0 }5 e& {. { - }
0 S6 }0 ~6 |7 ?$ G
7 }* a: @# H/ u: H- W- for(uint8_t k=0; k<3; k++)
5 h: i5 J" z) M& _9 l- N- Q+ ]1 H - {1 V$ e6 Q# F- a. n5 r7 W
- Sensor_Data[k] = 0x00;
8 G0 |+ n3 v2 q ?1 s
6 z, t! _7 f$ k: @% I- for(uint8_t i=0;i<8;i++)
& V% i+ ^) w" p# g) x" y - {
2 m$ S( W4 K7 I: i! F: j - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level- D6 ?; s) b5 ]1 V" S
- HIGH_level_read_time = micros();' |! J% R, N- [# M0 I
- LOW_level_read_time = micros() - LOW_level_read_time;
. d0 D4 h% O/ i) U
( x) \; U2 x& I$ I' U4 t/ \- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) s) a0 X2 Z$ F0 w% E" K
- {
. D. S) U' {5 U, z* f - return 0xff;
9 ]2 O- ~6 W* ~9 C' `$ S9 H* \ - }8 O$ Q' E ?3 h) p5 x
- 0 y# J; z0 V! O) Y5 Y6 `+ r1 H
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
& i4 L! O! o5 y) t$ ~/ @9 [ - LOW_level_read_time = micros();4 k! A R$ K; `$ n) N
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level$ F, ?# |9 c2 Z- X9 J
- ' Z+ d# B% B- m+ x
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
d1 ?1 w+ T. ^/ L* E+ S2 n - {5 A% b. ^. C, T4 q5 d
- Sensor_Data[k] |= (0x80 >> i);
; H) L/ n) R2 j- q J: I! r3 p P - }/ x+ y/ a3 ?$ b
- else if(HIGH_level_read_time >= 100)- \" ~% p: Z4 m$ N4 e) ?7 o" z
- {
$ l4 u; {/ [# p* u/ Q - return 0xff;
: @" \6 X. @3 y' w! h - }( t& D# `# D* l# t$ @+ p8 s
$ A) X# O! B2 |1 r2 y& O+ N, C- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
6 \+ v9 f, C/ x' C: h/ ] - {
, @- p( K1 ?: n' x3 W% g' _& D" [ - return 0xff;
0 W e: S( t' `$ ^/ J - } o# U L+ M. c! J5 [
- }
. d5 w% F; v8 ? - }, N+ x2 G2 C! N( o$ M0 O
- * L; f! J5 s& c% F& S
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level# j7 T+ S M# I, J5 Z2 K2 D
- HIGH_level_read_time = micros();
. X3 m+ L$ U) F! d" y6 ?' T - LOW_level_read_time = micros() - LOW_level_read_time;
+ c9 ?( |2 h) [
! R3 l0 k# E# W3 I4 d$ Y; b- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 U( u# e- o6 r9 M8 A( z3 k - {8 T$ v' X* j. V
- return 0xff;
/ l2 Z, p4 k( C0 s5 M% h+ r& Q - }
6 n" Z' a$ q2 N - 6 i$ W- m- F- B7 H& b1 y$ W& E
- pinMode(_DataPin, OUTPUT); K6 U) F9 y7 s# Y/ N
- digitalWrite(_DataPin, HIGH);$ H) Z/ V9 h2 A- q6 u
- 4 w+ b7 d! `6 U- n E
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
' g- n% _+ F* H" v% l% ? - {
) [; Q0 ?3 i( z5 {4 ~: v# N - old_data = Sensor_Data[0];
. H7 w8 S, g. O5 V# Y- L& s - return Sensor_Data[0];% h& ^1 m O7 W+ v ?1 I* I
- }
$ f4 {9 I9 `; ]( C% x3 g& b - else
& s" k0 H- l# E2 V' O* J. k; a" K6 E - {4 B" q) \ c+ a9 [# K E
- return old_data;
6 x: t& l: J! _9 B5 E/ i - }8 F! i, ^9 h1 M) i; m2 H$ ^
- }
7 b1 s0 y' {* o" H
複製代碼 ! ^( c( j7 \! g5 g
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ }/ i; A% A+ ~9 M* ]0 J/ V. F1 C" c0 D8 r9 c4 Q4 `3 m
|
|