|
|
% _, d! f7 r. w% o4 o( o) o, i& w2 y- #include <Arduino.h>
+ R( m' k" u; {* l9 Q - #include <MeAuriga.h>
& z' M z# E5 O4 N$ S1 a; ~ - #include "MeLineFollowerArray.h"
$ t4 \* U& z6 T6 I# n# m
3 y; j8 \# n3 H, i+ ^- MeEncoderOnBoard Encoder_1(SLOT1);- s, y @: P$ w+ t
- MeEncoderOnBoard Encoder_2(SLOT2);
. |& O1 q. E" \6 R* T; x1 V - MeLightSensor lightsensor_1(12);( D7 {* B+ [) T# b
- MeLightSensor lightsensor_2(11);+ e4 X( o2 C' n }
- MeBuzzer buzzer;' E# j' P( P' @9 g, i
- MeLineFollowerArray linefollower(PORT_6);
- r7 @- ^# t8 C3 r! T& c
0 x2 m% p( S3 Q; h- #define Error1 1
4 K2 I5 c6 I1 E3 H6 R& ~ g, ~ - #define Error2 2
" v& z E$ B1 A. _- d* Y5 Q) K4 E - #define Error3 3
% c2 V1 M" T0 R! D: k - 7 F( _4 e& Z; [8 O5 w+ G
- #define Kp 15
% e/ l6 W0 v) t9 { - #define Ki 0.155 p0 N2 r& _! S5 ^4 p' \
- #define Kd 0.036 s5 X! |) P5 ?
- ; y/ v$ [. A- S' N, z% u
- uint8_t sData;/ f+ o# x P; P4 P7 F' U
- uint8_t D1;
2 p- d2 {1 U: {8 K- f" h" ~7 c - uint8_t D2;
0 @5 r( } @. L& h - uint8_t D3;
B) j( l9 o0 R5 Q1 i - uint8_t D4;
- N2 L2 K1 h6 ]) G8 t# k* j/ h' y - uint8_t D5;
$ ]. l* c, N2 _9 a: c. r2 ? - uint8_t D6;0 X' K c: S$ i) z2 L
8 I, [% z. o& N9 L/ s6 [* x4 g- float previous_error = 0;0 x1 h' D; k. y9 c* s" i: p @
- float integral = 0;) {. S1 }; g6 o* W v- [+ l
- float derivative = 0;/ J) L6 P; ~& ^, \. }# O6 {
- int Speed = 160;2 C7 _6 x& ^9 W
- float output;) x. T/ N6 d1 _/ [# Y
' H- Z7 n# t, I% i' a! B- byte Left;
' t6 A2 b! ^2 k" J+ I
! _; C. M& I8 w$ r1 l4 v- void setup() & e2 P1 A+ I l6 t
- {
; J0 P- F& N( `* y' L$ f - //Set PWM 8KHz( N9 r4 J4 M3 [7 I% [4 r/ d+ R
- TCCR1A = _BV(WGM10);& Q+ I) ~3 Z! L1 k1 W# S
- TCCR1B = _BV(CS11) | _BV(WGM12);, x% \8 Y4 Y& o, _
- TCCR2A = _BV(WGM21) | _BV(WGM20);$ x3 w& L6 @7 F* X6 l* A8 q4 q
- TCCR2B = _BV(CS21);& o/ u F3 H2 U5 j( c9 b9 [
- Serial.begin(9600);" ^) H* q) X. d; p" ?) s
- buzzer.setpin(45);
+ H/ O& T! J# V" S - }) u% y. H; l2 ^0 r2 ^5 \) c; ]
- : X e- e2 H7 J/ c! x
- void loop()2 Z- K, G4 v9 s$ ]3 R. b2 g9 g
- {
- C- D: M& j: c% h% p4 u! m - while(!((lightsensor_1.read()) < (10)));
: c0 J" @* K, j9 E) j& O S* s - buzzer.tone(1047, 500);
( F4 ^1 d& O! T7 A8 Z( k - delay(500);- x9 y9 v. @" l# Z3 I1 N4 A
- do
" s$ y: x9 D! k" ~- F4 [ - {
& v' F0 y4 `2 m4 A- p5 ~% M - int Err = getErr();
* W7 R/ `+ `! q M- a3 r; M- ^ - if(D1 == 1), P% O" w) G3 Y8 C9 q; B) X
- {
# Y% M" D& r& _( ~7 ~ - Left = 1;
) @( |1 @' |1 N% K' m - }
' C6 \& } a0 ], C2 @6 } - if(D6 == 1)
# O2 Q+ e# E2 H' y( X - {* y. ?* y+ I, X
- Left = 0;
1 P U/ m+ u' @7 e0 s5 p( q - }
+ E: `' `, q! n5 h& Z1 G; A - if(Err == 99)
' }) V4 A' |/ L- s1 e: [6 M6 b% c: K. f - {
: ?+ S6 [2 N2 F6 S' \4 e4 _9 I- w - if(Left == 1)
+ W2 z: w/ G, N6 H' v4 ` - {9 I" q2 T+ ~5 i& S
- Speed -= 5;- X4 S1 X, T8 `5 q* \. c
- moto(0,Speed);' s. \* P/ s0 z* h" c
- do
: q3 G8 ~8 [+ o6 x! K2 { - {
; O$ Q: O5 G) @9 M3 L5 q5 Y - Err = getErr();) d% W3 a+ t& [( W! ~! t
- }while((D1+D6) == 0);
4 q9 @' [7 E% ]3 ~% y" ~2 P3 Z - }) D1 P" U' r8 d, E* @
- else
* R* ~7 s; G) D - {
: k A1 ], c, m* l - Speed -= 5;1 P/ B1 Z: L- Z2 e# g/ ^3 [
- moto(Speed,0);
4 _4 j* l4 R. m" D7 g) B4 x4 D - do
9 \, R; ?- i$ P7 } - {! _$ \2 j0 s8 c9 T, R) z( N' I) R
- Err = getErr();; `+ t" P7 h( ^' k8 n3 j6 {
- }while((D1+D6) == 0);% {" ^9 L. M2 Y" V
- }# P0 }$ i. u4 U7 V! C2 i& O: s- x
- }* y1 T8 C' _' a' r `
- else
/ O3 z; _6 m% b - {
! K5 S% r6 t" C2 T l: X8 _ - if((Speed < 160) & (Err < 2)) Speed+=1; 1 a6 |' \1 @8 } ^
- if((Speed > 100) & (Err > 2)) Speed-=2;
% T8 F7 a. a! r2 Q& S - integral = integral + Err;
6 q8 L, J. u: A6 r" \. N5 e - derivative = Err - previous_error;
1 o# M/ R5 h) x' _3 ~- i - output = Kp*Err + Ki*integral + Kd*derivative;
" c4 C2 N+ |; _& s1 F - moto(int(Speed-output),int(Speed+output));$ g# H6 a& k6 I* w8 K+ j3 Z0 s
- previous_error = Err;: ~+ C3 ]2 m2 l$ _; W
- }
, E; c7 e, ?% K8 `4 Y% Q2 V - }while(!((lightsensor_2.read()) < (10)));4 U) T N. ~2 n$ {+ ?& _5 {
- moto(0,0);. p0 e, n4 Y& P, D6 I2 b
- delay(500);
2 w3 }1 K. ~; _* E - buzzer.tone(262, 500);; Z, G, r! w" g
- }
4 B( H1 g3 }. N5 X o2 c v; X - 9 h) s3 j* b3 @6 \% S8 V
- int getErr()
: F3 [% [7 f i5 v/ | - { 0 T7 e3 a j7 w) U
- sData = linefollower.getValue();
# f6 w4 U" }+ O6 ^: j - D1 = ~(sData>>0)&1;# V# i# C; a' W, I+ H3 o
- D2 = ~(sData>>1)&1;! g8 u x# o! [6 b
- D3 = ~(sData>>2)&1;4 s# h0 a' M, }2 b
- D4 = ~(sData>>3)&1;
# ~* J( h. d5 E# x t6 G - D5 = ~(sData>>4)&1;
, A, w. k. i2 B! k; J$ p - D6 = ~(sData>>5)&1;
' l- \) |, r r8 b0 i$ w - int downD = D1+D2+D3+D4+D5+D6; g- F& _) @ z' H9 O1 A
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);8 A. H1 z# V& P# W
- if(downD == 0)/ s0 _( V+ X2 e5 k5 i; d
- {
& x4 z. R2 T3 h; c6 |7 s - return 99;' Q5 k( K7 y+ B1 P0 y
- }
# C( X5 P2 F' i$ C4 G6 `/ V - else
H8 @8 n0 t' |& `- o - {& J; F, J( s+ J6 k
- return upD/downD;
) S6 n7 V J' H( z - }
) ~6 b6 j! M( W' \ - }. N* |( Z5 e) [& w3 o
j9 f! m( I1 t& }- void moto(int Speed_L,int Speed_R)
K0 B6 e& f2 O" V( R# ? ]$ Y - {& z5 }% h, E) X b/ r- L6 M$ R$ A
- Encoder_2.setMotorPwm(Speed_L);8 x, R$ o* Z$ p q
- Encoder_1.setMotorPwm(-Speed_R);* `" q' [# }1 v9 C
- }
複製代碼
7 W& o# E E3 k* ^MeLineFollowerArray.cpp4 A8 ]! _# ?* i( j: ~
- #include "MeLineFollowerArray.h"
7 X3 u8 k; u' |. R
2 k3 \ B x% G. `" _" R1 |2 s: W* p- #ifdef ME_PORT_DEFINED
0 j) U! L0 e: E+ @: [- i" o) J - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
2 }& ^4 W, P9 u1 q/ H: M - {! [' ^! q+ o, Y2 Z. ?/ S
- / k& P( i2 {9 A' [# `) a
- }0 @! k$ ?: d' b( v
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
( A8 m. v( @7 ^ - {
N; a+ g; F0 u5 ? - _DataPin = mePort[port].s2;
( \' Y1 x* y+ F' k0 B5 x% J - pinMode(_DataPin, OUTPUT); ) {! [: h+ }3 Z
- digitalWrite(_DataPin, HIGH);
0 v( r L' y& e - }
+ f& K% M- _$ Y9 ~ - #else // ME_PORT_DEFINED
: }- |; D5 h+ x& j6 o* I6 |+ X - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
: [5 {: y8 t! B# w. L8 ` - {. t9 Q5 R' R9 z
- _DataPin = pin;
: |1 \* ^% P( j1 m- ~- }8 x: x; m - pinMode(_DataPin, OUTPUT); 1 Q8 y+ N2 K8 }2 R1 q
- digitalWrite(_DataPin, HIGH);$ e# {1 P# N% b2 O9 P: w
- }
' ^! z9 F/ t, V6 J' P' n% } - #endif // ME_PORT_DEFINED3 l# p: S2 w9 O- m! j+ Q
; \, V2 t. q0 ~/ [. {. {9 G
, y2 }5 T' J! ~# t3 M# ?5 y- void MeLineFollowerArray::setpin(uint8_t pin)
: B' D) u+ ]6 O; e$ D) H - {
% _+ `0 K/ P6 [; k1 G2 N3 t - _DataPin = pin;
8 ?& b# `$ k( L& O1 F - pinMode(_DataPin, OUTPUT); 5 W% T& b+ C- _$ u, ?; i1 E
- digitalWrite(_DataPin, HIGH);
0 P2 h {1 N6 X2 @7 m
# I1 e1 z: Y1 N8 m- #ifdef ME_PORT_DEFINED
6 f( [7 a( i2 _5 D7 r - s2 = pin;
' X0 u( |4 g5 h, g* j - #endif$ b, c* v$ h |" Y H% X
- }
. }9 m% z3 R% s" \6 l - 0 p, U' |. ^% h% p( u% R2 @
- uint8_t MeLineFollowerArray::getValue()* c( t0 t$ ^7 A. h+ W' f
- {
8 @9 o9 ]2 ]; I4 j: C0 ~ - uint32_t LOW_level_read_time;( z: ~3 r% z; @( f
- uint32_t HIGH_level_read_time;/ ?9 w4 w1 U. ?& \
- uint32_t time_out_flag;
- X: F2 l% a8 m7 G" F - uint8_t Sensor_Data[3];7 d- }4 R! ~+ o9 R, R+ j7 k, _
- static uint8_t old_data = 0xff;9 i8 V7 `" T1 r0 X* f
, j, v7 C( Y0 f3 j- pinMode(_DataPin, OUTPUT);
7 c* r x, p. o0 ~; I2 Z% b& [ - digitalWrite(_DataPin, LOW);; `6 Y2 E& ]+ o
- delayMicroseconds(980);8 z% A6 V3 i" {% h/ o1 S5 S
- digitalWrite(_DataPin, HIGH);
8 {6 u( v& W7 Y: h' Q" |; | |
, K* @0 H. s/ y+ X2 P1 s8 S s- pinMode(_DataPin, INPUT_PULLUP);# f* Q" d& S9 F0 r$ A% C
- delayMicroseconds(10);
* W& `; z4 R! p: X k' e* s _
/ Q* ~: |7 q" o' L; y, p, u5 n: t- time_out_flag = millis();
) {) Y7 @' V( Y3 W" u% u: V - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 u, W3 Q! a/ Q8 ?; B
6 C. B+ {9 t: K( o1 B! L- LOW_level_read_time = micros();
5 L- E% {5 m) z& _& t X+ r3 Z" {( p - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) @8 Q; c9 I. a. b9 R; V/ ]) o% `% q
- {, {- v. `7 M( D0 ^7 X
- return 0xff;! c2 r4 F; {! k7 x' O
- }1 b+ C7 j: l( B, ~0 j# W3 S
: Z6 `0 Z! Y+ e7 M$ i! h- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: x/ c' O+ y$ Z% Y9 {
- * ? a, L( r, F; P( D8 y* U3 T
- HIGH_level_read_time = micros();" E, M9 s+ n/ u4 T% t
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; b3 R! I/ N( @+ g
/ J/ r* T' W- A h/ w- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ V7 J3 L0 _8 ]+ X
- {
+ I4 H5 @ A! h7 p& d& D0 L: U* W - return 0xff;
, A& h8 ^2 e3 s9 I, X6 L& M+ q% ^' A - }
* f: C# B3 H. T8 L8 G* b; x: b/ d
5 P4 D8 ~' r$ P3 }$ I9 R8 i. [% S- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
! d3 g6 E4 m! G - {
. s( L; N: p4 g' M( `8 G- {9 i6 e - return 0xff;
( ~0 i2 t% A9 f+ U - }; z* C/ B; {; R' `
- - [) L- ?# J) ?9 ^6 t" _
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# T/ `7 T% D. \2 t, ^8 B* K+ l6 I
- LOW_level_read_time = micros();" E; }& o- M/ ?- P2 b# E
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level' o1 P0 y, `; e5 G1 ^
6 F5 u$ b% u1 M' a8 Q" v- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
# E, [* x' f! d - {+ X- Q( D0 X/ O
- return 0xff;
! s: j0 O# W- w, _2 K - }% o- \6 J$ |3 n7 P$ u j
- : Z# u5 H; d3 a2 r1 r9 z9 B- q( w% S
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)): w) @; S- u6 H+ d! l+ `8 {# _
- {
h3 a5 f+ I/ _0 a( p4 ]4 w - return 0xff;
, K4 L" H+ L* a1 A% s" ?9 g/ s - }
! k% [' Q( ]$ z - + c0 m" ~$ f6 r+ n. r, m( N) Y$ d
- for(uint8_t k=0; k<3; k++)% ^- |0 ~. y7 w4 T0 F0 o
- {
, e5 p2 }: |6 [4 k+ b9 a/ ] P0 R - Sensor_Data[k] = 0x00;
' y7 u7 D }2 b. X1 W4 E: @+ S - " d6 b+ ]3 T6 e
- for(uint8_t i=0;i<8;i++)1 I8 ]! ?' c+ o6 C: t4 y: ^( K0 |
- {" p* A L9 |$ x4 `, D
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level# K+ }9 C5 _( [& v0 w) q7 Z
- HIGH_level_read_time = micros();( M, T6 J1 D9 Y: a/ B K
- LOW_level_read_time = micros() - LOW_level_read_time;2 X0 R- |1 X. Z/ E7 i g* }
& v; z$ Q2 z$ n8 r9 }. ]* Y# E; I( j/ e- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 A; d2 M# c k1 b7 p% T
- {: f4 g4 m) U) ~$ z
- return 0xff;# C; _# b( i/ e4 W7 U. A
- }' r* r6 b( t, X+ m% u( Y0 K
4 S' ^4 r# d' s/ t- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 X6 l: V" b3 z0 o4 Z9 H7 j - LOW_level_read_time = micros();* K% R7 ]+ a4 C4 s( U( F# A
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level; @% ^* z; E! A. v
* Y8 B1 E4 d' | t3 @) Q- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1* A X+ i- P D1 p+ g8 z8 W
- {
3 L, p1 c+ g9 A) ^ - Sensor_Data[k] |= (0x80 >> i);
; S( U* k2 i4 @. L& v z - }
% S! b) S* i; J% d - else if(HIGH_level_read_time >= 100)
8 Q, y3 z/ D* P8 x8 M2 X - {7 t+ T6 Q$ ?) J* N
- return 0xff;( y6 S9 [' C4 v0 F5 M) _. N
- }
0 G2 q$ T( w8 T' I J0 I - - x4 g5 t, L1 y; p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)" ]6 X7 r) X$ w, c [8 L" f4 n
- {) V- b) l ]( g" v& {5 B
- return 0xff;
" W1 g& H5 A8 m4 B/ @ P1 P - }& _1 E# o/ G9 C2 M
- }
% x+ U) z( c! t" `' X& N - }/ O- i* ^" u _5 {' V5 @9 z
- 5 O, I4 F- o, @2 x$ c
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* b4 w: z2 b6 s" A- m
- HIGH_level_read_time = micros();
) m+ y5 v" K! y( @) ^! e3 h - LOW_level_read_time = micros() - LOW_level_read_time;
( f. V4 k5 g9 I4 I6 \' H0 p
7 H: J/ x- ]. n6 [" q- I- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% N& |4 w' f6 G+ m% P/ l! h+ j4 c2 f [* X
- {
7 i T+ s" t: `( ^/ n - return 0xff;
- {8 a& r" y5 T1 k) | - }
9 g9 v+ P1 v/ v
. A$ A4 n7 e3 Y2 D- pinMode(_DataPin, OUTPUT);
2 k3 K% ]9 ~, H1 G) Q1 D; v5 E/ N) v - digitalWrite(_DataPin, HIGH);
) ]3 c. _1 y3 F* P1 A+ I - + ^8 k+ B v" }9 o! h/ E
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
b; T# U9 |* r - {; H; K( m3 f7 R+ p0 N
- old_data = Sensor_Data[0];$ n1 E& M! b9 G' D$ b9 ~
- return Sensor_Data[0];
; n1 [3 Q. g$ t. ~0 {+ Q - }
{, s9 @- T: B% \; K A' E - else7 [$ J2 X: F% K |0 ^+ i7 q
- {
) N# w' {! g* B4 b: N& g& X - return old_data;
* ]6 m- Y) X2 \' o" q( m - }
}# J7 A4 o M% n - }) u+ N' a3 A5 t* M* j4 |
複製代碼 ' X2 B! n2 B, E+ c
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
. u4 h) C! s5 K
5 F3 D$ k3 x( E3 p+ u1 X% ~6 n
|
|