|
: o3 Q/ Z3 K' v) Q# h M- #include <Arduino.h>9 I6 `5 \7 C* Q
- #include <MeAuriga.h>/ o: p% Q1 Z! y4 {
- #include "MeLineFollowerArray.h"( ~. y3 s2 n% ^% G, {7 k
- 6 R% H; L# s: W2 y6 z3 F
- MeEncoderOnBoard Encoder_1(SLOT1);
0 v- ?( F5 `% N* z' M% p& j4 d0 U5 @1 Z - MeEncoderOnBoard Encoder_2(SLOT2);
9 r7 K; d" \7 G! G- ^! F - MeLightSensor lightsensor_1(12);" n/ r; L# \ S* v3 `6 p" w) o
- MeLightSensor lightsensor_2(11);$ v' i+ q# z7 D4 S% x5 D2 q
- MeBuzzer buzzer;2 G) a4 A: f. ?: y+ h( T
- MeLineFollowerArray linefollower(PORT_6);! V" E4 O. _' `+ g
- - H# ?1 _0 r+ |
- #define Error1 1
- H7 `; f& s" U" t J! d1 F - #define Error2 2. E* u& k1 Q% D' Q
- #define Error3 3
/ p$ N9 F/ j8 g1 L+ l7 ^+ R
+ a) B( _4 W6 d5 q- `- #define Kp 15
" X7 r3 V C4 ^6 m) \! Q ]0 Y - #define Ki 0.15
2 o: S! O3 X: K - #define Kd 0.039 D" Z, X6 l/ y, H% A, K& T( [
- , M4 ]3 C: U! l0 { g
- uint8_t sData;: ~" e" O# ~: u4 j
- uint8_t D1;
( m) n. r+ w* n8 @# Z, w - uint8_t D2;9 u% N+ C5 A' ] Z! ^
- uint8_t D3;
7 _7 W4 z7 @' f - uint8_t D4;" r6 A4 \6 ?5 s
- uint8_t D5;5 w2 q; g# T) y1 B2 {
- uint8_t D6;
0 y. h* S7 R k- }7 M - % e3 ^, @1 S! o
- float previous_error = 0;
0 r( M1 b5 J: F+ p8 O, S6 A - float integral = 0;
1 O+ v+ L% z7 w1 v9 s( X0 Y) ?* { - float derivative = 0;
5 W/ t9 i2 E- V" `! R E0 k - int Speed = 160;# ]6 T1 S" j' @& _$ N y% a M
- float output;
h) J0 o. G0 a' R; C: i1 Z - * @; y0 j! E8 W0 U* o+ y( M
- byte Left;% r5 M6 J6 T; t+ Z4 B8 M- l8 @
- " b- I7 f, p% {0 d+ i$ h2 G
- void setup() 6 H: T/ G( }5 ^4 x5 e
- {7 \/ h9 A; w' O y: s. n
- //Set PWM 8KHz- T1 z0 T3 s4 G+ ^1 O* S; F5 @
- TCCR1A = _BV(WGM10);
& R/ i C k+ m( a: d: t5 \ - TCCR1B = _BV(CS11) | _BV(WGM12);; B- ?1 ]* J$ o9 I! u" Q, _
- TCCR2A = _BV(WGM21) | _BV(WGM20);
/ d; G3 I5 \; a2 T6 D - TCCR2B = _BV(CS21);. V4 z) e( E) ]) @# R2 s; x# |0 M
- Serial.begin(9600);& Q! m7 d- N9 b/ F5 a& d
- buzzer.setpin(45);
V) p4 S4 {* X, I3 n - }
' M: U0 R+ R8 H! A2 ?# k+ @
n: d* \! n) h- void loop()
8 l- |4 ~; c5 W7 H1 ?6 a - {
! c0 A( K( }6 K0 u" T3 L8 T - while(!((lightsensor_1.read()) < (10)));
) g* a- ?% A3 J& _6 b% |/ p - buzzer.tone(1047, 500);2 w, d. w/ e$ y* f9 i$ S# W
- delay(500);$ a h4 B H& d
- do6 M3 `# N8 ?4 D; U1 t
- {
& Y/ u$ r2 S) T9 B/ s7 L. K Y - int Err = getErr();$ l @4 @6 q2 R' V5 ^" k
- if(D1 == 1)0 _% B. z6 t) T; i% y" T4 B
- {0 A( E/ _' X6 F2 M1 I
- Left = 1;
$ P6 i" X5 t' o! B - }
0 e0 [! l) D0 {4 _ - if(D6 == 1)
& S3 k2 s0 C4 |8 q& \ - {
+ s/ Y: q1 N7 s# | - Left = 0; b, w) {% w4 R
- }3 P! a2 O- F- z6 z: f2 i/ Z& X
- if(Err == 99)
b5 \1 G$ V+ K8 S - {6 U$ L% c6 L. j, I, P5 ~4 d7 a
- if(Left == 1)
R3 l' @0 S6 m* D+ _( _ - {
3 _ s7 d" b& _ - Speed -= 5;
' C" |2 y- w6 F - moto(0,Speed);3 s; R4 F% D6 U
- do
u* u8 y: k. f - {
# C, S" ? j8 F# Z) z0 ~7 l5 s - Err = getErr();$ o) _+ h5 ?6 q# w
- }while((D1+D6) == 0);
( P3 v( ~- k8 I& o* H, O: s6 \ - }
/ F* ]4 q$ l& m A2 {% ?4 @- [ r3 i - else. ^; f4 L3 T! E- g" f5 N2 b C1 y
- {8 G5 \2 l4 M1 J; j- u
- Speed -= 5;6 c G; m# s" T; I+ q
- moto(Speed,0);
2 c" B8 m. S2 x4 L - do
0 m+ \, D6 j! |3 C7 ?2 w% f - {
! W1 b Q3 G$ W% ~& _' J# ^ - Err = getErr();
9 H" W0 n! C' `7 ?5 E9 g5 C - }while((D1+D6) == 0);5 d* Y. P7 v) f
- }0 B e, |$ T: r/ C( x B! R
- }
; i$ C' W( R% T' m& V" v+ W - else
0 V: ~; ]- O( p2 A2 d - {0 m+ {. W, m3 I+ ~ H1 J
- if((Speed < 160) & (Err < 2)) Speed+=1; : }, N3 Z" z4 ^5 ]% S- P
- if((Speed > 100) & (Err > 2)) Speed-=2;
) e6 h2 P7 ?8 l6 X7 ` - integral = integral + Err;
( b4 A5 `( L z# |1 S# k - derivative = Err - previous_error;, R* v* y) ~+ X+ U
- output = Kp*Err + Ki*integral + Kd*derivative; y4 u$ t) }; E. J9 `( K' r
- moto(int(Speed-output),int(Speed+output));
( @0 Z* ^) G, w2 U: T. D4 E - previous_error = Err;
1 _1 D8 m7 o' Y y- D# ~- J9 |- a - }7 v3 L, S( [2 J7 H) N* m7 i* {7 K; D
- }while(!((lightsensor_2.read()) < (10)));
. r! c- ~4 q) q5 C0 _ - moto(0,0);
$ D; H! `: Y$ l- k( ^ - delay(500);$ G, U T$ C |& u7 B
- buzzer.tone(262, 500);5 h2 h' c- ^9 W
- }
) y4 d4 c9 F% r6 ]) ^ - 2 V7 I# V8 T! @, ?1 ~( s
- int getErr()
' m/ n1 s& ~9 Y$ U. y8 G - {
- p% H1 Z# l8 U - sData = linefollower.getValue();6 ?3 p- A3 f. H3 t. \: ]
- D1 = ~(sData>>0)&1;
- c) G7 X4 g: c6 O$ f+ y - D2 = ~(sData>>1)&1;/ l% Q8 Y }4 R, U* ?! L! y
- D3 = ~(sData>>2)&1;2 c- m) m) o8 E$ s3 {' U1 E
- D4 = ~(sData>>3)&1;
4 d; o" m, l# v% b7 D+ Q - D5 = ~(sData>>4)&1;
$ M4 m" K, Y6 p: u5 X+ Y - D6 = ~(sData>>5)&1;$ B, S/ ~! Z" t' z% j
- int downD = D1+D2+D3+D4+D5+D6;4 J/ {- \/ ?8 i4 }/ z
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
2 L% m- [$ g/ o6 \7 \. v- S& W$ w - if(downD == 0)
$ ]2 Q1 Q3 o3 i; j l# F - {# q; b- x' M+ c: z7 f5 Y! f
- return 99;
- K# `% }- R- G# _1 q5 }- I - }7 J5 @3 \! E0 d p, h3 d
- else) t) c; |/ b8 G* V& F2 A
- {4 A6 t8 e# ^. f7 m9 }* D* H: d
- return upD/downD;
* V+ J; h& w( Q5 H8 h - }1 z- H, F& j% g. W6 g: E' T/ u
- }. b6 n* @' F3 E1 n. c$ O
- , ~6 B9 t! ?( D: {; O5 k
- void moto(int Speed_L,int Speed_R)* p" I' Z y" o1 ~
- {
7 a5 f+ }! U( o. p6 x - Encoder_2.setMotorPwm(Speed_L);
. a( a8 S8 G: c - Encoder_1.setMotorPwm(-Speed_R);
( _( O- Q8 {5 q" v - }
複製代碼 ! @3 Y9 D3 W. u2 i
MeLineFollowerArray.cpp
7 J) e% _7 i0 Z# B- #include "MeLineFollowerArray.h"3 Y: K; w, s$ H
- ) t% M+ B5 Y. ]+ n9 M' l- Z
- #ifdef ME_PORT_DEFINED) n4 s6 g1 ]6 X3 _7 ]: @4 B
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)8 t% ]. U! l' \8 o
- {. P. i" M0 R- T) a" A6 u: Q R
3 w9 ~0 W' b& u5 \) J3 w- }; Y) P0 I# ]7 U; _5 [6 C2 h
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port): u5 C0 X5 @, p9 Q) v; v
- {' @; e _0 ^* B$ q
- _DataPin = mePort[port].s2;& N: D2 Z$ C# [7 X7 G4 V
- pinMode(_DataPin, OUTPUT);
8 j8 ?- x7 o. h: W/ g, O% N - digitalWrite(_DataPin, HIGH);
2 ]" \. _: M! Y4 s - }. f) Y! ?' @& ^. I4 v o2 @; I
- #else // ME_PORT_DEFINED
5 N' `4 o5 }; m1 T0 ?2 X - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)2 x7 t& I& [* H; L
- {5 x1 n* [6 V1 l( ~" F
- _DataPin = pin;& O9 r- {; `& u- m
- pinMode(_DataPin, OUTPUT);
4 @: X; X/ c. r1 z2 w - digitalWrite(_DataPin, HIGH);
9 V( b2 p8 ^! f1 a - }
4 F% A% n1 P' M7 E, { - #endif // ME_PORT_DEFINED
1 n( y9 t5 X0 ]7 X( t0 e; T - ; I7 \$ M! |: B: o, S% g2 [
6 Q4 L; Y" Y# |7 _) u7 B- void MeLineFollowerArray::setpin(uint8_t pin)
- @/ G* N/ i, O: c4 t! X - {
$ F$ p- K& X3 z4 [ - _DataPin = pin;/ A, ^) Q# I( F2 ~8 n& c3 M% B
- pinMode(_DataPin, OUTPUT); 5 [1 I) ]/ Y8 H& ?6 f4 M
- digitalWrite(_DataPin, HIGH);
" _ |5 y0 G6 e3 C& M - + |0 _4 {2 ^! ?8 [
- #ifdef ME_PORT_DEFINED' F# [+ m' u* O
- s2 = pin;! }# K' V k6 P. m
- #endif
: i2 }2 u+ @$ O+ |0 Y - }' c; i" }9 m# O. i9 n2 [3 S/ v O
, p" n2 e: H R$ E) @9 e- uint8_t MeLineFollowerArray::getValue()
7 e- J) L5 e7 W/ V7 ? - {& ?# t3 x% Q# y, G6 T
- uint32_t LOW_level_read_time;1 b2 r* ^: s( a% K3 q' `
- uint32_t HIGH_level_read_time;% B4 f7 d( S5 T3 p
- uint32_t time_out_flag;
: F0 K( Y6 R# _' x, d2 q/ a3 @ - uint8_t Sensor_Data[3];
( E; d/ o$ n# J, | - static uint8_t old_data = 0xff;
) I7 Y. S8 o% j/ j$ N0 F6 w - ! }6 ?; ^) n9 U7 }! t( _0 a
- pinMode(_DataPin, OUTPUT);
0 a }' b% B4 m5 S - digitalWrite(_DataPin, LOW);4 d. e7 T) L& j& P5 \
- delayMicroseconds(980);3 J9 a7 T) ]+ o# s
- digitalWrite(_DataPin, HIGH);" c! f2 _: K# t2 @6 f$ n7 m/ N3 q& u
: u8 P. |. H$ q v; ~) A% s( }- pinMode(_DataPin, INPUT_PULLUP);6 _1 o0 o5 _& P" r1 H, t- H- y( C
- delayMicroseconds(10);
/ t/ [2 d8 G/ F# v1 t - ; ]- Q" T5 j/ u
- time_out_flag = millis();+ ]8 ]7 \0 y7 b% x( K' M0 @2 C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 t9 ~0 Z! U1 _8 K% f9 b: d - 6 Y0 V0 ?2 O% }4 f8 _2 d3 `& ^/ y
- LOW_level_read_time = micros();
$ b' \) p( _4 @" y- T$ Z" J - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* m A Z$ n( E$ b2 H4 m2 N8 w
- {: t3 m$ p+ O" T2 j9 e! l
- return 0xff;
% H7 e. B2 ]" Y1 _/ `9 E8 l - }
" W$ d+ |& G( S C
- ^2 g" N0 }) N x% b8 }- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. l: N1 D; E q7 n7 g9 o' I - 9 k# P" l2 {! a1 H: [$ k0 ^
- HIGH_level_read_time = micros();
* R" ^: ^% Y4 `( b7 u) O - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level4 [: `+ S6 K9 e) W
8 t8 F1 ]" _5 u) b# b- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; C3 e, M- L% K9 X; P, N& c
- {
4 M- j* ?0 \. g% ]' B1 X - return 0xff;" e2 L w! X! K! t, g: v
- }
6 ~3 e ^" K j. r0 V0 y. z3 r - ' C1 P/ T$ W6 @) m) A
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
+ n5 Q" Y. O# @: i4 D: l1 J1 q - {! ^4 z' D1 ~0 e/ B* V
- return 0xff;- P2 t6 K7 o. h
- }1 R& t1 ]" J( V
& ], B9 P1 Z: s9 \/ P4 w- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: Y( X! A# [+ Y; |
- LOW_level_read_time = micros();1 Q2 t9 M( B8 {; _
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
6 H- W, L5 p. G# B3 e - $ n* z5 S( ?2 I4 I
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 b: R% D S+ ~ - {8 a1 L' T7 r' X8 @
- return 0xff;
) O/ A; s' G: x0 S2 S4 m' E% C - }
* e$ g m, Z5 \% ~% s - ( F) I* p3 p# [+ ]
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
5 A- x4 N+ i: B) Q( j/ g7 ^- f - {3 L8 u. z- }% p/ ^/ |
- return 0xff;( f( a; P2 X/ P
- }, e+ l9 P4 O, B% H' u* `9 R5 {
- ; ]% G" R" I7 C: O
- for(uint8_t k=0; k<3; k++). c/ j0 B: ?4 q8 Y. {" Y
- {' j: H1 v8 K1 g
- Sensor_Data[k] = 0x00;0 y9 w' ^1 L) n& B) w& K
- # z" k: Q# X$ k! w) W
- for(uint8_t i=0;i<8;i++)% u$ ?! u0 ^8 a Q
- {
5 o1 Z5 ~% p7 w - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level) q3 X8 b$ S* p6 r0 k# w4 O$ }4 P
- HIGH_level_read_time = micros();
9 M2 {& s# s" ?8 S3 } - LOW_level_read_time = micros() - LOW_level_read_time;" k- K" n7 H, ~
* T& d, S6 R. c) F- B- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 }" c& G6 s( n% U" X& r0 z2 d9 W - {
5 g& n1 u0 j8 {& p7 [' s - return 0xff;
$ X" z8 K) a! i - }" i5 Y* I, F$ x+ y9 ~& \
7 P' s+ k+ P/ V0 a4 ]- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# E; |' G; s# @ - LOW_level_read_time = micros();* d& L/ m: j3 i! g% ~
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
. V! g+ V) a8 C& V* {* I0 n9 p
# ^6 _2 c1 t; Q( J1 |- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1. k7 D. D6 S" q4 j
- {
+ r4 L9 B9 @+ i4 E) { - Sensor_Data[k] |= (0x80 >> i);! P- H d6 w$ ]5 {# W [7 j
- }
4 A8 T, f$ {! J7 I/ B3 w - else if(HIGH_level_read_time >= 100)7 A- O+ _8 K: _( q, R/ ?* @
- { k( A' d" W" a, D
- return 0xff;, Q) Y2 X2 u& f0 `3 N' f
- }
5 J. E' O0 `; }) h
; I! k) ~ t0 x$ |- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out), {, N( c; f8 q+ |4 W; `2 ^7 T5 R0 ]
- {
2 }' [0 @% n1 S- v3 b* R; _! ~ - return 0xff;
8 }6 i: S% U. @) o8 J - }. G7 ~( m. a* P9 A" ?! f4 F, j) T
- }2 K& i# u& t% v
- }7 W% l! m; |. k4 r6 _
/ R: G. L; U k- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level' d; ~+ T- |9 O; P9 a' H( N
- HIGH_level_read_time = micros();
% A5 \/ F1 ^/ ?7 w/ D - LOW_level_read_time = micros() - LOW_level_read_time;! N1 B# J7 v5 I/ x6 d+ H8 r; |
* ]( g. [. N* t2 _/ ?- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
" G+ _# F7 I4 q5 ?8 C - {
" Y c; [. I6 R6 }+ g4 p - return 0xff;
( H' P1 `2 r; C. r5 `2 ~ - }
9 s( z- P/ z" n* I" U' w# a: M
% T$ I4 f N& F9 V, O- pinMode(_DataPin, OUTPUT);
, v3 D4 g2 q" Y - digitalWrite(_DataPin, HIGH);6 f1 u+ T/ {1 D- r. {( T' F7 U
) e( Q: o# r7 Q4 C. _- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))4 ]2 u0 S8 m! C3 y& u5 Y+ k! E2 V
- {+ G( h% H+ U) B: D# E$ `/ g1 ?
- old_data = Sensor_Data[0];
+ G) K5 h3 e- {/ K Y* e( B/ U) Z* x8 f - return Sensor_Data[0];
- ]! q3 X2 S0 b; j8 z& s( h7 [' H - }7 o* ^9 _4 a, t, k9 X4 N" X
- else
2 m5 J( H8 q; z - {
6 G3 w9 } D8 L5 { - return old_data;
; C) e. t' K' A$ ] - }+ a) O" u8 v+ o; R# o- f
- }) T2 v' X- `7 h! p: f7 B1 K
複製代碼
# _: m( ?" x+ a7 L) p# MMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 L b/ A* j7 e' Q V$ q' m4 k5 ?2 I" n. W7 Y2 E
|
|