|
/ X0 v* a$ C+ `% b
- #include <Arduino.h>
/ s. D& B( ?+ \$ k5 t6 f9 z - #include <MeAuriga.h>
) G V# ? B/ n2 [( C g) k1 F - #include "MeLineFollowerArray.h"
% D5 ~7 m$ c$ } - $ r( H. f, |' X3 c& l, H5 Y
- MeEncoderOnBoard Encoder_1(SLOT1);' E6 t) M' Y6 K! J c, m
- MeEncoderOnBoard Encoder_2(SLOT2);
6 X2 A# V( b( T+ e, B/ @3 N - MeLightSensor lightsensor_1(12);! o% g6 H' Y/ A* G7 }7 h; C5 _
- MeLightSensor lightsensor_2(11);
( ?; K) v f+ V2 I - MeBuzzer buzzer;/ b& |: l, ]8 N# @' f9 N+ T4 h W
- MeLineFollowerArray linefollower(PORT_6);
7 z3 Q: s3 G! R x2 J. t2 ~ - 7 X; ?' Q9 K0 _& z( r
- #define Error1 1
8 O: u0 c3 ^9 D$ ]2 V - #define Error2 2
2 _* ^- i7 G& n8 v- T - #define Error3 3
! i8 Z3 O: V* _( R: |9 ]3 {6 ]1 h0 d$ |% W K
* O# ]% @0 g+ Q; V4 J- #define Kp 15
. p4 v3 y! `0 w - #define Ki 0.15. [* _, A' z% x& Q
- #define Kd 0.03
# V# x6 \7 w( W" [
" s0 D0 K2 X% {1 K. m- uint8_t sData;
& f) k) p! h' n - uint8_t D1;
5 N. o- B9 H. l - uint8_t D2;
/ h: Y: u1 _: ` - uint8_t D3;: w% S, o! O6 x9 {" t0 S1 j
- uint8_t D4;" x& ]" u) u3 Z) F/ N+ n$ w
- uint8_t D5;* t( e* [; o/ _! F/ N' ~
- uint8_t D6;. W/ `6 P# z) M5 } v& \
2 B# _5 P' T# h' j& X I- float previous_error = 0;
8 V G# j; b! N0 T& d7 } - float integral = 0;
* G9 V& W/ E/ E" z$ e Y( E' h - float derivative = 0;% @5 t: p) ~/ P
- int Speed = 160;# Z8 J7 H! o7 S' H- c& r( ~# K1 n; E
- float output;
5 r1 M7 q0 M6 h4 k) } - & _' Y, [( X) K: n9 d7 v
- byte Left;7 D) v$ z! N& i `7 Y) I/ D
- * c: {2 M' ]$ b9 k0 M/ I+ l3 w
- void setup() * T8 z% ^1 Y& Z+ l1 P* S. L* n, \, W
- {
. K9 _( r* I" t - //Set PWM 8KHz9 {3 _8 x) c5 j/ D8 ~# I% `
- TCCR1A = _BV(WGM10);5 [# m2 ?; Q: z: a9 H
- TCCR1B = _BV(CS11) | _BV(WGM12);
0 K7 U+ B2 z) z' q) ^ - TCCR2A = _BV(WGM21) | _BV(WGM20);: L( h* Z$ i( e! ?
- TCCR2B = _BV(CS21);, |% ~) v; I8 k1 S: p
- Serial.begin(9600);- o1 p* `' U6 @' z: i- g6 e, B0 M
- buzzer.setpin(45);
$ U- C1 a! U+ e. l - }3 [) j# q. w9 M
- 4 j/ [- [0 l% L2 D! l* D Y
- void loop()
1 e+ J- O9 i7 b$ ?& @; n - {
0 v3 t* d8 M+ C! i( w) I7 k( b - while(!((lightsensor_1.read()) < (10)));$ ~6 w" p, q: L2 z% G
- buzzer.tone(1047, 500);
3 w5 n/ Z/ i, H - delay(500);' L; u; A% k6 {5 ]6 [% ~
- do
7 z! O8 s8 t1 [1 h - { A6 y0 p/ V$ z+ R% H8 P
- int Err = getErr();
9 S a% g# P* ^7 E4 q - if(D1 == 1)) X$ X5 T- B' q- M# a+ x( n
- {4 E" x0 U* u ^+ b
- Left = 1;
+ @, z& x( V& y. `2 O/ X - }
# y2 z. ]5 D9 j0 d H! F ? - if(D6 == 1)
0 Q' i9 x9 p& q/ F* H: v# Y - {: S. a; `4 v" t
- Left = 0;
) _- m L8 |5 a, t6 U - }
9 l9 a6 m+ O+ O* _ - if(Err == 99)" g" z Y! E9 v" `
- {1 e5 J4 o3 f% J+ x- O/ Y
- if(Left == 1)1 m1 u+ U b$ y9 s' |& q
- {- s* R1 ?2 {& i) @8 A2 C- E6 n) H
- Speed -= 5;
* W' q$ U9 O1 L) ~ - moto(0,Speed);" Q$ {, y/ ?! ?. K8 C- W* R
- do
8 i" w' i. s+ x - {- a' w% C0 y$ y2 n
- Err = getErr();
: A* t! i7 w- \ - }while((D1+D6) == 0);
2 X% Z/ r. C6 g" p - }' ~ }) ^! h1 |" @$ l
- else4 ^! [3 X7 ?- r0 }7 S+ _
- {4 ]3 S1 ] E$ x# L2 g6 @
- Speed -= 5;. W7 D4 b; W# Q( c, o/ K/ i
- moto(Speed,0); ) l" @( [. C% H( ?( n
- do, n5 ?% H5 y }6 |
- {+ o9 Z3 O5 u! O- q$ z8 v
- Err = getErr();9 \2 S G+ v8 B- `5 B8 G5 L! l
- }while((D1+D6) == 0);
7 g2 j2 g% p* R2 Z+ z - }# a9 L1 h& @# r( m
- }
5 ?; i4 f. ?3 ]) ] @& N! n - else
, y7 \3 |5 n/ w7 x2 f+ A w; |9 V- L - {
5 q4 U K; ?& n7 [7 E$ O - if((Speed < 160) & (Err < 2)) Speed+=1;
1 q, e* K3 q' H0 c - if((Speed > 100) & (Err > 2)) Speed-=2; & X3 v4 i9 u6 u P6 y3 Q
- integral = integral + Err;
, G( m! ]9 H* p3 B \+ A9 D6 H1 R - derivative = Err - previous_error;3 N- B8 B0 Z% @; z, l
- output = Kp*Err + Ki*integral + Kd*derivative;; |5 T1 S1 O/ E
- moto(int(Speed-output),int(Speed+output));
. F# O0 I `0 R8 ?# y - previous_error = Err;$ \3 Q0 C4 X8 c$ d0 [& }
- }
) x3 Y+ X N/ X; ^+ g - }while(!((lightsensor_2.read()) < (10)));7 H, A3 S" k8 _' [6 ]
- moto(0,0);
& }* B4 C# ^- x& z* N - delay(500);0 i1 q" u4 P* o- Q$ X
- buzzer.tone(262, 500);- u" m, w2 k" U) `- b6 \) Q
- }
; ]" O9 T. {2 U - 8 h. r3 A9 n" \) D6 l7 u9 Y1 s
- int getErr()
7 e- p/ G$ ?9 u6 w( E - {
) M6 x' _& S5 H - sData = linefollower.getValue();$ G, r. N: p4 V1 L1 ]
- D1 = ~(sData>>0)&1;
' r- u4 [* X1 n - D2 = ~(sData>>1)&1;
, F) h- G5 R# }9 X7 ?1 Z$ {+ K1 M0 | - D3 = ~(sData>>2)&1;
; k: S Y* C* s8 t6 h - D4 = ~(sData>>3)&1;( c z. {6 u) c& t
- D5 = ~(sData>>4)&1;
% c* T8 J' L5 a, v( j8 O9 v - D6 = ~(sData>>5)&1;
4 L; P6 l) w' ~' L* G - int downD = D1+D2+D3+D4+D5+D6;
0 a. V* v0 F3 y8 P" M) T- l - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
+ [, B! z$ e7 ^. ? - if(downD == 0)
' u: u, g' i8 x2 l7 e7 X - {; [/ r8 }8 s3 s4 {# k8 k2 @: @
- return 99;
a7 n( ]: V: o! G' [, M - }
* }4 P. W0 v- B* r4 `% A: c& ?$ Z - else
% L: M v: V: v1 M; {( a; Y8 m - {
3 C7 ?. i/ ]' A# U - return upD/downD;
7 b$ u& i4 O6 ?$ o2 v - }
, A& Q. _* Z/ C9 o- d' \# q - }
% ^) N& v7 e' W/ ^
! |8 e: l2 L; O- void moto(int Speed_L,int Speed_R)
7 }0 b7 b4 r* l - {3 p% e# \; y9 q8 M7 U
- Encoder_2.setMotorPwm(Speed_L);9 M7 Q# N* [- ?) Y$ L! T
- Encoder_1.setMotorPwm(-Speed_R);+ C5 Z! u1 s$ v, J' O u! j
- }
複製代碼 # ~- P7 y) p; X& J- S
MeLineFollowerArray.cpp
3 s, f! N1 y4 r- j2 [- #include "MeLineFollowerArray.h"
8 z5 P: C- h7 B; ^3 J/ Q - V% m4 @% w' O
- #ifdef ME_PORT_DEFINED
$ x$ t1 r5 L& Q# r - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)) D# S+ ^, w) o+ {5 J( {% |
- {
. w4 _2 k) R9 L" C$ @$ K: c - 5 X' g% m7 f; o" y
- }
f8 |, A; X. G8 W4 z$ \" | - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
6 E3 y* a5 K+ u - {, E+ H3 ]0 t' \1 B& n/ p4 y
- _DataPin = mePort[port].s2; I0 t$ M2 r# Z; }9 K0 t5 |
- pinMode(_DataPin, OUTPUT);
: g& g( Q% M2 Z8 ~0 Y - digitalWrite(_DataPin, HIGH);
8 f2 r' X; G( L1 x) a - }
2 t1 ]3 P ~* b/ l( B& z2 S; T - #else // ME_PORT_DEFINED
# g( F* d7 ^) }1 c# s - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
: Z; X3 }7 I* x% S ?6 |+ K - {% P9 X8 p# E4 K' P8 V5 o
- _DataPin = pin;
! `5 \ E6 ~3 {, c7 Q/ u& {' m - pinMode(_DataPin, OUTPUT);
* {, \- l% G+ }: O$ r - digitalWrite(_DataPin, HIGH);
! F$ w6 N$ S. g - }8 {$ V: W: W7 c
- #endif // ME_PORT_DEFINED: m- Z: F( T" {" a3 @
" ?2 }6 N# ~: v0 Z, _5 y. a- 1 ?) w& j, T3 A1 H7 g$ i8 A( ?
- void MeLineFollowerArray::setpin(uint8_t pin)
R# S: u) A0 I# D- g9 A) X8 z' l! | - {
* J7 C/ k$ D9 q* m& l5 r; o8 S6 ` - _DataPin = pin;
_ r( X Q4 y2 \ - pinMode(_DataPin, OUTPUT);
1 |4 {$ Z% x# B - digitalWrite(_DataPin, HIGH);3 }, Y7 y( {1 u0 q/ F! h7 V# U) F. k
- . t6 ~- D# j3 x/ D* O9 N: p3 m. i) @
- #ifdef ME_PORT_DEFINED$ J% S$ V1 d6 h
- s2 = pin;
# n' m" F! G( [# r2 a - #endif3 |- K5 P, L3 u" J5 j1 G/ b
- }% N- _/ Y9 S( D, }8 e% T9 {8 Z
- + m6 C- R. K: M
- uint8_t MeLineFollowerArray::getValue()" Z8 G8 }: |; n8 y
- {
! R* w( i* M# F3 q - uint32_t LOW_level_read_time;
4 r9 ^9 J1 o) `, h! w6 h - uint32_t HIGH_level_read_time;$ O7 w. `9 p `+ Y% X0 B+ N L1 b
- uint32_t time_out_flag;
6 L; Q+ |: ?* {. {% ]2 | - uint8_t Sensor_Data[3];+ ^( f- v9 s3 B+ |# W3 L
- static uint8_t old_data = 0xff;0 B( h/ S l. _: G+ ]$ L6 K+ v
- K" V6 S, |1 M; |" a$ Y- pinMode(_DataPin, OUTPUT);
( |% A- a0 `2 n1 l - digitalWrite(_DataPin, LOW);5 \: k; k \. O. G* n
- delayMicroseconds(980);, o( M! r& f) w8 k. d
- digitalWrite(_DataPin, HIGH);8 Z, ]7 a: X: R7 e
- 2 l' s, @/ w5 z" G
- pinMode(_DataPin, INPUT_PULLUP);6 n1 a2 U2 ?: v! I# N) Z) V
- delayMicroseconds(10);
1 [& Q% s# k2 ~. i& E+ S6 w - - l" Y. j8 T5 b/ ]8 Q
- time_out_flag = millis();# U" ?" f) f& ?6 ?$ [
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 _- q: u2 _' Y: I1 k% R" u - 3 \2 U! ?# A4 ^0 e
- LOW_level_read_time = micros();. b/ l# b. c6 F# `. e( L7 ^
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 p) D. }, |4 m# }( P3 A
- {
% x0 {9 a x" j* N' Z - return 0xff;5 s! ?3 A+ `1 g2 V! P2 s5 h' D/ z
- }
( [ r( s6 l" Z0 J/ B: {) P
. K6 J8 n, J9 v& }1 r- k- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ X" Y( @$ ?, v, c7 @
+ j1 U+ X% m8 W9 C& I* d- HIGH_level_read_time = micros();) R% G( J5 A5 m$ x
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level+ z- b6 z5 [7 E y1 m! |
- ' m ^4 U2 n Q* E) L0 y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, r& ]7 r5 [5 v# {
- {/ @8 X- p$ Y1 C2 C; s
- return 0xff;2 y+ p+ h# S0 @' o% ?" v1 c Q
- }
Y/ r: \+ m- F+ P* q% _! P2 W - " f2 S9 S* E9 A" u, u3 }
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
0 z! Z/ I9 h2 d7 g4 j3 |8 P8 q - {
: R% l0 i2 B8 D - return 0xff;( S) F$ d6 W% l! s- ^$ F
- }$ m' f( W) o* t3 M% _
' f j# `4 H3 f5 j- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( L! [! y" m) t( F( Z! B0 I5 s - LOW_level_read_time = micros();
# K8 l2 t- S! O - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level8 Z3 m8 |% @' I+ V) P2 M
- # \, x8 I4 Y5 D7 t+ N9 K7 A- z' D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& q$ [0 j. y& w b% E' u) {8 P) j V4 e - {
8 ?3 H& w, z }- ^) U) b - return 0xff;
2 |& A* e7 j4 e - }0 [ d1 t( W7 y( [, b+ h8 y
- % n. d) j g" j2 c
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
4 L7 M4 e" S* ? - {' q% N% S! @) _& X; K
- return 0xff;
( J4 v. z& w( r5 }" O. Q - }. u/ ^6 D1 n' P8 p/ Z: e5 S
- 3 \% z+ D9 c/ E! [4 l0 _
- for(uint8_t k=0; k<3; k++)+ I. b/ z1 n; X# t' L7 h/ R, Q
- {2 I+ | y5 Y+ s7 `) p
- Sensor_Data[k] = 0x00;' s9 v3 y: R# r" `& O
- 2 K/ O4 h1 z4 V0 x7 X# C! g! P
- for(uint8_t i=0;i<8;i++)' j. W* h. ]; B8 q
- {, ~' i" M1 m* p
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" l$ S5 c0 Z5 _$ j8 E6 R: L
- HIGH_level_read_time = micros();
/ x. I& L! u6 P# `3 O2 u - LOW_level_read_time = micros() - LOW_level_read_time;
1 V- y2 Q+ B+ X7 y8 ^$ z - . ~, j) y! s2 `
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
# ~, Y2 x$ o7 v- s- b - {& v4 D7 M" {$ E$ p1 E
- return 0xff;5 k4 q& O7 A; ^& ?
- }
4 I% P! c* J3 j- `
# u( l: s3 j' J3 }( m7 v. a- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ Q9 O. m. I# Y- _; Z8 X
- LOW_level_read_time = micros();4 F" {' I& A* v2 t2 z& b
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level% L" k2 d$ N& Q$ f4 k0 U% p" b
- 2 ^; }/ f$ G; T# f8 P
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1 c9 e0 x6 a9 ~/ @7 l* k
- {# o9 p- C7 b5 N$ c
- Sensor_Data[k] |= (0x80 >> i);
, c" p2 D- g5 Y- [: t/ L - }
; }8 u8 u9 Z9 K+ G6 X - else if(HIGH_level_read_time >= 100)4 C2 P* W: y( O8 c3 X
- {
8 t( ~6 e( O5 r# z" I - return 0xff;/ m* Y. [" u) O0 O/ v# E5 i! t
- }% T8 e5 b% D) p w; u8 G
3 o+ ]& S! H/ ~7 j; |+ Y1 w, i- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
- i9 a& f2 v$ S- c - {
6 n* Q6 q: E9 E- ]1 s, t6 ^5 H; v - return 0xff;
9 U5 \, J9 e( L) S7 b - }
: a* I7 m! k# X0 q& G; C. l - }
& s$ \1 T- p1 O0 f. q8 G - }1 o5 _3 T# a9 f m$ @
- 0 P5 ~$ a# _. A6 d9 ?% i$ J5 F: t
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level; `9 `; s7 a: g" G6 I) ?/ D- b
- HIGH_level_read_time = micros();
, T1 E& ^- s9 j& X% W1 S - LOW_level_read_time = micros() - LOW_level_read_time;% H4 v4 _1 B/ e4 K8 p
4 l( I/ p" n; ~) T) s6 x+ z9 v- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )1 j6 N K5 r! P
- {
& o* f; `+ o+ x8 Z$ R: V - return 0xff;
+ X# S# d6 X& _" X# y& |) Q. |/ D/ g - }
% K: q/ C/ F$ W: { n1 J0 K% Q - ! V- |& J; I Q6 Z' a- q7 | r
- pinMode(_DataPin, OUTPUT);
/ D) h3 ]5 i; e/ E8 \& t - digitalWrite(_DataPin, HIGH);
" d0 a/ n! S9 ` S - ; }+ T( y- C! k& u( Z
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))' k) c% s ~" X( k3 ?, G s
- { ^" m# v* }: p: [1 K: \
- old_data = Sensor_Data[0];9 p* k) F+ H; i% n2 X6 }6 O" y+ v
- return Sensor_Data[0];
) G7 u ^" e* ?3 j - } C5 r+ X# L3 }, u
- else
' ~! g8 i9 C+ o - {
1 j8 Y; }+ g( B" | - return old_data;
: z, O6 q% }$ {) E+ e/ Q! j - }9 ^3 K/ u- M/ y/ y
- }$ Q) S) d; }7 o/ Q, U' a$ o
複製代碼
+ T# b, r2 g; c8 ?0 ?' ~+ R @MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
( A( I9 p1 L% x% n- L. x% y4 s) L; }& g. _% c6 L! ~
|
|