|
. z1 C1 r1 P' x4 @
- #include <Arduino.h>0 m q# A( |1 ~6 A3 [7 j0 E
- #include <MeAuriga.h>
& q3 K2 W) a/ u& U8 z) R- @ t - #include "MeLineFollowerArray.h"/ R* [( _8 N' i. d1 y* s
- " [4 Z6 l: H% ]& @% |
- MeEncoderOnBoard Encoder_1(SLOT1);) k; p& Y: Y1 l) X# I. [ I$ z
- MeEncoderOnBoard Encoder_2(SLOT2);
5 [5 T1 A- [' `9 d1 G0 W - MeLightSensor lightsensor_1(12);
3 M, K3 n2 n% r/ ?5 T9 F8 {" |: ~ - MeLightSensor lightsensor_2(11);
. B0 B2 T4 P* w - MeBuzzer buzzer;
) }0 Q* u$ g" {4 l' V! l# T& S; f/ J - MeLineFollowerArray linefollower(PORT_6);) y4 D+ k; J: d5 m$ g' _6 b. c
- ) M3 N& S1 u+ |. f, l: a2 z
- #define Error1 1
7 l( F' J8 ]. ] - #define Error2 20 w! i4 g, |3 A
- #define Error3 3
& ^( h- M) {0 i) f8 `3 ]0 |$ ? - ; I1 [2 u7 H7 N& |1 C0 l/ R
- #define Kp 15" Z4 s4 E5 o# P8 |
- #define Ki 0.156 ], p: p2 A9 `9 k. T2 i
- #define Kd 0.033 o( y5 ]% I# y2 W2 E0 F) L
: C! `0 Y5 V# g- uint8_t sData;: h* H$ |5 |9 G
- uint8_t D1;$ g% x) [" X8 A
- uint8_t D2;$ U: F }% K6 m9 L; ?3 M. |
- uint8_t D3;
: Q. ?& j8 Y' n1 R% F - uint8_t D4;
4 \" t; G0 R3 g1 W! V3 c3 o9 q - uint8_t D5;8 p& E4 W( V- D, P6 `( x
- uint8_t D6;
+ U0 W6 r1 j* @; s
$ W* Z- F0 W$ Z! U* i' C- float previous_error = 0;
, p/ y) u7 _% O: n" I: z H/ c. v2 L - float integral = 0;6 K: B0 B$ I; q, R G6 m
- float derivative = 0;
# W6 [4 T4 H- g+ R0 j T" T& F - int Speed = 160; m6 K8 f2 X* Q( ]2 O1 L! h
- float output;
8 z8 m$ t( H, K2 z' | - 0 q$ k% n: c+ Y4 i4 ]- q
- byte Left;$ u0 e" u2 l2 Q8 t6 b5 E( B9 h1 i
. G# ?3 U' y3 q3 w9 _. X* \& y- void setup()
0 P# [. N& V7 q/ n - {
# y+ C& T4 s2 o/ k) @5 ` - //Set PWM 8KHz' Y1 u/ Y( \$ {
- TCCR1A = _BV(WGM10);
! e; @3 K8 b# l$ G R - TCCR1B = _BV(CS11) | _BV(WGM12);
" m! k# H+ A' C& {; K. G - TCCR2A = _BV(WGM21) | _BV(WGM20);
6 H8 l+ |' B& ?8 P - TCCR2B = _BV(CS21);
/ F6 h0 l' d& N$ {0 {8 W, y8 ?. p+ Z4 V - Serial.begin(9600);8 w* ]4 k) ~* f# X* Q; x
- buzzer.setpin(45);
1 B7 U* `# D0 k; w; V - }/ X* _9 h8 q' E) B7 I* P! j$ ~
- + U8 P2 z6 [7 Y8 |
- void loop()
) F' m/ F4 k; S# ]( H - {
6 i& X9 p* z, z1 ] - while(!((lightsensor_1.read()) < (10)));
" w9 Y, ]) {" }9 A: o; v1 M - buzzer.tone(1047, 500); C' Y5 N5 p) N* Z8 ]" W
- delay(500);2 a# m2 J3 \( Y. `0 X4 T
- do- O: q. h* L9 q
- {4 U8 N3 Z3 |1 J/ |# A
- int Err = getErr();
9 }7 J% {+ W! v& K8 E% d - if(D1 == 1)" `) d2 x0 P% R. J/ L* c E' u
- {. G/ `$ z3 o& Q% E+ B4 x( E
- Left = 1;
& z3 y1 y% y- ~9 C" }- M4 s! E2 d0 t - }
' |+ x4 ]) p4 w - if(D6 == 1). C, d; \5 o9 ?5 l5 H
- {4 w6 x: ~/ f3 O6 Z# z1 t
- Left = 0;
4 J7 D% p6 V/ Y/ r7 t- ~ - }
( U- i2 P7 v! L4 } - if(Err == 99)
/ F: S5 v) n2 U! M/ P0 R: M - {
9 P; F% X8 e% r, g# \# w9 l E - if(Left == 1)
# U |/ u6 B8 V1 Q% X9 T; g" x - {0 s+ F. H" }: ~ H
- Speed -= 5;8 }, W! F7 ~% R4 o3 u, a
- moto(0,Speed);+ y) e$ y$ _; k, f2 Y3 D/ l) Y1 p
- do% d' O# ]- X8 r1 @1 X! Y8 r" F2 F/ G
- {
: Y7 }+ g1 z3 N( P - Err = getErr();
7 G; F- {7 T% E3 P9 Q. y: L2 ~ - }while((D1+D6) == 0);2 |0 ~8 [# n8 w7 Z2 u F
- } y' Q1 k( g0 b" U+ T0 `% b; }% j
- else) h7 A6 i* L) U9 F" @% Q4 h7 k- O! K( B
- {0 @. T/ p) Z8 ]9 E4 Y& S5 W9 i: i5 a
- Speed -= 5;
1 ^6 U' {" T4 q$ `( n - moto(Speed,0); 5 ?" o0 b; Z( P7 g6 L: I
- do
D3 h5 K# P$ m% A1 u3 Y" v- [ - {2 S6 i3 L8 T$ e5 F% i' S
- Err = getErr();' o2 A# w# ^% T+ [
- }while((D1+D6) == 0);
( G9 J0 S1 ]0 X& y+ v9 U - }: R- C' g9 n2 T( H7 M# @
- }/ X9 O w* T/ Y3 V! M( F
- else7 a; f3 ]: g! Y0 ^
- {
* S& Q7 |2 S& s3 y8 C; ] - if((Speed < 160) & (Err < 2)) Speed+=1;
+ X8 U, W& x( } - if((Speed > 100) & (Err > 2)) Speed-=2; ' a @+ I$ K- _ M
- integral = integral + Err;5 P- _3 a# k, Z0 W0 A
- derivative = Err - previous_error;
8 o# U1 g$ z( L) a - output = Kp*Err + Ki*integral + Kd*derivative;& d: _% X3 H( D) P" J
- moto(int(Speed-output),int(Speed+output));
3 \2 o" R# F( K9 W" ~$ x8 L - previous_error = Err;" [: p: ~+ S! v9 M; X2 @
- }
) ~+ w+ o" r" ^6 b) B! h - }while(!((lightsensor_2.read()) < (10)));
9 P" `0 [; y2 J4 @ [ - moto(0,0);
- e6 l/ T: J0 t9 \) x - delay(500);
5 }+ s- {2 z, Q, w, d - buzzer.tone(262, 500);/ B; z6 a7 Y+ p8 L
- }' r! f' t* \) D: Z
! O+ k- w9 c2 @1 w- int getErr()
: i# o8 I( P6 l1 b4 f - { $ M- E. p+ ~7 _) |; q
- sData = linefollower.getValue();7 h$ @0 `% w2 q+ k; E
- D1 = ~(sData>>0)&1;
' G6 `) M) ?4 D' M# S - D2 = ~(sData>>1)&1;0 h+ B- c) m) D r+ D& k4 P
- D3 = ~(sData>>2)&1;
/ \, R" F/ d6 z# R3 [# {' _ - D4 = ~(sData>>3)&1;% W4 n6 n3 z! H
- D5 = ~(sData>>4)&1;4 _7 X M! x5 d( g8 x0 Y% Y
- D6 = ~(sData>>5)&1;
/ K/ p) {5 U) v z. u9 Z& } - int downD = D1+D2+D3+D4+D5+D6;
% d; u) o/ ?, E* d% ?( ] - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
; `2 K; z1 C* ^% h - if(downD == 0) u0 w/ w% G0 J1 W( M# i5 u( H! u3 o
- {/ K" |+ U3 U$ |( F; H( B
- return 99;6 |" I+ \+ x( l. |. e
- }
: i; D3 V. R& N0 M- D" q; I - else
; b5 W9 w+ Y# t8 k( B" {* n+ S! L' N - {
+ M6 ~& h2 N- t' b1 v. `( {( G - return upD/downD;8 @/ `9 O3 F& R5 s8 t0 o+ \3 |
- }# Q9 {- U; M+ g% h% z
- }3 w& W6 ], { o$ b! T
- G( o+ B$ d) u' q- x
- void moto(int Speed_L,int Speed_R)
! E/ d+ B3 j, j: m u7 u: s - {1 \5 D; X; F0 ^) S& P! q4 w# Q9 w P
- Encoder_2.setMotorPwm(Speed_L);3 w5 U, b2 e4 Z2 u6 p5 \
- Encoder_1.setMotorPwm(-Speed_R);% U& p! w! N% p
- }
複製代碼 1 M+ J+ N7 u* U9 e0 w9 j( g
MeLineFollowerArray.cpp8 F3 k) Q3 I) H3 d" l% c
- #include "MeLineFollowerArray.h"
3 v: h/ Q" J# t7 @! R
/ b1 ~* z0 [' t& i# [- #ifdef ME_PORT_DEFINED
2 P4 S0 S+ p- Y" m. `# ` - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' J& v$ J: t2 @! Z5 x7 p; ?# U
- {
8 V: e# n" t0 Q+ _" Z: q - , Z( K% k8 Q+ @) L
- }- b. v% R q. [/ B1 S
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)# o+ b( y! } Q# h4 W6 n
- { L/ j& [* G+ k
- _DataPin = mePort[port].s2;
) }6 C, i9 c7 ?; | ^ - pinMode(_DataPin, OUTPUT); 4 a! F1 `4 u8 _! N$ I- k' H
- digitalWrite(_DataPin, HIGH);( b3 b1 Y0 Q5 _9 [! w
- }
* n2 \ e3 A, w# m4 J+ }/ K/ B - #else // ME_PORT_DEFINED1 c4 m- d/ r% z, s
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
; ]1 f. S, d; Y+ t - {
" R5 P) ~" B2 V: x4 S" H4 ]+ h) p3 J" h+ m - _DataPin = pin;1 }0 m* w1 C; G0 }
- pinMode(_DataPin, OUTPUT);
7 ] m+ G- b+ [& u @8 n( P - digitalWrite(_DataPin, HIGH);
0 A1 ]0 S( @5 `& ]/ {/ J - }+ ^6 E* i1 H+ t( j8 [% m) [) L
- #endif // ME_PORT_DEFINED, P3 @, d* T6 d# b! c' [. t
- 3 y8 d' Z5 M b& p7 L( Q) f1 z. j
9 i+ [; |( R! R% S+ A3 R ]7 J- void MeLineFollowerArray::setpin(uint8_t pin)$ F- N$ ~# r5 _- T N% ]# k: H
- {4 X& i) s6 c+ I4 a B9 w& {8 d9 A
- _DataPin = pin;
0 n3 H8 y8 b7 X! s- X0 r9 G - pinMode(_DataPin, OUTPUT); }* F' \9 K* y5 c! q
- digitalWrite(_DataPin, HIGH);
4 G4 j- f( K' r6 {" E5 ^
6 m8 U5 V/ b4 O. w2 ]- #ifdef ME_PORT_DEFINED
S7 \, s1 I' _; U4 Y0 N - s2 = pin;; r- K. ]! j {4 l% t
- #endif
7 }' A/ D* O9 I - }
1 P9 L7 r* X" u6 w( Z0 Q
5 s: ~! ^4 ]; X/ K( a- uint8_t MeLineFollowerArray::getValue()$ }, s3 V& P+ v: v0 q. T
- {
- k9 @2 _# f7 R" n/ h' g - uint32_t LOW_level_read_time;
3 u4 H) B3 B' |* c5 Z/ Z - uint32_t HIGH_level_read_time;
$ S$ m; R ?) x! k& n; p6 q9 C! |2 a: h - uint32_t time_out_flag;# R, b9 ^* _0 ?8 r5 T
- uint8_t Sensor_Data[3];4 A$ }2 _8 [4 X" d
- static uint8_t old_data = 0xff;5 n: t( T7 M8 O1 n" I" r8 e
6 G- `6 B" X* }; a) j7 M- pinMode(_DataPin, OUTPUT);, `. _, i1 V1 K" Z
- digitalWrite(_DataPin, LOW);: Z4 C* f$ A+ C: j- f4 S3 F
- delayMicroseconds(980);
2 j( x. s& b5 }; [* H5 \* w. o - digitalWrite(_DataPin, HIGH);
+ n8 Z9 z/ u( g0 k4 C
( {7 I/ q! T$ B4 j. V0 [" w, k, J- pinMode(_DataPin, INPUT_PULLUP);
' V- T8 a+ E, _ - delayMicroseconds(10);
& X$ K0 d% c( u
% o/ M0 y# u) \1 W4 Q6 N- time_out_flag = millis();: D1 m: d& c1 `( o* _% o, u! U
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 v' w( I: b( [ Z- s/ y% l3 Q
) g( i8 E8 Q0 N- |! Y' B$ C" a- LOW_level_read_time = micros();0 P& _2 Y$ e2 W8 q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 O; u5 o* R0 f7 N4 W6 L. y" _( H9 w - {
- t4 _/ `' U( D# C: f. H4 A - return 0xff;
: u4 s7 _& S& Z J - }; p: h `- U7 I
2 I ]6 c, @( C0 p- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( P( J8 [5 V. K* U
9 C* Y7 H' h3 n2 g: e: x+ f- HIGH_level_read_time = micros();
- W9 x# g# a+ Z: S$ y! a# w - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
8 E5 S4 S! K9 Y4 r$ \4 [- l; _
2 E! f N$ M$ p: C; h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out( M% t; f( Q$ [% g* z. n/ E
- {$ v: w2 m" s, ]" D9 ~$ ~* `
- return 0xff;/ ~' a. y9 a4 q' n- ]; V
- }
+ {$ T0 [* y. W) O
# w9 T2 S& G0 z. W+ Q- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))+ x% I( Y) g i$ Q1 a4 ~4 t
- {
1 m* Q) [0 R1 Z' K8 N3 d6 } - return 0xff;& H3 Z" Q5 A& `4 U% z
- }: O) b, p: h* ^9 P* z! X0 J# v
7 v! i) E5 V& ^5 |' | f, _- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
: v2 i" k; m) K5 V5 o - LOW_level_read_time = micros();, J$ e" G/ L6 m( V9 Y, {0 y
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
9 M' t9 b5 E( T* y+ l/ V- w, W( k - 5 a, ]- y4 X8 R& [+ h+ x5 Z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! B5 k: T2 G4 [7 t/ x. o& v D
- {( V3 J: {9 ~" n% v; p8 S1 E% t$ ~# p
- return 0xff;
+ ^5 J$ S' {1 M - }' o) y7 ~' Y- }5 H
% {2 {7 X& I; ]8 j& M, a! O) X- t- S- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))7 } h0 y8 A, U8 V8 h% r, P- B
- {
% ^$ V- K9 P ]2 B - return 0xff;5 S; Z: h0 \4 @: n6 Y+ k l
- }
+ d* p5 t# c M5 h; w" a8 `
5 ]3 X; s' X* l) F, L+ ^/ X0 m- for(uint8_t k=0; k<3; k++)
# J S5 l! A" L - {
1 d# Y4 _( p7 c7 V - Sensor_Data[k] = 0x00;+ u* o8 L$ @4 F: A; U
- - S5 v, L: I' x' q* _$ Y: M
- for(uint8_t i=0;i<8;i++)
( p- T0 Y* z5 _ - {6 N* d! q) M# ^) B; G- A$ P H7 k' ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
! P- l- P( ?" \8 b - HIGH_level_read_time = micros();
9 H( t z9 W3 q3 G9 ?$ z4 p7 \/ e - LOW_level_read_time = micros() - LOW_level_read_time;
1 g# |' _- C9 U. e, {% k5 ^ - # b! S, X0 v$ v' _4 r
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): k/ |! w) e! O$ k
- {
5 c" ~5 {8 S H5 x, T7 q - return 0xff;
! A) p# }0 N- u6 N* t0 s5 A' F - }+ f6 G2 X8 g. x: l4 o, ` m
4 S3 `$ _+ z& ?, ]* ^- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: q a( K* @" h8 w
- LOW_level_read_time = micros();
, d0 i! e5 ?) O# t% V Q' s. X - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
! V& \, z" }/ k$ J0 V% q
/ D6 G6 t- p. E o( N- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
, u, N& q _5 l6 ~3 G4 r - {: p2 u0 b; b0 q' v
- Sensor_Data[k] |= (0x80 >> i);
, b- W6 k5 n! X3 b5 O; ^1 E) l - }
1 m) O- u$ T3 a H1 d6 R - else if(HIGH_level_read_time >= 100)
% U% }2 N' h# e6 A/ `! B% r - {
$ h; I# Y3 T6 m+ e4 @ - return 0xff;$ x5 A5 y3 u7 Q$ {5 s2 \
- }: Y; l$ d) t0 M5 k& B
- $ H: _$ e( a( g; ]- B5 P' F) [
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
5 \ E' R9 Y- P+ s - {
/ U% D U& @7 ?' I! T' H0 F - return 0xff;
& y) r6 K2 y% z' A - }" F7 n0 ?' o; H+ B9 P" K
- } g# Y Y) E$ E. `1 F& e( |6 h& } r3 N
- }5 d% Z+ d) x1 B+ F% I+ K& P0 I
- 0 K( @& v f% V! s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' r5 g: ~! `9 A, Q - HIGH_level_read_time = micros();: X+ o p2 @; H# @$ M' `' J' i6 m" k# i
- LOW_level_read_time = micros() - LOW_level_read_time;
6 G+ N0 ]+ U6 E% ^6 l5 \ - 6 y7 g+ Y+ a; n5 F5 K. n
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )* |$ y: H1 h1 [
- {
3 C: ^: T2 I2 J h3 {9 a. r3 ] - return 0xff;
, n% p4 S2 c; v0 l/ D5 A) D - }
F1 ^8 N& z# I a& [0 x" D - 7 y4 Q* f7 d' q/ q. a( X6 ^
- pinMode(_DataPin, OUTPUT);
) L! a4 c) l' i - digitalWrite(_DataPin, HIGH);% s4 X9 a0 j. M9 ~# X" i8 {- g5 ^
0 z( J3 f8 T, ]; h L- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))7 `! H7 p& }0 c) T' D$ \! `4 y
- {" S* r. N) M6 y! b9 ~3 D
- old_data = Sensor_Data[0];6 ~9 z7 u5 Y# G1 d& Y, ^' W
- return Sensor_Data[0];7 f3 S* P; H7 q8 m
- }
: Q* C' e3 [( y/ }, w0 q - else
- T0 J) P0 k4 f1 A; ?. p5 C - {
, y; V- J4 l2 J q6 E; j - return old_data;' `- G' `4 D$ q8 m& A
- }" [ V& J7 y6 W) c1 n
- }
$ q1 C: ]+ ~0 c, w4 P8 F% {" a! |$ r
複製代碼
+ x1 D8 A3 q/ o' W/ R) K! Y0 iMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! M0 U$ u. d( k! i: B% _1 P1 l% [0 Q
|
|