|
|
% {7 j! S% j* T C' w. e! J- #include <Arduino.h>( V3 V5 U* J7 \3 t" e$ i3 m' K
- #include <MeAuriga.h>
! j( B- Q& {4 w, u - #include "MeLineFollowerArray.h"# _3 {9 k2 t/ V+ r6 @% j1 W% z
- / s( ?) L6 m' P" u, M
- MeEncoderOnBoard Encoder_1(SLOT1);
+ _+ [* _, N0 b, D3 _0 b. O5 ] - MeEncoderOnBoard Encoder_2(SLOT2);
8 f5 ?+ j$ R; @) Q" Z - MeLightSensor lightsensor_1(12);) K) F% ^. P4 l, }) i3 Y" ~
- MeLightSensor lightsensor_2(11);: R$ J; b1 k/ O( @* L0 n/ I
- MeBuzzer buzzer;
* f, n, @4 B' j" \ - MeLineFollowerArray linefollower(PORT_6);
7 w; K4 K3 ^( m - 0 _$ z+ k& v1 ^$ M- m- _
- #define Error1 1' z3 M7 @; I) Z, ?/ K2 ~3 E
- #define Error2 2
: ?/ `6 }; b# ^8 Y - #define Error3 3
0 B8 W D2 y6 ^' O3 ^9 ~- \/ y, \ - ; x5 t+ @2 D6 n8 _/ M
- #define Kp 155 |5 S7 e; _8 [0 p" B
- #define Ki 0.15
" ? r7 B5 Z7 T. T- b S' X; G - #define Kd 0.03
2 l; J% {$ A. s: P6 C. Y
- N# t3 m& G2 d) B( z" V/ E; z {- uint8_t sData;
K( g" l" A a. I - uint8_t D1;+ T# R- W- Y V. Z2 c
- uint8_t D2;
4 N. e. f# J% F, N - uint8_t D3;& }8 c* ^# k6 |6 f
- uint8_t D4;/ r+ a3 [6 J; W, Y/ u8 T3 G
- uint8_t D5;( Q+ n7 [% Y8 {& l) T
- uint8_t D6;8 W% i0 S4 A+ L3 Y- G6 s- _# o
: X4 ?7 ?3 r0 w- float previous_error = 0;
/ r7 R+ V2 S* o; b8 i# ] - float integral = 0;
8 O/ X" T5 s( }- r3 |# r \. u3 e - float derivative = 0;
3 M) K3 S* }+ c( `4 W) J7 `' A& O - int Speed = 160;0 l! R t& Y" v/ m/ W( u/ G9 j
- float output;
4 `- O) M$ a3 T, A# T* G# I - 1 X0 k# V/ Y3 {+ U* f* U+ O
- byte Left;: Y% X, M( f4 z( C. x
( e3 D7 O" K+ {- S( d. `* n- void setup()
1 s3 L) t* L9 W- b - {9 u2 g- L! l. J& K8 u
- //Set PWM 8KHz
% @$ @8 O8 E* o: z% Q; G - TCCR1A = _BV(WGM10);' D. [- P+ v+ I% V8 \% y# A0 N$ r9 z
- TCCR1B = _BV(CS11) | _BV(WGM12);
: j* W* p7 N% G. _ - TCCR2A = _BV(WGM21) | _BV(WGM20);
^3 @! c2 w7 S - TCCR2B = _BV(CS21);
/ @! e2 Q+ i Z - Serial.begin(9600);
; U$ N( b8 U2 c' M! ^% F - buzzer.setpin(45);) O2 k% ~) y* @8 N% l# U9 W; h/ R' j# G
- }
( O" `, L) ]5 O4 C( b
, W0 K* T1 s% Z& l" r- k8 b+ ?# D- void loop()
1 _4 F5 t9 D) i& e$ e: P - {
5 D7 G5 U8 k1 |3 k - while(!((lightsensor_1.read()) < (10)));2 B3 c5 Y( P2 n4 m- Q5 l J3 F
- buzzer.tone(1047, 500);
& A% k9 o. I: {7 @ ] - delay(500);0 S# R& b1 u) V. D) c$ }- E
- do: r8 x! l0 G8 Y2 y
- {
0 z- U7 G* m/ G" D - int Err = getErr();8 W& ]4 {# _5 M" j
- if(D1 == 1)
% H) d+ t( G' B2 z H - {
7 _) W8 p( G3 a5 P - Left = 1;
6 I: U V& _* l5 g9 C' z - }
( X3 z0 A7 C# u+ j+ Z0 z0 E - if(D6 == 1)3 _* b+ ^; }3 U
- {! V! A! {# Q* z* d1 u- f% F1 t
- Left = 0;
3 L& E+ P! f7 t2 T - }
1 C' q! A- H) {0 x# S% R% A9 g3 a - if(Err == 99)! D" C7 b. p: S
- {/ a6 v$ ?: f1 F% u& P, }# f/ M
- if(Left == 1)1 J @* \& a, _$ g& ^/ V
- {
m" h' t4 P7 I4 f) x, P - Speed -= 5;! @4 C; e2 q1 ]8 K- v2 x2 x
- moto(0,Speed);; t6 [% |9 e, s) |
- do( b& r0 s) a; O/ o6 ~9 l. V
- {
! m% f; c- ^2 M* L% h% _% o - Err = getErr();; a( f1 W& S2 q
- }while((D1+D6) == 0);4 `" x8 T. c% w1 E# Y
- }
% N+ x7 m5 r J - else5 U2 i( g3 j, {9 H5 L+ C
- {
7 H8 G3 n1 J9 h E1 @+ ]& P - Speed -= 5;1 s( w! T% c2 {. h8 F
- moto(Speed,0);
! u5 a' z4 `! ?+ H - do7 L w d/ q" g6 w# k: n- X
- {' S5 E& x- q- D) G" { E
- Err = getErr();# h% F; Z! Y2 C J
- }while((D1+D6) == 0);: F) i2 v3 ?/ F) M
- }
. c1 O- d: J' f" E: e. x; ~7 n' ~ - }. N5 S" A* }+ g: n& f$ _3 e) S
- else
: b; _1 J, v; ~5 V - {
/ y3 V, C% D; c& g( X - if((Speed < 160) & (Err < 2)) Speed+=1; % x" x# W+ k" o4 n, P
- if((Speed > 100) & (Err > 2)) Speed-=2;
2 p/ R' A1 @0 u) K! p! p+ Q' n1 A - integral = integral + Err;& W( l6 v$ Q8 r& n' B
- derivative = Err - previous_error;
9 t6 O8 g2 F D - output = Kp*Err + Ki*integral + Kd*derivative;8 c% [: @; E4 U
- moto(int(Speed-output),int(Speed+output));" h$ M+ a+ W* i# \% ~
- previous_error = Err;) J& G5 G0 F/ a3 P+ W
- }2 ?$ K+ |8 o3 M5 F
- }while(!((lightsensor_2.read()) < (10)));
3 W- y3 ?. Y5 \ - moto(0,0);
' w" |3 |9 T, |. u" O- ]3 J - delay(500); e6 S+ F+ h s" H0 r
- buzzer.tone(262, 500);3 z9 x& ~' L* G3 V
- }" D2 y$ }7 i ]4 f
- ' } A' w% l) N8 l, |
- int getErr()
' N2 E' v7 @) l0 w1 l - {
# c1 W0 m; L; I+ M3 P* X - sData = linefollower.getValue();& @, x' H4 W$ g4 _ ], R7 r
- D1 = ~(sData>>0)&1;- }* z4 Q x5 E' N. u: t. I6 Q" [
- D2 = ~(sData>>1)&1;
: F% P; g1 e/ x m+ M" Z- j8 [/ t7 j - D3 = ~(sData>>2)&1;" G1 o% ]8 @8 O" Z: N
- D4 = ~(sData>>3)&1;
9 T* L& o0 B& l- Y - D5 = ~(sData>>4)&1;
3 N7 F- [3 c3 C - D6 = ~(sData>>5)&1;
( V& ^' z+ i7 Y- F) C5 o9 ~) S - int downD = D1+D2+D3+D4+D5+D6;- \; s F# ^$ s
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
( a0 X" w" H: q/ n! {" V' ] - if(downD == 0)
4 \, H) G( R, J5 L% j C - {
/ V# @, G. H; W( p. @$ Z - return 99;" B* \8 @6 g8 D
- }
5 {4 Q3 e7 |- K - else' O" c! J8 A+ `; R. K) x
- {
1 i2 Z3 a! y+ | - return upD/downD;$ p: u- U$ T" \) N
- }
1 w" ?# y7 x1 s5 z# j, i - }- J3 h" z# ]8 v& W6 Q
( F1 F7 V6 ^4 D4 t8 Y0 Y2 j% `+ L- void moto(int Speed_L,int Speed_R)
0 {4 `; J- s! q( i S1 ^ - {0 q W5 W& q4 g& I0 F1 r
- Encoder_2.setMotorPwm(Speed_L);
0 Q" ~4 l, F( n, }8 t - Encoder_1.setMotorPwm(-Speed_R);' H$ d6 x* K% a4 O! I3 [
- }
複製代碼
9 K' o' Y& z) U; n" Y8 uMeLineFollowerArray.cpp' j# J7 E( R! ^$ ~# }
- #include "MeLineFollowerArray.h"' n! Q! m! ^& l. O. ^" ]
3 Y+ A4 V6 z; i; Z- #ifdef ME_PORT_DEFINED* t& F* E3 e; b h' U% u
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0) n/ X* D1 g t* O( s( t
- {
# }0 h; p. g( v0 y! ^; f
% \1 p# s5 R* E$ b- }
% I5 |2 E8 g3 _. c - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)$ D- _' q1 K( U) }
- {
# e& W- o$ R$ w2 V/ Q" c - _DataPin = mePort[port].s2;+ ?2 ]* C% T4 O
- pinMode(_DataPin, OUTPUT);
& J+ `% o% v& N) {, M - digitalWrite(_DataPin, HIGH);$ F$ A c7 @% Y) }% O9 r
- }1 D4 Q* Y- g) |. z' I
- #else // ME_PORT_DEFINED& N: S$ b8 K: \# ~% ^% X8 I
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
$ t' G4 T7 K9 R7 X% Q - {9 l+ L% y( O1 I" _5 `
- _DataPin = pin; U6 B H8 H$ R7 c* d
- pinMode(_DataPin, OUTPUT); ; k- G' |3 _7 S2 {4 |+ j, h, N7 U
- digitalWrite(_DataPin, HIGH);
5 q$ Y# T4 T# V7 R. i8 O - }# z- m0 Q4 T) H1 I2 U# M
- #endif // ME_PORT_DEFINED
- w8 t0 y, U4 u - 4 c. @% J- w2 E. ]
- + c, t2 ]* z8 L" O9 p H
- void MeLineFollowerArray::setpin(uint8_t pin)
) M$ {( N; c3 ~0 J/ E0 X" m4 o- V8 C - {; D! P/ G* j9 i/ i( e( _5 C
- _DataPin = pin;
. C: r5 @/ G; r G - pinMode(_DataPin, OUTPUT); % f6 z$ @' U; }; S
- digitalWrite(_DataPin, HIGH);/ b8 o) C1 P# I1 c, l
9 J' o8 B- }" o. ~& O, y& ]) c- #ifdef ME_PORT_DEFINED h! q) G; b+ \# z
- s2 = pin;
/ o1 w, r6 O& c0 N1 d/ K4 f/ [4 V - #endif' Y1 ]" N0 d5 U; ?/ r* D
- }
& }2 J) j" \# {, @
1 I2 C4 K$ o1 U- uint8_t MeLineFollowerArray::getValue()
* W/ c" S/ |3 `* s9 t - {
& W0 J S3 m- r6 ~: F+ } - uint32_t LOW_level_read_time;
- n. P' B8 } _1 O8 u8 S - uint32_t HIGH_level_read_time;1 w( Z# j$ |6 L2 l
- uint32_t time_out_flag;5 r: e! U# f- v( C; C4 p
- uint8_t Sensor_Data[3];
0 F0 ?! W6 F4 h9 G! Y$ e - static uint8_t old_data = 0xff;1 T# l8 c& P( e3 Z4 _- R, J; K
5 G6 p/ N) [) Q6 P- pinMode(_DataPin, OUTPUT);# ]# J8 f: @# e" N1 C+ B$ w; D
- digitalWrite(_DataPin, LOW);6 Z' A; t& T4 i$ g3 z% a
- delayMicroseconds(980);
$ q7 l, ~: p& d7 M" s/ B* u - digitalWrite(_DataPin, HIGH);
w: |- c. [$ R: X0 b+ w
h8 E# T% Z& u. F' n0 ]) r- q- pinMode(_DataPin, INPUT_PULLUP);5 m: U+ F+ @. K* z. P
- delayMicroseconds(10);* `2 I$ ^- W, R0 q
- 5 m; K0 e! P; c- }. D; E3 T) F
- time_out_flag = millis();
/ `0 O# N6 n4 s2 ]* o - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 t$ a# c4 S; `" f$ c0 F- G
- ; n& e! q# {& F2 N
- LOW_level_read_time = micros();7 P$ ]" E( r* x9 q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 J8 H8 k, O7 W
- {
. A8 ?" z& Q/ v) P - return 0xff;
/ ]5 M5 l* s) A0 |! W2 o! q$ l - }9 D- f$ J4 I& F
1 q3 B3 @2 J! W7 ^! D- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- I! [" B6 m1 B4 G t
- 9 S9 S+ V( y4 i1 a5 `+ X q
- HIGH_level_read_time = micros();3 o" ?/ k/ r& C6 j+ W' }; L _, W
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 } K7 Y" O% H$ F8 g z - 4 [5 t. M4 g, n C& k4 ^: [
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
D4 Y+ [! T X7 w, V/ V0 C2 r - {
/ K; D) X- ~: w9 w+ C3 F/ Q. I( ~ - return 0xff;
# J0 p$ K4 ]7 y# O" f: }/ D - }+ |: `! q$ U( d8 u9 b9 E5 m* w
9 C/ P5 D' r& m- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
$ f# l/ n* U0 H+ s* t - {" M: y' V. K; n; N* t5 D
- return 0xff;! \. y3 |( {3 I: b$ R' ?# K! s
- }2 ~- [: |$ e$ V4 n8 e) `: v1 m
- 5 \: R" L2 B" C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 k! D) {8 m: i0 k# k) M - LOW_level_read_time = micros();
4 d3 ~% x( Y0 q! Y' E# _ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
$ o# X+ l/ y% x. J$ F
# S. ~* l+ p; f# Y& _- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# K7 h0 }2 ?% w. K7 U
- {" W: z6 B# _5 J, Z, y
- return 0xff;
3 t, u V8 g# Z; F0 A - }
& d9 T6 x& l) u
9 F9 t# r3 i8 s1 N. E8 X- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
& \" }3 k) b( P9 f - {/ q! |! o9 e w# e$ }/ s/ E. D" O
- return 0xff;
" Q/ q8 m% i; u8 J0 a6 L - }* ?( `! K4 W5 P
- 9 @7 c3 S, {# O C- O: l1 J5 {
- for(uint8_t k=0; k<3; k++)8 F0 q( }! o" c
- {
/ A7 [' w! i* v6 A" Q& Y$ c - Sensor_Data[k] = 0x00;6 }2 ^' A# |( U, d3 z
- 3 J0 D) K6 A1 N6 f3 C# D
- for(uint8_t i=0;i<8;i++)0 V0 k$ D6 F, H3 O3 P6 n& R
- {6 n6 o2 {& g# A0 |, V0 ?. |
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
8 L# Y8 }' V. S8 M: A8 S - HIGH_level_read_time = micros();
( A$ b" S+ W9 d& L, X( S8 ] - LOW_level_read_time = micros() - LOW_level_read_time;
0 r# g$ C9 \, I" w1 w1 d - + `$ H/ l2 ]0 J) G* I* S
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ), E* g; g2 X2 ]' K. j
- {
n9 Y$ @" T* B4 @$ Y - return 0xff;
/ _" K6 N0 O. N3 ^0 T9 |3 [; v/ y: K! i - }8 C2 R5 s5 u+ Z+ F
/ o' _& I9 l7 H9 G' A0 r- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 E ?6 T! k+ l2 W+ T, s7 M+ P+ x - LOW_level_read_time = micros();- ^: A$ m1 W1 n* Q+ H, m( u
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level# h$ z# @% m- ~
# K" K9 a6 C3 X$ y \# a- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
: k, J" l# p# r# B! I+ T - {
) \& J; d! d" ?- i, s+ S - Sensor_Data[k] |= (0x80 >> i); Z' b6 Z" C; K7 U9 G9 R: e/ \
- }7 n6 ~* q l, j j: W
- else if(HIGH_level_read_time >= 100)
( O' \. q* u7 r1 c/ ? - {
: A: n. h; w- k G - return 0xff; ?% H, v( G/ n+ B
- } R @$ b/ h8 O9 U6 g
/ \( X' s. r* a8 V* S3 Z2 {# c- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
0 E/ _5 l/ }$ i$ { - {
+ T" Y! N! {5 b4 e# T$ [3 G1 ` - return 0xff;& M7 Z$ f: M; S/ o
- }7 ?+ O( w7 t+ m- T4 C0 r
- }0 D6 Z0 I8 @4 ~! ~( }
- }- h; N. @9 W Q2 I/ x8 H
- " t+ ]" ^4 h9 Y# a
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
; A2 {1 h" f( N4 ]# H# k# v! _ - HIGH_level_read_time = micros();
" o. y) L# @5 ` - LOW_level_read_time = micros() - LOW_level_read_time;* W N2 T9 e( W( Q
I& v } ^' w3 y& ?! s- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
4 _& h# ]5 D; I( H2 S# X - {/ C [2 _8 B6 g- {0 s" B( {
- return 0xff;# P9 f6 I: j; v9 a
- }
2 @# F5 c- P9 n
3 y0 R/ n' i7 J- pinMode(_DataPin, OUTPUT);
. ?3 r7 b$ g! x( P% S+ M - digitalWrite(_DataPin, HIGH);- S, N$ \) B" u9 k" \7 {
- 0 k+ n2 ~7 N6 M
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))7 Z- V' ^( P, Y% t$ ^
- {
0 T* Y. U6 [( A - old_data = Sensor_Data[0];' G4 }) f6 P( u8 b2 F
- return Sensor_Data[0];
4 p5 b7 V( k5 D" U - }
5 l- y+ u0 }+ P: `+ z - else
; U; g; m( X& O: N* a - {4 C' {( i5 N/ ?; R9 W u
- return old_data;9 n" [, _, I' h# [) E
- }; i" ?; s- y- v3 d$ O3 M
- }$ O, E4 W7 t5 g8 y+ Q3 G, {
複製代碼
! y$ G; E2 q6 g: gMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
6 J: T& E0 P O: {% v- ?6 F" \2 C" j+ e
|
|