|
|
! k3 L* _5 @2 x6 N1 r4 ~" }% \
- #include <Arduino.h>
2 a! d# P) H' r$ D6 y9 Q/ e - #include <MeAuriga.h>
( \& F( I6 y1 E1 A - #include "MeLineFollowerArray.h"# C6 P2 f0 T- f! @! [
- 4 l% L; d# Z$ ?, t% Y
- MeEncoderOnBoard Encoder_1(SLOT1);& L# \( y( G" Y [- M# U
- MeEncoderOnBoard Encoder_2(SLOT2);& I6 K* h( Y1 W$ m
- MeLightSensor lightsensor_1(12);. B0 R% i# C6 b2 E# h Y5 {
- MeLightSensor lightsensor_2(11);
2 L, o/ d O+ r9 V. b - MeBuzzer buzzer;
! ?5 g5 H2 U4 ?$ \, l' ]6 J& [6 ~ - MeLineFollowerArray linefollower(PORT_6);
: t0 N' o. F# i1 K, l. N9 ~) g0 }
' G4 h; t/ u5 o0 \- #define Error1 19 Q" e" `8 r, R5 Q' t7 x, l8 Z1 R
- #define Error2 2% I# L. H: s9 X) o
- #define Error3 3
' v3 A- K. y+ `" y& G# R
$ S1 t: d" `! j- #define Kp 15( s" J# U a: c5 q |
- #define Ki 0.15& A9 a4 x6 p$ _9 n' J/ e
- #define Kd 0.03; M6 j: M7 W1 O+ b+ c6 c# L' ~
8 q4 s% \' B. R: @% X- uint8_t sData;: B" a, S4 [3 m5 e
- uint8_t D1;
& Y& X0 A$ k# g6 v9 ^- @+ V9 ` - uint8_t D2;# y5 g+ R9 J. `6 z9 C
- uint8_t D3;
* B& A9 \3 Z4 g# q6 Y1 n - uint8_t D4;
( C6 l) M0 j9 _5 B. y- M - uint8_t D5;
7 d# t+ C. X: n7 |# H - uint8_t D6;
) g4 {- D, R. ` - ' F% e( E5 T3 E3 d. ~* A" `6 g D
- float previous_error = 0;' _) `1 o+ U+ x7 f) v- ?2 X$ T
- float integral = 0;
6 n6 g4 ^9 A% ^. D" x9 R6 q - float derivative = 0;
, I3 P4 `) n/ L0 z0 D - int Speed = 160;
1 D& l% [( m1 K* ^ - float output;
+ {* o( Q9 ]8 e" D* v. B/ w3 v - - q7 n: r" C6 x
- byte Left;
! n; a2 n; @8 H7 g( T8 H1 l - 3 x$ R6 E- q b% _
- void setup()
* ~: K# T6 d* J$ h$ M - {
1 ^5 l7 F9 \6 v - //Set PWM 8KHz9 i2 N7 y7 }" B$ \2 e
- TCCR1A = _BV(WGM10);* U# o9 ^4 u- Q. u% M- D' p% y
- TCCR1B = _BV(CS11) | _BV(WGM12);
8 g1 \- }& K+ E2 P) P' m - TCCR2A = _BV(WGM21) | _BV(WGM20);
~0 M$ W$ N" L. J+ T - TCCR2B = _BV(CS21);
3 X6 U9 _1 |7 x7 {6 D! @0 e - Serial.begin(9600);8 ]" U& V! U# `1 c0 y
- buzzer.setpin(45);6 y1 l. R5 S2 O7 c# q$ F' k
- }
Z: r0 ]0 A2 ^0 G - " q$ ?( W8 w, q
- void loop()
( V" {% F t0 z8 j6 B& I - {) B; l% m- N6 e& T d- a7 j6 [
- while(!((lightsensor_1.read()) < (10)));
' q( ]$ N$ W& H$ H: b - buzzer.tone(1047, 500);
( z( F4 o4 ]: {/ Z/ _ - delay(500);
0 M5 U3 J* h* g; L' V - do
& h$ x+ k& o' C8 ?6 a* S7 y& ~ - {
% ~+ w1 y0 {! S- t" g0 a( O - int Err = getErr();2 j3 l- ^# x K9 Y& X+ E
- if(D1 == 1)
- I8 R4 f' O5 Q3 v \ - {
$ i; g9 W, r( X( h3 ^& R - Left = 1;
5 l" g# {; }3 n5 p - }) L) a* m7 q3 B: x7 x, b& h. G
- if(D6 == 1)6 q `! A* S' a2 ] V
- {: k6 g, a/ `; o1 H5 {
- Left = 0;; e8 x* ?! w8 W _" @4 ~% i/ M
- }
1 O: l( c3 w, T5 e' k' X, l6 Z% M" P - if(Err == 99)
0 @$ i/ U1 F0 h$ B - {" H% ~$ m! w0 q+ N, E
- if(Left == 1)2 x8 J2 K) J+ N/ |' j- g( R7 q; ?
- {" `1 z; e9 b3 m$ m3 [
- Speed -= 5;9 x& L3 S' J$ ^. N
- moto(0,Speed);
( d0 \, }$ n A. F3 I - do* z8 T' E5 M8 L5 H% P9 k9 z/ X+ u
- {
, U. b( I" F. U- E: {9 Z( z - Err = getErr();% o2 G1 ]& K; _! s9 H
- }while((D1+D6) == 0);
% M; U Z: U/ @$ S |* q - }& t2 Q* q! y/ U: m* P: v
- else
. l7 |5 m3 f, y6 l( l" R! P/ E) {$ ] - {: b) `8 \( I {3 X2 P
- Speed -= 5;
* A$ w' D% |$ [0 f - moto(Speed,0); ( C4 o( m$ q( @7 F
- do3 ~' F+ J" W3 Z9 ?
- {/ j7 {) h8 H7 V( S" s
- Err = getErr();7 t0 {/ p1 s; |7 b) [
- }while((D1+D6) == 0); D1 U7 M5 d6 ^# E
- }
7 v" j2 E3 @8 C5 Y. u - }
4 m2 M$ [' T6 ?3 G6 e) m# | - else
# M: D& t- a$ p6 a. X& o - {4 b. E& J- @8 @6 p* A/ I5 V
- if((Speed < 160) & (Err < 2)) Speed+=1; " q' d6 J; H$ m
- if((Speed > 100) & (Err > 2)) Speed-=2;
6 r3 h5 n, _7 Z& w - integral = integral + Err;7 O1 X# h4 z: v4 `3 C
- derivative = Err - previous_error;9 m9 `, `( ^7 o# v. \- }
- output = Kp*Err + Ki*integral + Kd*derivative;/ {# v, v# `, G& {5 a2 d$ E
- moto(int(Speed-output),int(Speed+output));. q: x" {+ K7 e* f4 W: |2 S4 F
- previous_error = Err;9 P5 G- `6 q' b3 e
- }
$ M$ }+ U# F! C5 u& b& ~ - }while(!((lightsensor_2.read()) < (10)));
3 j0 \ W$ `3 c j6 H. Z9 w+ @! i$ y - moto(0,0);
; R! V3 W0 ]) }& _' C Y5 z - delay(500);
. r( h; i# p' L6 } - buzzer.tone(262, 500);& J0 q8 v% `. X% Y1 @' v, J
- }$ G/ g# `6 A" ~$ S. U
- 7 ]/ N8 L8 [+ x" p) b9 j1 [2 n
- int getErr()6 H' `" ~- A( P6 N) Z
- { " {$ n. E2 U$ |; p. @7 A
- sData = linefollower.getValue();% `7 C5 H$ c; H7 H8 c: ?+ q
- D1 = ~(sData>>0)&1;
4 r4 Y& e- `9 w - D2 = ~(sData>>1)&1;
5 }3 U2 F4 q( _ I( L) d- {) V( i- H) O* G - D3 = ~(sData>>2)&1;
# ]+ O; {0 T" s i/ b7 x - D4 = ~(sData>>3)&1;7 Q. `4 K3 E) @1 a$ E, y; I
- D5 = ~(sData>>4)&1;7 m0 }6 j3 d8 D& W& l6 d* W
- D6 = ~(sData>>5)&1;- Y9 V. e+ W- t8 @8 _' G# f2 O% W
- int downD = D1+D2+D3+D4+D5+D6;
; s; K/ F, J1 j! A% _5 c - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
' I) o; i7 r. \0 H - if(downD == 0)
7 H! l; d0 s4 G- y ^- I0 C - {
. T7 A6 [9 r: ^ - return 99;: q( P0 F' [" {: n! d2 ?$ G
- }( X& o7 I; m1 m$ [( o
- else
9 O& T6 _: Q: e* \9 `" b+ E - {
, ?- U) k, u1 B. ?/ Q3 ` - return upD/downD;2 w8 U0 B# J+ B7 t I( v( {
- }' @! |* R! J) r
- }
6 h* A3 q. k* I9 G. \/ @. ]5 T
5 o4 i; a! e- u- void moto(int Speed_L,int Speed_R)
4 n3 G; x- H$ ^ ~: M0 S - {
7 [8 y4 m, \3 A, W. s; W# }% [1 N - Encoder_2.setMotorPwm(Speed_L);
) p0 [1 z0 m5 k1 Z `4 t - Encoder_1.setMotorPwm(-Speed_R);: V2 \3 S6 } R* O" {8 j
- }
複製代碼
6 V+ f8 L) ^6 [7 i6 Z& c, W" j! lMeLineFollowerArray.cpp
/ w' X" F6 I+ O. b/ V9 j- #include "MeLineFollowerArray.h"
4 u$ l* ~4 p) c$ F9 ? - ; u4 A& }! m( l7 Y3 s
- #ifdef ME_PORT_DEFINED
: Z u4 n ]# u& z) n$ x - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
# r4 N& M& [- W - {/ B) H; M( K5 y/ |4 ]8 c
- 5 b" `7 h8 J- ^/ P3 S
- }
6 z1 Z- m5 z9 f: {/ j - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)- S4 T V! t1 u
- {
7 `* p6 t0 B' l. G, V - _DataPin = mePort[port].s2;
5 J# T/ e4 D* S; m4 ?7 b - pinMode(_DataPin, OUTPUT); " l0 A, C5 @) A, J! c; ^
- digitalWrite(_DataPin, HIGH);
" G4 y( v1 q' N6 S$ L0 b- I - }
8 B( W, q, j3 C5 U8 C4 T - #else // ME_PORT_DEFINED% d' V* A( Q; a5 [
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
( k3 W8 ]* ]* M0 c( g5 L( i y - {9 n/ o( x) N8 s5 @9 Y! i
- _DataPin = pin;
1 z( v/ W0 \6 u( U, x7 X8 G - pinMode(_DataPin, OUTPUT);
2 [" w# ^4 v/ ^* m! L6 w - digitalWrite(_DataPin, HIGH);# {" D' X7 J. z; q' K5 K+ E5 p
- }
6 R9 {% x& e2 J* N; {; C1 Z4 e - #endif // ME_PORT_DEFINED8 M( S$ c3 H+ f6 L1 H
- 5 |$ x# i# n1 X" H/ y1 V- p3 H
( f& g7 ]2 y7 ^0 p. ?( ^$ ?- void MeLineFollowerArray::setpin(uint8_t pin)) |% ~- I) o2 y8 }8 b
- {
$ t: B; u- s Q2 k/ e - _DataPin = pin;
" @- v" U! \% P3 `+ a, x - pinMode(_DataPin, OUTPUT); ( h: y, t5 F: B
- digitalWrite(_DataPin, HIGH);8 C E; s2 L) G) z
/ {! e+ n/ R# d6 x: d. \0 i5 S- #ifdef ME_PORT_DEFINED3 c7 X2 s9 K$ O: Y1 Y0 L& T1 F, V2 e6 P
- s2 = pin;
" F( P& a' |' r; h$ ~ - #endif$ @" a+ b2 U1 m3 P' c) r
- }
4 p0 B9 ] [+ ?, j5 }* i
8 A, p2 _4 f: l- ~! c% K7 m- uint8_t MeLineFollowerArray::getValue()
8 c- k+ S& ?7 N) h7 ^ - {
: ^2 N) {# l: \5 y! S7 G' d' t - uint32_t LOW_level_read_time;
/ }( m4 a2 d& ?( j) z/ E3 ` - uint32_t HIGH_level_read_time;' p, s) l) z: P5 R3 Q$ S$ K
- uint32_t time_out_flag;" _4 X T; v6 _1 N( C4 |
- uint8_t Sensor_Data[3];
5 Z9 Q" C7 Q! |3 ? - static uint8_t old_data = 0xff;
# \: Q& [" E" o8 `; y* _0 A - " [, c' a, w% e v) R3 f4 Q; |2 X" |
- pinMode(_DataPin, OUTPUT);
0 J. R3 q1 ^5 M1 m) K$ z! S, A - digitalWrite(_DataPin, LOW);
% u! I. _, D6 |1 G- V* k - delayMicroseconds(980);' w9 o5 L* h; _+ F
- digitalWrite(_DataPin, HIGH);6 X0 `4 I1 ]' c- o% C5 R& V
9 u- y5 F: U8 S6 I! e- pinMode(_DataPin, INPUT_PULLUP);5 K' c8 e2 l) `# i9 F6 \
- delayMicroseconds(10);* `0 G9 [7 K1 t; a
$ s) i* n, m- l3 M7 e T$ Q0 B- time_out_flag = millis();! k( ]! X" g+ } e6 |1 R3 i! `
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( ~' ]2 O; R$ x7 x' \ x( d - ( @5 @2 i" T8 B
- LOW_level_read_time = micros();( O+ Q# v9 _- U, u7 v- j0 w& {9 l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out; ^/ R% @# y9 p
- {2 K: W$ J# ]- f/ {) X8 `
- return 0xff;
7 q: N, @; _/ Y" _2 ~) x - }
7 Q2 R+ R1 J) A9 i$ |- H. I
; P( |% y+ J2 g" e- l. B- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 I- u: @! F2 |
- o: ^7 u: g |5 J' Y8 s- HIGH_level_read_time = micros();
' Z' L! p! s Z: l# V) {8 ] - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level* b1 c; F+ d1 e$ a' j1 q" j. x/ ^8 I
- * T6 a- |" R9 v
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 C9 W! a e9 D7 b( Y" `
- {
% [( e8 |; {/ _7 Q7 e) m - return 0xff;- Q7 K; c5 f9 v1 ] O5 D
- }
V+ b7 D2 v. r) C! z - ) e- h+ O" v) v4 C/ e; z5 x
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
6 k, P/ R0 |% Y$ d5 z7 q9 P- n v - {4 ^0 g+ F- o, E& w" K
- return 0xff; w; d; K1 K* o
- }% |1 f; H* R% @, J" _
. {# ^" B: Y6 v- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* X! x7 {( m" T& ]6 \ - LOW_level_read_time = micros();
. L5 J" L2 b- V* v - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
/ n) t* ?8 D5 E$ ~# Z$ a - 7 J" ?4 B8 |- `8 P: o8 H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out- u/ p- T6 E9 B' S; } R5 u
- {! U$ N9 b! {( b! E! j
- return 0xff;
5 m B+ A3 ^! i1 `: Z - }
( x# V, S3 D8 p( G q4 q3 x - 7 K$ h5 Z3 t y, {+ S& k0 L
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
4 Y, b2 b! u" A0 V1 P. ~% ~ - {
& P8 J( }+ `. c$ O$ S' j9 i" S - return 0xff;' [8 B k ?6 \
- }
! _1 @$ U4 Y* c1 O! X) \9 e# V' n - / I S l# u8 g. C) Z: C2 c
- for(uint8_t k=0; k<3; k++)* H9 m7 p/ n6 V2 Q2 K
- {( ^( E: K3 ~4 i6 I& Y& ~
- Sensor_Data[k] = 0x00;$ }8 J7 D f$ i1 k" [) a, |
3 N9 n6 j! r% j8 O- B. v- for(uint8_t i=0;i<8;i++)
7 S+ l% b P. E/ ?1 ?% Z - {- Y, k5 B3 z0 V) `
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 M. g3 G' L" K! N4 @: p
- HIGH_level_read_time = micros();
. c# c, j1 U+ O* O7 S - LOW_level_read_time = micros() - LOW_level_read_time;( O2 C* G1 a- L) R% e$ B* J
- 7 G; ` N- x, e+ i
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# P! Z* c& Y7 F! ?+ z
- {
% }1 R) [. B' v0 z9 J( b* o - return 0xff;9 j7 d0 f9 O' X% s
- }' P, X7 Y% u! ]& P; W5 q2 d% S( |
- # }9 N1 \/ h' |0 F4 A" ?6 {
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
8 E4 o$ r9 L0 K* v$ h: h - LOW_level_read_time = micros();
. ]; k0 A2 f# O8 A7 m - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level, c& n v4 f0 T' f( R2 q5 V
- % u1 h# Y! `& n8 }0 r2 d( }7 `
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1' s6 Q) a4 a) Z9 X& c" ~9 j. {
- {
: v Z8 e" J7 r% s! M - Sensor_Data[k] |= (0x80 >> i);5 Q6 @+ c3 k; k7 t+ `7 b
- }
2 N! X) j7 ?3 z9 s/ B - else if(HIGH_level_read_time >= 100)
% _2 I7 u& T: C - {
* x7 ?5 O# t4 y* G) m: j - return 0xff;8 D) b; ], \1 i3 b
- }
+ Z$ }0 W! ? X7 T, H* t7 w
8 m' X: }* T+ {0 Q2 `* a- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
9 c- m3 _2 X, ~5 k3 O9 ~ - {
9 i9 k+ W4 k: b6 r3 T - return 0xff;2 g2 v0 z$ ~* t
- }
. q3 E# p0 q. k- |/ ~ - }/ q: D4 R: f# f! V
- }
( y+ Q3 G+ x$ _% e - 0 w/ i) }( J7 I6 K( g! X2 M4 W
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
/ N& Y: G& x+ P3 O$ \" b - HIGH_level_read_time = micros();. o# }+ P+ q( L5 Y q# T) C
- LOW_level_read_time = micros() - LOW_level_read_time;
$ \ e4 \! f8 ~$ I' G8 i$ A' ?
1 v7 h m) a6 P8 x* s- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& K, H: L* i$ e# g# s - {
/ a: h5 \* S- d5 V W4 | - return 0xff;: l# Y" b T3 R
- }3 W( K7 a, y: Y( ~1 m& p7 [ I2 B P
- 8 }, l& s5 f' r+ S
- pinMode(_DataPin, OUTPUT);' c A( b! d" O
- digitalWrite(_DataPin, HIGH);5 Y; C. F7 M M& G" \
- : u7 W2 b) w% @" r" y* M+ b0 P/ E
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
! W _1 J7 n! g# ] - {
7 V2 j, ?3 j9 H; }8 [ - old_data = Sensor_Data[0];6 X; f- v# W: \5 Q% h
- return Sensor_Data[0];
/ y' l) ?; m& i+ Z. I# E - }7 {- s+ {8 S/ m5 e1 V# |% U
- else% Q$ i# H3 m/ E1 Z2 B4 _$ P. R
- {
+ f4 b% c7 X& M5 X3 q( O& } - return old_data;9 W* T+ x" [/ c' x
- }" M2 q% k2 F9 R, y
- }
- g! c/ O6 J1 C7 t. \6 u7 S
複製代碼 " d0 ` r! e7 e# R% e# E
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
1 K5 {: U- [/ _) }7 Z) d/ q3 o: l9 o8 ~
|
|