|
|
* A7 c7 G. M7 F" c- #include <Arduino.h>
9 Y* {4 e1 n9 o5 Y& ^: w+ a& U - #include <MeAuriga.h>
+ s$ M4 s3 E7 q$ X; D; { - #include "MeLineFollowerArray.h"
8 V+ F! p% S' o8 p O6 J8 s1 P
) C0 O% N6 I5 q t$ q2 u1 d- MeEncoderOnBoard Encoder_1(SLOT1);
1 F* [; F3 a8 y+ D6 j( Q. n" T - MeEncoderOnBoard Encoder_2(SLOT2);) [! E2 c) q8 x; }
- MeLightSensor lightsensor_1(12);
# G" K, F; J- V4 q - MeLightSensor lightsensor_2(11);
$ E: Y# r# D6 `$ p( Z r - MeBuzzer buzzer;
+ R' o! M2 q" E, I+ t7 n/ Q5 U - MeLineFollowerArray linefollower(PORT_6);1 j" ^/ U5 z7 B% _1 |6 g
) L% ^% _" O$ p% n9 y0 T* {- #define Error1 1
* I1 W% K+ u9 J" Q3 T) p2 n - #define Error2 2
6 ^! A( ~# P, X- z - #define Error3 3
5 J* ~ \4 w$ W; }* P
) c% q9 ^& k6 p0 @, I+ ]; _0 ?; O0 o- #define Kp 15/ l( q& b- O3 ~$ Y4 {! H* \$ s
- #define Ki 0.15
; ~- C+ h) X* h# c+ p" `7 }7 h - #define Kd 0.03
7 v: q5 t% h% g$ f9 H; s
) Z: g4 y/ N' }- uint8_t sData;4 i8 `# H# L) z0 ~( p# q
- uint8_t D1;) u- p! d: L" D' N7 V' f8 h
- uint8_t D2;. i! ~- R! C) j0 b
- uint8_t D3;0 [+ T1 A6 I( [& m O/ g
- uint8_t D4;
* ]% F3 \4 c* D+ g2 @" P; E) ~4 B9 e - uint8_t D5;
% n- o$ X- t6 f# _( E - uint8_t D6;4 o5 z( J0 P& p
/ V' q, ?! D: N, r3 m- float previous_error = 0;
8 u4 U$ @& Z" Y8 T. P" t5 \ - float integral = 0;
% \, I7 f1 y/ G8 W0 C# V& K - float derivative = 0;
) ]) ?: G$ P( Y/ Z1 W- T' U; g" q ` - int Speed = 160;
. [& ?; V0 P* p# w5 N( X - float output;
) {! O8 [! y% O, i/ J( G5 D - . Y9 O g* U) N. r. A6 \
- byte Left;
. Q3 V8 D& T0 o5 L5 @; h9 L; E - 7 b) G0 J0 O, E o8 B3 d; w( z5 l( Z) z
- void setup()
* s+ J4 O: z3 b* } - {5 n, B. F: W, B
- //Set PWM 8KHz* ~# ~. d- i0 A! u
- TCCR1A = _BV(WGM10);4 |, |7 P( e" ?6 c0 r
- TCCR1B = _BV(CS11) | _BV(WGM12);
- ]# V" [. Z9 P4 L - TCCR2A = _BV(WGM21) | _BV(WGM20);
3 q4 }0 T5 E9 d( `0 S# Y - TCCR2B = _BV(CS21);
2 ~6 a- n. m* a; y, u - Serial.begin(9600);
' d, N7 T2 C6 n# D* e7 V. X( g - buzzer.setpin(45); Q) \3 K9 H9 a0 J0 J" [
- }( f2 }4 x/ R G4 d: f
6 ]# w! B, @- w4 J8 q- void loop()
6 z: U. q8 B. D' a$ o2 ?" T - {
8 u, K Q2 E( k" w. t3 |( }* _ - while(!((lightsensor_1.read()) < (10)));' D# K5 z9 B1 {' I9 b
- buzzer.tone(1047, 500);% N+ f8 F2 ?5 q# _
- delay(500);
3 d8 Y1 X, U. l - do5 q+ E/ W, q: u- o2 I$ O
- {
$ t- \% J. |9 ~3 V' F - int Err = getErr();
1 Z9 i; [. x! i; K - if(D1 == 1)" J0 B% g: x0 f& ?
- {5 N2 T8 j! w4 E% ^% g* ^: P7 v
- Left = 1;7 L; w) A8 l+ f, o
- }
6 ]! o7 @' {$ b) d( ? - if(D6 == 1)6 _# T1 |* m6 ]/ w" T2 x
- {
) v, N1 j6 k. d; I" F - Left = 0;1 E, t* `8 ^+ ~, b0 T& V9 n
- }
; m1 M b2 I$ X/ k - if(Err == 99): L; R/ n4 {' Q* U& `6 @8 F* A+ h
- {9 J8 h* k6 U" U5 s
- if(Left == 1)4 B# l; e2 m( p; r4 { \4 A1 C
- {- a: L7 P& J* |1 G
- Speed -= 5; j" m0 L+ `. R4 R+ F p% C# ]& z/ d
- moto(0,Speed);
9 F" O& R, Z3 M! I- V - do; Y3 [1 }6 H5 ^; [2 o7 N
- {
2 e) v: t* s8 P/ A6 C* X a8 ?; k. } - Err = getErr();
' V7 G& n+ X/ {$ I. h; O - }while((D1+D6) == 0);( O4 Z( v( c) Y1 _
- }
( o k9 u; u! L - else
+ s U- \! P }2 ?' n1 Z; i - {
1 O! P, s, C' O. i2 k - Speed -= 5;
9 K5 {3 M1 g5 }, J+ ?" n - moto(Speed,0);
0 m( |2 H$ C- A7 H" l - do
4 q+ S4 h+ |$ Y+ F# b - {& l2 K3 |, q+ z
- Err = getErr();
; D$ g" I, B8 L4 x3 X% s' L - }while((D1+D6) == 0);
2 _% W S# a" I" F - }
: Q1 j* T. s% w8 I+ F - }
: ?1 X! Q% E; J3 B4 u0 G% A0 y9 e - else0 W% W" x+ [3 f$ C: M1 M
- {3 O$ t5 y& W( }8 ~
- if((Speed < 160) & (Err < 2)) Speed+=1; ) E3 i- z5 g1 A" P) N8 [5 i
- if((Speed > 100) & (Err > 2)) Speed-=2; O; ^: f' X+ V0 v3 b1 B) `, k
- integral = integral + Err;9 U/ y8 U; [8 I5 M
- derivative = Err - previous_error;3 w1 G$ |7 l8 }1 ~; f
- output = Kp*Err + Ki*integral + Kd*derivative;
5 M' q4 Q( g9 p7 p6 r - moto(int(Speed-output),int(Speed+output));
3 \: ]2 G4 |* b% G& V: `" X8 Y5 v( n - previous_error = Err;# t0 c w% m" V2 t8 ?0 J
- }
1 }1 m7 F1 k$ o4 J- k - }while(!((lightsensor_2.read()) < (10)));1 H6 |4 a; ?8 a. ?
- moto(0,0);
! j8 P$ z) Z. M% ?. N8 V - delay(500);
6 T' A3 ?4 ~9 y( [ ~$ h% a3 Z - buzzer.tone(262, 500);+ z( s5 P9 [5 d' L) ?) U
- }( {9 n. n2 |7 r! |. I, y9 U
- 2 m0 J4 z6 S$ U2 P
- int getErr()
. D% U- p( X+ \$ s0 S1 X" F) Q1 _ - {
# r7 A9 J3 q! X8 b: F& }# @- T - sData = linefollower.getValue();
- P$ e+ @; {# m: ]8 k- Q1 r3 v - D1 = ~(sData>>0)&1;. [8 v3 C" G; w; S$ O8 m* D
- D2 = ~(sData>>1)&1;
0 W* D3 W* u, T/ X4 u& L - D3 = ~(sData>>2)&1;1 ]8 k9 g& \( r
- D4 = ~(sData>>3)&1;7 x, W5 C( M# k8 ?5 s3 ?4 T) g
- D5 = ~(sData>>4)&1;! ~) W5 c0 F# }6 L( R5 I7 U* I
- D6 = ~(sData>>5)&1;& K% T1 u. y& o4 z& I! ]9 F5 y
- int downD = D1+D2+D3+D4+D5+D6;# n/ k; p; {7 U1 m' e9 B
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);% Q) I6 {7 P; }" E
- if(downD == 0)
' F0 p4 W# U& [8 Z) J7 v" S - {; E& O' G2 `3 c1 y) ?# p9 a; @
- return 99;
+ O4 p+ G3 N9 `. J q, _ - }
1 Q" i- @, e, Y0 m - else! ]3 h* @; y8 t% B: `3 p8 r8 M+ v
- {
$ l& \+ a0 X# @ D$ c0 |0 N x - return upD/downD; }+ c- x$ H4 |5 X
- }: S7 B! t, h0 C5 L9 x& ?9 O& \* G
- }
' E/ @/ x8 q7 w# R9 t - & `4 N2 X7 ~; y2 q+ _
- void moto(int Speed_L,int Speed_R)
) x+ o" i2 s: `; A( V; Y9 p1 K4 F - {+ ~/ y' J- M( u2 V$ ?! [$ O
- Encoder_2.setMotorPwm(Speed_L);
% V# t9 o6 o/ D1 ~ Y; h' { - Encoder_1.setMotorPwm(-Speed_R);
2 x! G/ Y) o8 N - }
複製代碼 ( e. k% j3 C* N* R( y
MeLineFollowerArray.cpp
- A& u6 N8 O& O- #include "MeLineFollowerArray.h"/ n/ y/ w* q- j6 V( j. ^* B
8 [% L! g4 R, W7 `) E- #ifdef ME_PORT_DEFINED% A6 D& i9 d) H; N( {0 {8 C1 O. F
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)/ U2 H) H) N" W) d& f0 s3 W- |
- {
% B2 {( H+ G9 D4 e$ ] c* B - # n4 d6 p3 e' I3 U
- }
: m/ o) O) w% ^- z* Z0 M - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
( B/ [6 b% S" |! w7 j - {) d0 g) y0 c: ^3 A2 t1 @
- _DataPin = mePort[port].s2;) r+ N' |& }; R
- pinMode(_DataPin, OUTPUT);
I; [% ?% M* u, a - digitalWrite(_DataPin, HIGH);* d% o0 u ^+ ]/ v; T |9 j4 _; k
- }
& g0 p5 e4 M# `4 w+ a- {* H - #else // ME_PORT_DEFINED
1 z7 j% c! ]9 e2 J% `+ L - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
/ t) U3 f( x1 n2 [$ G" s$ n m2 X - {4 h2 }9 @& t# |; k* P( F
- _DataPin = pin;
, u+ l) X$ j* Q" o - pinMode(_DataPin, OUTPUT);
/ E& D$ l8 Q: t, h. ~( y - digitalWrite(_DataPin, HIGH);
5 e/ a% q9 t4 E% L - }4 A. P s& f# ~5 z$ X+ ?
- #endif // ME_PORT_DEFINED
G& u, j, j4 [9 Q8 ~+ A! i
7 [& e7 I9 R, @: Y) l) e* L: d
?9 G4 z: t7 [( q b) h' r- void MeLineFollowerArray::setpin(uint8_t pin)
H! |. n4 S: V) k - {7 r; N, A( O! v! p( i$ F2 K
- _DataPin = pin;! T! M) E( _9 Y' b
- pinMode(_DataPin, OUTPUT); , v; V- g. i7 W( w' y9 \3 T5 e' [
- digitalWrite(_DataPin, HIGH);) U) M2 F* b7 `, i
, E+ B0 b1 i& e$ m4 F, e3 s- #ifdef ME_PORT_DEFINED6 `6 [4 ^/ H7 c: o9 g/ k- n1 D- s
- s2 = pin;
, n0 ~& e# Y- G/ i& N - #endif
8 }, ]) a+ ^3 [+ _ - }; `/ Z ]# z. ?2 |
9 k9 ]! X4 ]% O/ }! H- uint8_t MeLineFollowerArray::getValue()" [. y7 O+ ~- J# |6 `
- {
) ^3 Z% C4 M% Z1 g3 c2 H& ^0 P) X - uint32_t LOW_level_read_time;4 ^$ D4 ]* \3 z, _% V9 I! G
- uint32_t HIGH_level_read_time;
8 ]! X- D# e6 O: v+ ~/ i - uint32_t time_out_flag;
) v/ n1 b' Q$ q0 |, A1 S* \ - uint8_t Sensor_Data[3];: Y& p% Q5 X0 R8 S1 @9 x6 l7 H+ j
- static uint8_t old_data = 0xff;
! R) V4 l' g3 [. B" p" X6 R$ ? - ; [1 c7 I2 m' I* N D5 `
- pinMode(_DataPin, OUTPUT);" i% c$ _! i& A
- digitalWrite(_DataPin, LOW);4 |9 ~+ J1 w6 Y- Q3 A+ Q- P
- delayMicroseconds(980); r9 Z: w) p" a4 J
- digitalWrite(_DataPin, HIGH);
6 p Q; u* p3 r: V
: b& w W+ K L- pinMode(_DataPin, INPUT_PULLUP);
3 [. T' p5 y& k6 P5 h2 j/ q } - delayMicroseconds(10);
2 O8 W& }# ?1 e! q" }" L - 4 C* F: G: P/ |+ l$ s
- time_out_flag = millis();
0 y! W5 |- W W! q4 S6 S( g - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
0 \4 T9 T* a" t$ Q8 V5 s - / L0 O$ b2 v' s* j) U: {9 R6 o
- LOW_level_read_time = micros();% B. Y% Y7 r7 p: h8 f; J
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; x1 t5 V! K$ |% F+ z2 ` - {
; j0 P3 R% e0 Q8 n - return 0xff;8 z6 S" s: _8 r0 i: ~
- }* T T$ k8 L* x- e0 `; ?) r! r
- % Y# Z4 f% |/ C& V
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ U3 d. u6 R C0 C r
- " r; N7 Z( I f* [1 ?) r
- HIGH_level_read_time = micros();) D& F; X* A/ o' |% @
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
2 A. B" L7 h( I5 O. [
O& N1 G* Q- }1 T0 z' S% A- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: l; [/ c- @, ~2 U
- {: w- Q0 ?, E& `( `2 d. |
- return 0xff;
; B9 ]9 K$ e; x - }
+ b: x0 r, e+ u# E/ I2 ]5 \ - 3 f& o& |. k# s% \4 T
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
6 w. F2 U m+ _ - {; Y7 h- S' e% E- a) _
- return 0xff;( P+ c! b. L4 y7 b9 L
- }* |0 m& b7 u2 y
- 8 @5 X, S- K, ?7 J/ A
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, U C$ e) q0 o. K) F6 R- O; D - LOW_level_read_time = micros();
) f7 s, t! K* v- z - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level& N( ]0 I5 [ g! P: U
- ; ~: m4 o& v' b* K8 p
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) ^/ \% k% s; ]2 [1 W- D
- {
3 E( @/ q1 L2 S4 [6 ~- K - return 0xff;1 j }+ N, v6 v. g( n# M
- }' U# i/ i' t% f6 O
# Z3 e! i. k) A( j- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
3 K0 c+ P. C( q/ `' X5 { - {
3 } k% @0 V" B5 m) ^ - return 0xff;6 R$ E6 O' p- g* Q
- }+ G% x6 D7 p+ ]3 ]! z
- , L' ?# u" h& [$ j( X6 A/ |$ \
- for(uint8_t k=0; k<3; k++)& d- ^% H. T8 b& @9 d
- {- I$ j* i' K" g$ L4 e, Q
- Sensor_Data[k] = 0x00;% W7 w& H/ @3 i- }
- e( {6 z/ P$ q" f6 m; n I+ j
- for(uint8_t i=0;i<8;i++)
3 Z4 M2 t4 E% t2 R' ]5 l - {$ ^4 n; u) _7 U' j& X8 ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ [ R3 ]! \7 j6 V( ^& b* ^
- HIGH_level_read_time = micros();
8 H) b S- [( I9 n) b - LOW_level_read_time = micros() - LOW_level_read_time;1 \! \! e" B8 d8 C% j
- ) H9 f- K, Y8 Y( Y5 h0 _6 g6 x
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# h: U+ M3 c; ^
- {- C6 @) s/ i" a; \- b
- return 0xff;9 I7 G, Y5 A' H8 _
- }
9 q/ I' Z0 s* {: D) @( l% { - + }" A. ]' H' N6 R! Y. H) P
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( _; v) W* G" U0 j - LOW_level_read_time = micros();9 T& k0 @. y; r8 @
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
3 b. N0 V& f5 m1 b3 y) P# ? - f! H3 h. u) w* h
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
# c* u' [5 U( v1 Q7 g3 q9 @ - {/ r, U8 n7 s5 I5 G5 g
- Sensor_Data[k] |= (0x80 >> i);- R" H( M0 a) g( I7 k& x" z4 t' e$ X
- }
! [$ J; I% K! M9 n5 ~ - else if(HIGH_level_read_time >= 100)
: |$ h: V# O& u) ]7 x - {
7 @: i, _$ r; J) U - return 0xff;( r. C, z) U' H
- }
$ c9 E# Q6 Z! y$ g2 P3 g - ' Z6 l' Y% b1 V. ]' w) c9 r
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
! I" R0 ]0 R7 |& v; t9 r6 ` - {
4 v6 Z$ T2 M/ ~7 U# ^* O( k - return 0xff;
" k5 i1 S' R0 g8 u - }
) ?8 ]: p; D5 T5 T9 v" z - }: A% f4 X5 {+ g
- }
5 P3 p' M/ T# e9 J3 I2 ^
, a% O) U/ s) t1 R- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level3 P, e2 ^1 J, S8 W# t# [
- HIGH_level_read_time = micros();* }; \0 v' \. @! Q+ O
- LOW_level_read_time = micros() - LOW_level_read_time;6 g; a5 Z3 y: m. w3 ~3 h8 k5 s% W5 ~
- - b: D# E0 @& L3 D
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )$ h# X) b& ?+ s2 i& B
- {2 M) i) ?+ ?1 f/ v f3 C: \+ x7 u
- return 0xff;8 f% L- h7 p9 c5 |) f! q
- } L! r5 |2 a3 I: f% \ T W
5 s. ]) e6 U' s) ^* n, a* q- pinMode(_DataPin, OUTPUT);
+ h) F) c( Q6 u2 ^% V - digitalWrite(_DataPin, HIGH);0 s: Q% G9 z" {8 g2 ~/ ?' Q7 A
- / T. |/ |7 I3 j# \+ I: \5 g4 f! C
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))! ^$ B; @5 R) c6 e# N3 `4 Z
- {
( z: P Q, x- z& s - old_data = Sensor_Data[0];) E+ Q9 R! C) @8 U" G3 u
- return Sensor_Data[0];) f `! _& N; Z8 Q
- }
% |. P( J7 E5 M/ o+ N/ K - else& Y( a: D. d0 T: C D6 x
- {2 l! X: l! U: v; z P
- return old_data;
* E5 T' L7 Q( u5 q - }1 n! z9 r7 y. a5 C" I y# B l H
- }
; Y0 s" p- h9 P0 x: O' h
複製代碼
: i5 b+ i8 l) V- p5 DMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! G7 W7 X% E7 d& A
! ~% |) a: ]7 ?( m( i0 |
|
|