|
|
7 x: H/ b% E' x
- #include <Arduino.h>
; c8 p9 b1 u$ D" `: h - #include <MeAuriga.h>1 J \9 Y5 L0 k! R' l+ e, P5 V
- #include "MeLineFollowerArray.h": Z: Z+ c. M- j9 s" g
`9 M8 U; E. D9 H1 u7 e8 a* w- MeEncoderOnBoard Encoder_1(SLOT1); i8 d4 j+ k* g( z# B
- MeEncoderOnBoard Encoder_2(SLOT2);
( n/ _# O9 }. C( ~3 D/ ` - MeLightSensor lightsensor_1(12);$ Q6 R) Q5 K5 Z! `" y
- MeLightSensor lightsensor_2(11);
+ c j1 A: k7 I. W' P9 l+ I - MeBuzzer buzzer;
( c, k1 A0 P* X' R1 h - MeLineFollowerArray linefollower(PORT_6);
8 K6 e% X6 T+ \0 K t# P2 R# P: y - ) o5 i2 |. B5 L: A% m( m8 u
- #define Error1 1/ t6 c9 ]5 [% i; w0 i M8 F
- #define Error2 2$ C: h. N! T, x. N
- #define Error3 3
8 I+ ], N& l' x9 ~! h; a- L6 u; k" P - 3 R: J {% r, \
- #define Kp 15/ Z! N- x! y4 X, J
- #define Ki 0.15, ]+ d3 G" D9 D1 _
- #define Kd 0.03
2 `4 n$ W' T8 ~0 A# N5 e L - # R# K# S: ~: x C; a
- uint8_t sData;
6 c8 _) t3 N/ R" ?2 g# T7 M - uint8_t D1;' c8 C1 u. I$ Y& Y0 z% i
- uint8_t D2;3 ^+ n [9 @* q5 i5 A9 d- X3 [
- uint8_t D3;
: ?4 _" W. F. J( y; F4 V: v& m - uint8_t D4;, D8 p$ w3 w9 k' Y3 @
- uint8_t D5;4 \7 \5 s: c: B/ R9 S! P
- uint8_t D6;
& ~% z8 k/ n+ a7 A7 d2 T; E - % Q0 y- k8 {. z$ x* @8 y' B
- float previous_error = 0;
- t) O" M" ]# f4 P. x5 q - float integral = 0;
* h4 @1 M* t( x3 k9 ~% L. x: M4 @ - float derivative = 0;
, T# X3 }9 w" r - int Speed = 160;! _- e( b( D$ j" U. [0 k
- float output;
" N; i6 [4 u$ h - . w1 }% |' m4 h6 l
- byte Left;
# u0 T* \: ~% d1 c$ C! R- [* x# U
/ }0 ~3 d# ]! [( P$ h- void setup()
+ Q3 q. f" X6 V. E - {; u$ V9 S `+ S' J$ R* F
- //Set PWM 8KHz1 o0 Z9 j. K+ {% E: |
- TCCR1A = _BV(WGM10);2 m" g7 I% s7 Q9 E7 W! _
- TCCR1B = _BV(CS11) | _BV(WGM12);5 p' r! Z" `: ^7 ?+ I, V0 X
- TCCR2A = _BV(WGM21) | _BV(WGM20);
$ f6 z0 b8 W7 \6 r - TCCR2B = _BV(CS21);0 e" R& l/ k+ k0 O! I2 {/ o# [8 f
- Serial.begin(9600);) W4 K# X( j! g- n1 w' ?: O% d
- buzzer.setpin(45);
( b7 r5 e( c1 g0 `0 i - }& C; p/ c9 z j" R& K3 G
! e* A; I( j) D j* ]& _. M- void loop()& r0 M/ C9 l* }9 z8 i
- {
( Y6 V$ X+ ~/ V! A - while(!((lightsensor_1.read()) < (10)));" H x9 D# F9 T/ e: s, Q N
- buzzer.tone(1047, 500);
% v0 J! B& c7 J: D* W5 s - delay(500);
$ }( A$ {8 v( m, H: `' t b; g0 ~; F - do( K8 H) m; ~) B$ h0 ?5 l! {' ?$ c
- {, C' q' H8 I' O0 \0 ^/ `, e6 M7 A
- int Err = getErr();- }& h( N2 X' y- S- o8 C& h. R1 Q
- if(D1 == 1)
9 _; K k: U+ ^7 W r! e7 Z; K* Z - {
z. w- K& D9 k6 Z2 k( G - Left = 1;
: A5 @6 m' F& |( H# ^ - }
) i9 O3 K) N, @. U0 u - if(D6 == 1)
' T3 \* y' I, z* s" d7 u; }" ? - {
4 v& i* r9 ]; q" i% ?) g - Left = 0;
3 N5 V, l5 r7 g2 n$ C7 H& M - }
7 v- M6 {8 t1 d1 s: Q - if(Err == 99)7 @/ J* Y8 N2 {' D1 n6 K
- {8 }- M# f! y, c9 v' `( @ [# d
- if(Left == 1)
" t6 X$ ?7 L X$ }; y# M - {
8 d6 T' F( N; O9 i/ `+ O - Speed -= 5;
a W/ n; z" g9 u0 X5 Q$ a$ |2 d3 E( I) V - moto(0,Speed);
7 Y. o0 `8 b. L - do- q2 d! g0 X. U" x+ @0 y
- {
9 D$ n" i# `+ `2 \5 P) { - Err = getErr();1 m9 q5 ^- ~) R! P0 g4 A+ G
- }while((D1+D6) == 0);
y2 \% y4 R3 ^5 v$ a( ^5 P" I - }
; ^! p- N( ^) G& K H - else
, U3 E3 ^2 |2 K# m9 j, D! T# r4 v - {
* H/ j2 k( S+ l - Speed -= 5;
4 e, y, T$ w, a( s- \/ J - moto(Speed,0); 5 U4 ~. l0 w6 `1 T3 [
- do" b$ q, s8 G8 z# s" k) b
- {
. a6 \- r9 S" {8 q3 X/ S) S - Err = getErr();; ?+ R: H& @+ @$ E" l& k9 q
- }while((D1+D6) == 0);8 T, x( ]8 S1 V% H6 w6 [, E
- }
( R- p+ j; C/ ?" z. {: b9 p - }: m u0 a, y3 h- N# h) ^
- else" k) |9 y1 T& K ~$ f
- {
# Z) G$ L" b* ] - if((Speed < 160) & (Err < 2)) Speed+=1; / ^* u1 e/ d" y$ b Z6 o
- if((Speed > 100) & (Err > 2)) Speed-=2; ' Z3 K7 u @* P% S& |0 y- z Y( ^/ _
- integral = integral + Err;
; n, c! C0 Y1 X, W+ {8 n; R - derivative = Err - previous_error; O0 K, B `* ]2 E% f- G
- output = Kp*Err + Ki*integral + Kd*derivative;+ ]8 ]' R) O6 W+ q6 n5 C$ S9 h4 a) u
- moto(int(Speed-output),int(Speed+output));
9 J: H) T0 R2 _6 B% n1 P - previous_error = Err;) P$ @1 ]4 A% T) X b0 ]& U
- }
1 X* w5 W( q1 o! e - }while(!((lightsensor_2.read()) < (10)));( V F, U3 O- _$ k* E
- moto(0,0);
4 W, G( K3 j" J6 K9 O; ]6 U - delay(500);
2 d9 h7 z6 G5 D - buzzer.tone(262, 500);
! E, k% d/ o8 d' V( a* [- U. A - }4 |3 z( Y( f/ b' \
- " z$ z' t( o8 o; T
- int getErr()2 p0 N) E$ c p$ V% Q A
- {
' x- W! |. }& \3 u( ^ - sData = linefollower.getValue();! N8 Z- l/ f& W& X \3 p
- D1 = ~(sData>>0)&1;
9 ^/ B7 ~4 S+ X3 a* I$ i- E3 F - D2 = ~(sData>>1)&1;7 K, @, Z8 l. i! X
- D3 = ~(sData>>2)&1;
. [7 d/ u( ~6 y - D4 = ~(sData>>3)&1;. s1 h- V: Y& C/ ?5 X
- D5 = ~(sData>>4)&1;% d0 |8 x+ ]" O2 T
- D6 = ~(sData>>5)&1;( O# `, B# e' F) t6 Z8 n
- int downD = D1+D2+D3+D4+D5+D6;! L9 N5 ^" B& P9 P0 ~4 W2 c
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);' z% a& \: L% A$ n' F
- if(downD == 0)2 V9 y' h# Z% U6 h; d
- {
2 k& I. @: ~. t! ^' H( u2 V! ~' }+ W7 ` - return 99;
1 m7 h- I' p" e a% Y - }
( j" g3 v" G ` - else, s7 A3 G" C8 t
- {
- s2 K6 R0 O* m" h# L7 E, O - return upD/downD;
* Z5 K' I! R! G, S - }1 E& a9 H: G9 n
- }% U8 R/ e8 k1 S; d5 [* }
3 ~8 @/ u# L5 n! J2 q j! `- void moto(int Speed_L,int Speed_R)6 x9 V1 q8 x( ]0 r# N/ U5 I0 y' d
- {
g' M7 b' `2 R - Encoder_2.setMotorPwm(Speed_L);
" @% H$ X, @. M) ` - Encoder_1.setMotorPwm(-Speed_R);; H1 | t# S1 R5 u0 D. L! ~. t
- }
複製代碼
9 w) i) }( T2 a6 h" O" I2 d6 NMeLineFollowerArray.cpp
4 K0 r! F8 `& z: _3 Y% }. O- #include "MeLineFollowerArray.h"0 b( s2 \) V z% j% |$ b
! `, b! @+ l3 |/ F- h9 G% |' D" y* I- #ifdef ME_PORT_DEFINED/ d4 @7 C9 ?* B, Y6 c$ }1 h1 {1 R
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
8 {$ e ~' O) |! b/ \& ?' k s - {
0 ^# y/ i9 `( s2 @9 W* h
t+ z9 n2 O3 F5 s- }" B/ J. s' |# @2 N
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
/ ^4 M& V# c0 Q! V - {
& X3 T4 i8 j+ F( C; b; L, {; X - _DataPin = mePort[port].s2;
/ E5 x" \3 D4 i8 o, o. m& d - pinMode(_DataPin, OUTPUT); , t' h) X ]/ L! S& f
- digitalWrite(_DataPin, HIGH);
; i) b" h- n( X' K - }9 I9 Q; p! A8 N7 a# M- w' B0 d
- #else // ME_PORT_DEFINED1 _" ?" U0 v: m# Y. c7 l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)$ C- ]6 L- t! u- V& h
- {' t* a( q; P$ h: w
- _DataPin = pin;! S" I C/ p" g% d% o8 T8 p
- pinMode(_DataPin, OUTPUT);
, o G1 P3 g* Y9 ? {; x - digitalWrite(_DataPin, HIGH);
/ A1 ^ i# q/ t1 c/ X2 o0 ? - } J$ x# b U3 S* l' t, z1 [! }
- #endif // ME_PORT_DEFINED/ A& O9 ^* H' O0 a- _
; K5 w2 a7 X2 L4 |/ Y$ q- r- ' N: p$ G" O" A* K
- void MeLineFollowerArray::setpin(uint8_t pin)
4 V& x: q3 z' M6 O2 [0 [/ y - {
! U" _4 G6 Y; @. ~+ F, ^ - _DataPin = pin;1 r' S2 A; Z& N% b; O# B' A
- pinMode(_DataPin, OUTPUT); 0 [% ?% ?, P$ G
- digitalWrite(_DataPin, HIGH);6 F8 {1 W3 _1 a+ G0 ~1 w
- + M7 w7 Y0 m. Z3 x; Q
- #ifdef ME_PORT_DEFINED
) m+ T" D; @1 `/ @ - s2 = pin;6 C% ^, d- ~ i. ^. L( c7 d' H% q
- #endif
! ~0 P! [% ^: l2 r# W5 z - }1 q7 l4 \. z6 [. [- t u" s
- 4 m' L& m/ h3 o6 x w
- uint8_t MeLineFollowerArray::getValue()
8 j3 N- H. m6 Z, Y' K8 c4 Q- n - {
. s8 W- X& J! h/ E+ V - uint32_t LOW_level_read_time;
# c" a) Y3 d5 f$ c# i - uint32_t HIGH_level_read_time;
7 U' z. s' b' [' C8 \3 l2 C4 @ - uint32_t time_out_flag;
! B* S. g; ~& S; b# q8 H* T k% Y - uint8_t Sensor_Data[3];0 [: d2 o& ]' a' H* D; \8 t* [5 k
- static uint8_t old_data = 0xff;1 l a8 h& V% ~8 s/ }5 H: {8 E% P
- ) [ N" j7 C) Y! [. N: `" p
- pinMode(_DataPin, OUTPUT);
7 E4 J6 c5 {$ s: ^) z8 n3 r8 { - digitalWrite(_DataPin, LOW);' w% k* G8 [4 g& y% @$ t! j3 P
- delayMicroseconds(980);8 h0 f. _% k5 g6 a- ~3 u+ n* u
- digitalWrite(_DataPin, HIGH);" ?" z4 O2 Q. c, l5 D! T
- 0 N% V. |4 b0 ~' u3 Y
- pinMode(_DataPin, INPUT_PULLUP);
' x$ G) o6 j+ k! C0 X7 J7 N) e) { - delayMicroseconds(10);
! U: n) Q% z' h6 d$ `3 X, ^
! c( K7 G8 x( }' a" c- time_out_flag = millis();+ d$ k0 L3 s1 I
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 l, T9 l2 R9 [/ W - ; M7 K* Z9 z- c5 L) R1 j% Z; W1 z0 M
- LOW_level_read_time = micros();$ p2 C) u0 S& i
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: L/ b8 Q$ |7 o
- {
U6 D( m% Y' n4 Q8 N$ L - return 0xff;" Y' Y5 K" k, G" N
- }) j$ t3 |; A' ^9 W
- ' b+ c6 }- G# P) w: Y$ p
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 O R7 }, b/ s( E
$ T, L6 U0 @* _" e- HIGH_level_read_time = micros();. i& h- g1 R1 O ^- [& s3 T
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
E6 M9 j% z4 s: l ?# w1 p
, ]3 {+ z( o& x2 ~. U( i+ j& M- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 ^6 [$ b& g8 W' t) h# e - {6 }3 a/ u; E: K3 X& V* S) ~
- return 0xff;0 _9 B, P, R/ ]7 ~2 O
- }
2 _' @, s( \% p( P2 B2 W' v - & v- b+ b! J0 ^; Z) ?
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))% _: p s* A- `
- {% D, M9 A+ e) ~6 q: y# t1 D
- return 0xff;
0 u; o4 J8 Y9 S+ O' H5 S3 M - }
& _( }/ y( c, }1 P. s# I5 s
: M& d/ F# x/ y- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
' u" w: y! l1 x: v+ x - LOW_level_read_time = micros();8 ~& N, O# P) |3 M8 y8 G4 ^
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level, d3 a; E, n& ~0 F3 w
- ) i$ E# z: x R; h9 [) d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 ]3 `% }) S% S: V6 V O$ Z0 p
- {$ ~7 c$ Y4 w9 H m; d% `1 ?
- return 0xff;3 Q- z, x3 U) ~3 a+ h
- }
- Q5 l3 t1 `% ^1 ~8 l+ c - ! [; p H- A6 T1 v! M) S2 U
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))# l2 ]' B9 \4 R1 L$ s, Y8 @
- {
$ x& B5 C' y* g; H - return 0xff;
7 G! W( Y2 m3 u - }
+ C% S6 B# h" y6 |& ]3 j
$ G- k& Z6 k. B1 o7 \* Z/ m2 V- for(uint8_t k=0; k<3; k++)% z; R( p: g' D' g
- {
6 J; a5 _9 W \( Z! h" `; C - Sensor_Data[k] = 0x00;
2 v$ K7 U1 _, O2 n1 s7 o/ L - * C3 J/ F, n; g% F9 G1 E0 y3 L
- for(uint8_t i=0;i<8;i++)2 m% G7 G& Y: i ]9 e
- {
; ?0 K/ l4 ?: A: L5 E2 x - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( ~0 w8 J3 o3 b7 o
- HIGH_level_read_time = micros();/ c+ O) {8 Y& R2 M/ w( l0 e: Z
- LOW_level_read_time = micros() - LOW_level_read_time;- b# R, | o/ i8 }1 t
, m) y4 A2 h1 v; u- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 L h. Y+ V* z - {
7 [4 H* k1 S( {; }; u* w - return 0xff;+ z2 s0 ]6 E, h$ m
- }' u6 ]& y1 L/ \ Y2 {! e5 v
: ], Q' Y& ?0 U; b: b- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ y5 p2 z1 b& t! L3 G
- LOW_level_read_time = micros();/ Z* r& z4 P9 z0 v* {
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
`8 b& }; c+ S# [! t - . b0 E4 b* J* ]4 Q
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
: a. t% H8 g/ N: d/ N, c - {% b3 B* s( k+ b
- Sensor_Data[k] |= (0x80 >> i);& ^* t' E' s. M& s
- }
' S! j/ O/ V. \- x7 ]9 k4 B - else if(HIGH_level_read_time >= 100)8 W, N8 S/ f5 y9 W8 Z( k' L
- {
+ y9 X1 \/ M5 Z% O2 O" I8 O+ ^! T - return 0xff;. M) M- Z! I+ i& B3 v, F4 D
- }" K+ P$ ~$ {0 i; X# s" ` g, u
' b* R$ I, G; v( k+ K5 u- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
" Q7 t' h1 M' c d: P - {8 k% t" V) b$ Z- U4 D
- return 0xff;0 ]# }' n+ s7 B/ |7 `6 [
- }
" |5 O5 ?/ ]1 d9 e( C - }
+ i3 w5 y' R. v6 | - }4 n' j' p9 \0 ?/ l) `* m3 n5 [
; @1 m' ]6 T3 ?0 Q6 m: r- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level5 c3 m+ ]$ J/ w+ ~3 X: L
- HIGH_level_read_time = micros();! j8 B" @; N! B% k1 m
- LOW_level_read_time = micros() - LOW_level_read_time;
) t. \3 L' H$ e |% N - ! ^. q8 _4 R b* r
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! a# J# m+ z/ w% b/ J1 S0 V
- {
" q( t1 ^) d6 F9 A' G& K" ]0 w - return 0xff;
$ ^/ h' b9 W( H% K c' L - }/ J9 ~1 x. ?/ Q; d+ }3 ~) o
- 0 R1 i, |) s( i% v) ~
- pinMode(_DataPin, OUTPUT);
* y$ @+ V- Y' Y1 I - digitalWrite(_DataPin, HIGH);, l4 K U- L0 M
- B Y+ V0 T# ] ^- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
2 y! y* D2 F8 z3 A# E4 [- z& w - {
/ U6 X" i8 ~. X' ]- e1 K& U) Y - old_data = Sensor_Data[0];
( Y$ [0 N0 S5 J4 [ - return Sensor_Data[0];7 [( I8 \% O/ L: K, v
- }* u L* F' \0 w f; J8 c- |
- else: u, {$ D/ i1 U& ` u$ D6 H
- {
+ C. D4 a- p* h$ l - return old_data;: Q$ Q9 c" m0 _& C- ~* z
- }
7 |4 D5 S; H/ q+ r- V5 ]! E: F - }8 |" j0 j U8 Y5 w
複製代碼 * S1 X5 ?2 R, @7 [2 n
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 h5 L$ H$ K) s; d
! k1 ^5 u: W! I. N- H9 G |
|