|
|
! O3 y7 ^+ T+ s" C7 a' w* {) d- #include <Arduino.h>
6 \( J' ^' m9 o+ [' w* \ - #include <MeAuriga.h>
; T3 ]0 ~( n/ m, Z! q - #include "MeLineFollowerArray.h"% {: r0 `+ D0 B/ b9 a5 c
& ?. k* x8 s# p# m/ d- MeEncoderOnBoard Encoder_1(SLOT1);
o* m5 v2 { f. Z2 | - MeEncoderOnBoard Encoder_2(SLOT2);9 ]- m3 n) N% I* q# U0 m1 W8 `
- MeLightSensor lightsensor_1(12);# E# R. i1 s @
- MeLightSensor lightsensor_2(11);3 u8 j P! p% ^3 n1 U
- MeBuzzer buzzer;! H6 R- E3 g2 L/ i- p9 g0 q; A5 r. M
- MeLineFollowerArray linefollower(PORT_6);
p$ o) y3 r b4 _
, p6 c$ ^8 O2 ^, q; Y( ]1 d- #define Error1 1$ p' r$ F" q2 Q2 \$ C
- #define Error2 23 m5 U9 S/ C9 d( n* _( ~, N( A' r
- #define Error3 3
) t' ~- k7 G( A8 `' ? - ; c$ T* P R, S1 c+ M* Y, q$ \+ F2 j( u
- #define Kp 15
+ B4 @+ u' Y0 b0 z - #define Ki 0.150 l* n( D# A2 J( C1 d
- #define Kd 0.031 x- G, X; F8 ?9 R5 I& p0 w j
- + Q& Q+ x, F/ @; a& r
- uint8_t sData;" n9 M: r$ `9 {3 Z4 z* b9 R
- uint8_t D1;- r! b* T+ J" \
- uint8_t D2;
8 [0 f; m- d2 x1 @0 b - uint8_t D3;
5 ?' d) `& H8 b) q6 O) Z" Z - uint8_t D4; I* ^: p- }' z! G: A
- uint8_t D5;- }1 I5 z6 j$ O& P3 l
- uint8_t D6;
( M7 w! v5 @3 u - / `% Y: O5 v; P$ q' a
- float previous_error = 0; {8 ^* Y2 f+ f# X" _, E
- float integral = 0;
! {5 M$ V! P# ]4 s0 c( ]! }. w4 U5 w - float derivative = 0;
5 ^( ^8 X! c2 m" J v: F9 k4 T! Y2 _ - int Speed = 160;1 S9 L6 J) R# C$ @( ~ D0 F
- float output;8 x5 P" ]0 `/ N9 G- w5 L3 z5 n7 x
: Q8 ~+ z; T: W+ f- byte Left;$ P2 z, E! M9 Y/ Z% K
7 |) h0 F# Z; `8 L- void setup() 9 B) P1 R, l" d, L
- {
/ Z/ Y+ J$ \' p( q: [ - //Set PWM 8KHz1 X' C+ i$ q' i# |. _9 u& }
- TCCR1A = _BV(WGM10);4 L0 j* _9 f# R7 k% S9 |" e$ H; Y
- TCCR1B = _BV(CS11) | _BV(WGM12);+ w' V* \9 F: x- S) T" [ p4 a
- TCCR2A = _BV(WGM21) | _BV(WGM20);
' T. w& t; t+ c; P - TCCR2B = _BV(CS21);4 l2 n [( j) G1 n$ M
- Serial.begin(9600); |' O7 K' x) Z; g. W e
- buzzer.setpin(45);9 J0 k+ t: V, u; i0 Z$ _
- }
) n7 w% W& _! A4 _* }6 g. u2 S - 6 ~/ }# o# v n; I2 U: U! ~) O
- void loop()
0 \1 `9 x) k8 | n - {( D; O; L9 c3 u1 e8 {! u
- while(!((lightsensor_1.read()) < (10)));
! g0 d8 I& Z2 L: M' I - buzzer.tone(1047, 500);* F1 b! {0 s5 I4 k) S
- delay(500);2 o& [8 U; d% }: `# Y
- do
% A% Q9 T/ S& b9 n# Y' Q3 ^2 e - {
' D7 o1 M9 N4 p! d - int Err = getErr();4 m' H9 A+ y' O( u
- if(D1 == 1)
6 ^4 ^ f3 O+ ?" X F - {' }5 K5 [$ o1 u B
- Left = 1;( Z. K' e) q; l6 |; ?7 c9 o# L
- }' r, @8 M S- L) U* ?4 t3 V& O1 j0 @
- if(D6 == 1)
1 r- i. ]8 p+ n1 o - {
" A# |$ U' W2 z: b7 z8 M! V - Left = 0;
% p2 ?, X! Y" Y9 x/ }7 T/ r' p, j m - }( O) @7 y6 G D5 Q
- if(Err == 99)6 ?! X6 i0 F$ G7 M0 x& f2 |, U
- {# m2 g8 J% ]4 X+ m3 ?- C
- if(Left == 1)
, y. K% A& X, x5 U - {: q2 o5 a- X/ ]8 m% l) ?8 Q+ ^
- Speed -= 5;
& j; |& ?! O; ` - moto(0,Speed);
$ J& e/ J% v* o1 h7 k/ o - do5 |4 {5 l3 v1 E- r5 M e0 ^* A
- {
- z/ c: {2 z, T/ z9 t - Err = getErr();
: F. u0 N; ^$ ~ - }while((D1+D6) == 0);4 W5 _+ j) u J9 W4 U
- }: q' B( ^, K7 N: u
- else& L: y9 R0 H/ S
- {
/ q3 A% `7 ^2 K, P - Speed -= 5;
6 r2 E/ `& G$ B( T# {% n - moto(Speed,0);
# }; H, Z$ \, g4 h+ X - do' [7 A J) R! q1 h! b& E4 n- A3 {
- {
+ U0 Z* f$ K) ?% @" F8 W - Err = getErr();
6 W+ i2 t- I+ V \! W - }while((D1+D6) == 0);) Z& ^; ~; l- I8 q: P- p& {
- } {9 q! D5 M# |7 M0 h
- }, y/ J; w6 k- X, a) D
- else2 Z$ _( h j2 {5 }
- {
7 X0 }8 ^5 B$ R C: m5 v - if((Speed < 160) & (Err < 2)) Speed+=1; 2 j8 X5 A( |+ i) X/ k5 L) H
- if((Speed > 100) & (Err > 2)) Speed-=2; % p' S5 Z; M' p* `' G
- integral = integral + Err;3 q8 W" z$ U J( ?
- derivative = Err - previous_error;
6 n) F5 h" Y7 N2 ? - output = Kp*Err + Ki*integral + Kd*derivative;
# H9 M8 I# L% o9 o0 P, p/ w - moto(int(Speed-output),int(Speed+output));
, O3 |, o% ]. ?2 g) k$ n, h' Q - previous_error = Err;8 f a4 j& t# w9 Z. z) T( ]
- }
# t- K- L, V7 t - }while(!((lightsensor_2.read()) < (10)));) p7 {) E; }$ ?" U9 D3 E
- moto(0,0);) X( z. e* b7 A& V
- delay(500);* ^0 p1 N1 A' ?, p/ t
- buzzer.tone(262, 500);; s7 h( q. s: u+ h
- }
) P' C& u+ F( T, c3 s. ]
( ?- L( S. U& a! g# ^* h: V- int getErr()
1 j8 u, y: R% W$ J F5 v - { / b- r Y1 f+ q
- sData = linefollower.getValue();
6 q: N( Z# i: k1 U6 J8 _ - D1 = ~(sData>>0)&1;
5 b* J4 c7 L- h, \6 |+ @ - D2 = ~(sData>>1)&1;
4 x; L) y& j& ]9 k5 @; m6 @ - D3 = ~(sData>>2)&1;4 a4 |/ V4 d8 B( d# {
- D4 = ~(sData>>3)&1;
/ w0 b- f& Y' o% _* ] - D5 = ~(sData>>4)&1;
+ D( `" e' x7 l3 }& { - D6 = ~(sData>>5)&1;' j: B/ g) _! n: t2 J! b6 b
- int downD = D1+D2+D3+D4+D5+D6;% t( d, a$ j3 g Y+ l9 M: C$ v& u
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
" ^, p& v( |* C9 n - if(downD == 0)- Z3 a( B/ G3 V, U9 Q
- {2 i4 N2 |1 {0 [( \
- return 99;/ k3 R" }2 ^/ \; M& j
- }
1 c m. O u2 S' \. P - else
w- E! A4 N% V# v - {
m8 o, J4 W2 S. V- ^ J, V - return upD/downD;
1 \0 A) x$ H8 O+ ?) E+ S+ R; u - }3 \4 u% J" T& K0 W
- }
0 u) d- m# b' Z( W - 8 p. j# L2 c9 K; |0 a
- void moto(int Speed_L,int Speed_R)2 L8 P5 u' _. x
- {9 L1 J2 a O- d5 o* Y3 Z
- Encoder_2.setMotorPwm(Speed_L);! {/ E" M7 `4 w! |( S2 Q
- Encoder_1.setMotorPwm(-Speed_R);
( p( Q. e$ l& F- o0 i - }
複製代碼
6 W: G* n5 O* o. i, x ZMeLineFollowerArray.cpp
4 i ] r8 U; i+ ?) a% U- #include "MeLineFollowerArray.h"
6 U, W. K3 p$ k( X' S - 6 `* }0 R/ o7 p9 X; Y
- #ifdef ME_PORT_DEFINED( @9 v' H- i( y% \1 O
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 q1 X2 b8 ]1 _
- {5 X$ ~$ f7 v' p C/ I/ _
- ^) x/ c Y) o/ H: ~$ I; |. S
- }
1 t" l) }3 `7 J! R9 l - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
5 y& G- W% R6 C: Y - {
: I( P$ s" K7 E* W% p( o - _DataPin = mePort[port].s2;
, k3 B$ @' h7 a8 s+ f - pinMode(_DataPin, OUTPUT); , T6 _0 S/ ?9 _& }/ c& A
- digitalWrite(_DataPin, HIGH);
) u& V9 z6 Y4 D- y2 i3 ~5 C/ G& \ - }2 k2 U0 _" U7 K: m) m
- #else // ME_PORT_DEFINED/ m( p p0 |1 P& {# h
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)1 ^4 j' F1 I! o& Q( v3 h
- {6 G% z" t; X+ @; p4 Y( i5 h1 p2 r
- _DataPin = pin;0 W" O1 q. [2 [
- pinMode(_DataPin, OUTPUT);
3 `& U: B |. o% h/ E5 ` - digitalWrite(_DataPin, HIGH);
" i) e/ K2 N8 S" b& A2 X - }4 X/ q2 u1 D4 e( t( J
- #endif // ME_PORT_DEFINED; H; O2 h9 ?9 |! ]/ Q
9 n- q; o5 L/ \9 Y. I
3 {) L l$ E$ f# y* R/ e9 u8 G _- void MeLineFollowerArray::setpin(uint8_t pin)
! I& B* T3 B! {0 q, `( s - {
% W) t/ D( o# ^2 c - _DataPin = pin;
+ H% }$ e: T8 a3 M a# D% S - pinMode(_DataPin, OUTPUT);
. u0 c5 v$ `7 `% p9 X, S, G5 y - digitalWrite(_DataPin, HIGH);
) }$ h M- T! h$ p. U$ W. e - * l; S$ w5 ]/ z, @
- #ifdef ME_PORT_DEFINED
9 g0 H7 H5 q, ?/ C) c- T' [ - s2 = pin; b0 w# r1 u- T
- #endif) U Y/ w1 k: U
- }" c- m) a0 x- f) v# U6 P; t
- ; e- C! d; e4 x7 k
- uint8_t MeLineFollowerArray::getValue()0 Z2 S" g( v% j B4 Q2 O5 N
- {
4 M7 ]% `) L) h' F$ y' u! J - uint32_t LOW_level_read_time;( S8 X0 |9 v" m. t
- uint32_t HIGH_level_read_time;& X% g5 o9 U$ J" a, J
- uint32_t time_out_flag;: [/ s% D" y3 n. {: r
- uint8_t Sensor_Data[3];9 D7 g" D$ ^4 T& j
- static uint8_t old_data = 0xff;: S& e+ E% b+ D" k
# l; G9 O8 I2 {$ V- pinMode(_DataPin, OUTPUT);
* d2 U H; ^6 O& N M - digitalWrite(_DataPin, LOW); S/ a3 t# H& [ P6 J6 z q4 Z
- delayMicroseconds(980);
' F9 {! i U% N2 m# y - digitalWrite(_DataPin, HIGH);5 Z- Q8 N1 z( ]
1 ^1 P; Z8 b0 C# d- pinMode(_DataPin, INPUT_PULLUP);7 F% G+ k) b% i! q# k
- delayMicroseconds(10);$ K3 h$ j" U) `0 _6 A" U
; A+ U1 ?3 X) Y- time_out_flag = millis();
3 ~2 M+ }# |* O9 I: X5 ?, z. }1 I - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 g5 X/ g9 G( ~% C# i
4 c/ E9 ?* p/ I8 h- LOW_level_read_time = micros();
" C6 a, Y% N, ? y& ` - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 M/ e% y" X# s
- {
0 ~" Q+ ?# s# N* q* K O. X - return 0xff;1 G7 c: l* r( {6 w. D5 N1 c
- }
* m- W% t; Y$ j h: |& d
1 V2 L# F1 L; w" O' ~2 Z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 U" x, Y" j( K. o# p1 f- ?1 C0 W
1 G8 ^. Z; x' X; l2 g- H- HIGH_level_read_time = micros();
' ~% z+ ~2 m# | - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
# p8 |1 F+ U; @6 b3 G9 S- K - ) P: @+ c/ J" z: w
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- x7 i4 F2 ~5 J1 u& u - {8 N' @. D7 ~. ]% z0 ]0 a
- return 0xff;% ]' P4 M9 ]1 T) B# W! a
- }
' O3 s q/ u# T/ U8 c3 g - . p4 ?2 y1 I' c+ A
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))' U$ l" n7 p) Z- R
- {: C0 R3 T/ ?& `6 ~3 p9 f
- return 0xff;
/ n7 j9 y% i+ _: w' ` h( m - }
0 m' z8 D. J3 }( J1 J! s1 m5 p - $ _1 C# m- S' I: g. k- H' y# c
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' T! g* o7 o6 p9 k% E
- LOW_level_read_time = micros();+ U* E( U8 M0 e% b. b# Z8 Z! g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- w& `& s# F8 T; s( o/ F - % d( H# p) O7 ~* a. W( M `$ Z4 I
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! J. M/ b- e: C6 f
- {; W1 A- d; b8 {& l0 C
- return 0xff;
v# K: Z% d7 ^6 ?! P& R. @. y4 E - }8 y$ l6 B3 t4 J
. Y: R" N: S8 u4 P/ h- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))+ z( N. X) \5 ^ z0 R& h: x
- {3 u7 D# l0 I% o; L
- return 0xff;9 H5 S8 e1 |) U0 Q% a
- }
: b4 R. A. G$ d- p4 d6 G - / U0 [* E/ J! Q( F- \2 E
- for(uint8_t k=0; k<3; k++)
2 _% l+ q- O" z% H& U2 W - {
; _" [8 ~9 o2 l& } - Sensor_Data[k] = 0x00;
5 u, [$ e3 b* j3 Q) g! U - : U# E3 { b# N
- for(uint8_t i=0;i<8;i++)6 G% \* E; x- a
- {: a' [7 L e, U; n) W
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level. \; R( u/ l" C5 w* s8 B0 @, c1 {
- HIGH_level_read_time = micros(); k" m/ H' A- d% _- U
- LOW_level_read_time = micros() - LOW_level_read_time;
+ Y, |: ]! `% S' R( ^ - ( I# y8 S8 M5 P4 j! V7 t
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
! v3 N" j0 b5 B! C3 H - {1 R' ?- F" D W1 z8 g* R6 y" T
- return 0xff;
7 W( [; ` e! t9 [ - }
3 ^$ u) X% Y9 Q/ Z2 Q# X) L! }
4 R+ k4 S$ {# R- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ v* k7 s" P' D- w H - LOW_level_read_time = micros();
; Z) o; k- a2 \8 @- P# Q - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level6 z% c+ J6 }' ?/ u, w$ W
2 z% f) k; E) c- c- `: P- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1" Z. I& ^% w# z ]
- {# Y" Y7 q7 i9 v* @8 @
- Sensor_Data[k] |= (0x80 >> i);
: I- h. H' a# K9 V9 E - }8 G7 N- \3 _- N1 V
- else if(HIGH_level_read_time >= 100)
6 F: }. c' T+ o$ U' J) _ - {
: S# y( @9 T5 l7 ?! f - return 0xff;3 B, n* @8 b( O; p* J! [
- }
3 ?! C1 _4 a" q& k6 n( I9 G - ; u- }9 H, k; h$ {* | a/ E2 P
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)# [$ g( [/ M/ l" z) K% ?5 ^
- {" {6 i2 P* a" U( t# V
- return 0xff;
/ o. ]6 g. b1 U: x7 \ - }7 c% [! U: u& j" M! u
- }# g6 U% s7 s) q" U
- }
; s# _) ^& a1 M% s6 k: C5 B' ?! i
# U2 q6 N8 n; v, B7 l2 G. N7 \2 K7 U- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level( N4 A; L; w/ o$ C4 K' h6 u
- HIGH_level_read_time = micros();
2 Q0 ^8 d! ], n9 ~+ f z - LOW_level_read_time = micros() - LOW_level_read_time;" @2 A# ~6 t- e
- ) w2 Y) i7 k3 {8 ^% `" K4 y
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
. H, r- O4 F' p% P - {% j/ V2 L/ k- f0 m/ [
- return 0xff;
8 {. |2 C1 g! `) S( u) M" d9 N - }; B; g1 M/ @, P
- - i K7 d" R5 n$ {# J; d# A
- pinMode(_DataPin, OUTPUT);1 l0 M6 g0 ~" k9 L) b5 I
- digitalWrite(_DataPin, HIGH);
4 v7 p7 u) v) X1 g1 e% P& V1 J" e
3 V0 L6 H$ B2 y* S- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
0 V# L/ R W9 T R - {
# l. a3 h$ j5 i; U! N; `; Z, F1 ] - old_data = Sensor_Data[0];
3 o8 X, R, t$ E4 K7 Q6 H - return Sensor_Data[0];# e4 c1 ?) R" e9 M+ t. r* u
- }
. P) w/ u9 }$ ]. g$ A - else: L" {/ c' D* y1 Z$ u
- {
/ ?% \9 D, l/ x) ` l( b) J - return old_data;7 X6 m! ^6 l2 J* o2 v2 T2 n# w9 h
- }! T5 u. t/ a% Z! Q! Z$ c" V# K. j7 i8 H
- }2 z9 w5 B, q7 b( S/ b$ q
複製代碼
0 n! N9 ]" `2 ]: I5 w- rMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! H8 i- j. ~, x2 z* l- U! x
" P0 C% }/ D4 P& I6 ?, U. L+ S3 U4 w( G; _# k |
|