|
|
5 k0 D! D: W5 N. `
- #include <Arduino.h>
; y3 R1 z" V6 l4 c' ? - #include <MeAuriga.h># t, b& N& \1 H- S& S
- #include "MeLineFollowerArray.h": I/ I4 O8 }7 k" d8 c4 C8 l) Y
- 8 r2 U1 c+ g7 [
- MeEncoderOnBoard Encoder_1(SLOT1);: H8 u. i* v/ \: w
- MeEncoderOnBoard Encoder_2(SLOT2);2 g) V. t0 x% T& `6 y
- MeLightSensor lightsensor_1(12);
3 }8 g5 R( O" U - MeLightSensor lightsensor_2(11);
9 g) `! v. g- s% w- z - MeBuzzer buzzer;
4 ?1 N1 @$ |3 o% m$ g - MeLineFollowerArray linefollower(PORT_6);; i- j4 v% i) {7 c1 v
' G. E* N0 P8 {2 ]0 K! Z0 }- #define Error1 1
5 @5 Q& f2 q. z0 X! g - #define Error2 2
; Y s" v+ [1 m5 `! f, y/ y" F - #define Error3 3
* Z2 p, n( ]1 ]9 g" @$ s5 l6 y# ?
- n, }0 L1 T" `/ R+ v1 L4 F- #define Kp 158 V# w+ ?9 X0 a- d
- #define Ki 0.15
! d. \/ M$ n5 n1 h: m6 W- T - #define Kd 0.03
7 L0 V0 ^2 U5 B1 [4 A
0 }5 ]' C8 w7 c/ K7 ?- uint8_t sData;
# A8 ?0 x, V5 I7 R - uint8_t D1;
6 @3 {- {; K0 W& q, p - uint8_t D2;. p* [$ t1 R! J
- uint8_t D3;! O0 ^. u7 b+ Q& R0 n- Q( h" n
- uint8_t D4;
2 N G7 C( Y7 V- ^ - uint8_t D5;2 T E. _% a6 ^4 h G) v
- uint8_t D6;/ K: O/ y8 j9 y( p- I; b
4 F2 n& A, `0 J( C4 b- float previous_error = 0;
, s% V) O2 Y8 e. k+ i: R4 P2 E' l - float integral = 0;
$ q2 D) t6 @ [7 V - float derivative = 0;. | a! W* P& l6 `
- int Speed = 160;
+ ?1 }& b2 U. V' Q f7 X - float output;$ m6 f/ F) V l0 b
' r0 H2 A0 v' @ r- byte Left;) C( ~& B6 m- W6 b# A# v
- . m+ E) z+ t7 ]8 f
- void setup() $ m: s3 x8 q: O5 x8 J. c
- {
5 }; j$ b. O" ~' E: x - //Set PWM 8KHz
; ^* i* J" m; Y' |9 t - TCCR1A = _BV(WGM10);7 C. v1 f6 C$ \; B4 e; n! c
- TCCR1B = _BV(CS11) | _BV(WGM12);
d" G. Z7 z, H& ], f3 a4 }7 s - TCCR2A = _BV(WGM21) | _BV(WGM20);6 p* D, @# p+ y- ~: v) C4 C
- TCCR2B = _BV(CS21);$ P1 ]1 Z' V7 H9 r0 f; D6 j, q
- Serial.begin(9600);
. ^2 w; a; h) i8 O3 Y0 H: p - buzzer.setpin(45);
4 _! R2 z; C$ {, r4 ] - }
; T3 x/ |6 n: B8 [- z) Q - 6 C: g/ W% I5 ]* O. \ R* N
- void loop()
& h2 o; D4 L6 w! O5 [: Q' W9 E7 V B - {: E" `+ H) A4 T. Y3 c
- while(!((lightsensor_1.read()) < (10)));) S+ ^% W8 f/ I" i0 `, J
- buzzer.tone(1047, 500);6 ?% u& S! {$ @2 l, g- d$ C
- delay(500);' J0 r" M6 K6 o9 s4 s0 u4 e+ N
- do9 ^& M; y& N4 x. x0 v, y
- {5 Z* O- D9 E% w o
- int Err = getErr();! W' e# S9 g- ]7 _; z
- if(D1 == 1); B F2 O" b$ z: D
- {* C8 o$ b) W Y. r4 L
- Left = 1;
7 r! X& m: E- q - }' J. m: j( I, ^6 S$ v" B8 o: Y
- if(D6 == 1)! W2 U3 j4 h9 y! j$ b# L
- {
# D3 n, O/ b: s, [5 s% d$ m) v/ z - Left = 0;
! a# ? g* Z4 z) M: J: |! b - }
$ w2 p" S. ^$ P6 \ - if(Err == 99)
# f8 w1 X5 V2 G( N - {
& B8 g) {. V' I8 p& |! F - if(Left == 1)" Z3 Y0 Z! ~" g+ I5 L
- {( d6 D/ t" V1 i$ G q6 d6 A
- Speed -= 5;5 |. _! j- z7 k0 v1 Z8 k8 w& P
- moto(0,Speed);
3 i( X4 N) j# y - do
; c4 ^+ T$ |; ~ O9 z) x& a8 G - {
% |% M- B/ K, Z3 m2 t - Err = getErr();
9 T4 f" ~3 e- U/ z* U - }while((D1+D6) == 0);
9 A7 v5 A/ q; Z$ M* J: ] - }: l5 r8 V7 c9 R
- else
) S! ~& |" ?4 ~! a; K7 H" S - {
* C& K. e: L9 |0 H1 r& P1 ^# V - Speed -= 5;
8 I% T) O5 w3 j# N5 k6 W - moto(Speed,0); 9 _5 L) L" D5 U: M4 z" ?
- do
8 Y3 U1 c0 R' ~% ^8 A6 S( t& ] - {
' D7 _8 p9 }$ H$ P - Err = getErr();
7 Z2 `3 t' c4 \ J - }while((D1+D6) == 0);1 c" h6 w- H$ `2 n( P
- }
5 U8 R7 f( w5 [+ l - }: W5 b7 k7 f6 T# V, Z1 k
- else0 W2 ]) q M7 X# l: o
- {
2 H. \. ?! Z+ L - if((Speed < 160) & (Err < 2)) Speed+=1; 6 r. Z% Z! O) s6 [$ Y
- if((Speed > 100) & (Err > 2)) Speed-=2;
3 \2 ^( }) k& u4 a8 y - integral = integral + Err;
/ A9 ?* O6 ~ b: T - derivative = Err - previous_error;* T- L9 m1 {; H, z- a# _5 Q
- output = Kp*Err + Ki*integral + Kd*derivative;
2 g. M6 o& ~ K, r2 l& G - moto(int(Speed-output),int(Speed+output));" u5 D' [, k3 B( y# e5 r; I8 y
- previous_error = Err;8 E( T. K' q) W2 Y [
- }
* u( P6 ?: Q9 J) x - }while(!((lightsensor_2.read()) < (10)));
. G7 d: S5 h9 @; {/ l6 N - moto(0,0);0 V; ]" v* V, F# W5 w
- delay(500);% s0 i2 ~& t, i
- buzzer.tone(262, 500);# x3 z4 u" Q" M. H: k+ C5 V
- }
9 w8 r$ v4 X! E8 y U* p1 ^ ?
& j$ A( Q7 ^# x* o1 m- int getErr()0 v6 h# Z! t' _1 Z" R9 l0 Q
- {
; G q) ~, f( Z. O% @ - sData = linefollower.getValue();
5 O' j( S5 w9 g3 E9 ^ - D1 = ~(sData>>0)&1;, K) I5 G4 F6 f8 d
- D2 = ~(sData>>1)&1;: P u0 c Z" a/ O
- D3 = ~(sData>>2)&1;4 U. U$ n& N5 R, v/ q4 c
- D4 = ~(sData>>3)&1;
8 m- e( K# N! r" }0 O - D5 = ~(sData>>4)&1;
' H. U8 e) C2 t - D6 = ~(sData>>5)&1;, v* Q/ p/ I6 b
- int downD = D1+D2+D3+D4+D5+D6;1 U3 Y& b+ a8 s7 B/ D' M
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
7 v# t& L/ r- [ - if(downD == 0)
5 A0 |: M/ g7 o# |1 W1 C& z - {) U! c0 Q: {. |# \- o
- return 99;2 h5 d2 C4 B$ R+ ~( d
- }- n* I% _. A, f) `
- else
) `5 z+ W: m5 e( A) L - {7 ]7 S7 \. H X( p4 Z# G5 t/ W
- return upD/downD;6 q `1 |9 }: P2 t
- }
; z: m. w+ U. [7 L. n6 S - }- Y) V2 X& c: z2 c6 L& @
- 5 X7 B* G& ?- T, k5 P9 O0 I& c
- void moto(int Speed_L,int Speed_R)
2 E' S) b: j$ q; W - {
! e+ n: w& A7 ^# L. R3 t% [1 h3 [ - Encoder_2.setMotorPwm(Speed_L);
) Y- T9 A# k$ w" k - Encoder_1.setMotorPwm(-Speed_R);# j; D0 \2 w$ U
- }
複製代碼 # `) Q1 c$ M% K% o; k
MeLineFollowerArray.cpp, K% t8 k! q; s6 Q. ?& ^6 S/ y
- #include "MeLineFollowerArray.h"
( s" H2 J2 A: z1 _4 M - 1 z1 M; J8 L) q0 E
- #ifdef ME_PORT_DEFINED/ c- H9 r6 Z3 R4 s
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)! h( Y$ D; m4 q
- {
W! h7 R) }* B) _! \3 R; c - $ g/ L( y6 Z/ L. ]5 K
- }8 R, \* L6 x' J, G
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
7 | M+ q* X/ Q5 \! l7 I# }: B0 F - {
7 z" J5 q; e9 z4 C0 b# u - _DataPin = mePort[port].s2;
# C0 N% ^1 J7 n2 I# e) \ - pinMode(_DataPin, OUTPUT); 7 m& }) P0 u7 C8 i0 ?4 g6 H/ n2 X
- digitalWrite(_DataPin, HIGH);
2 [- }7 t1 K6 Q) b( q2 q( @ - }
# H" e j/ X6 w- [ - #else // ME_PORT_DEFINED0 X; e: e6 O7 E/ M" N
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin), s2 i6 Q( a% k) N1 u
- {
8 x5 Q- k: _1 d - _DataPin = pin;
# g; ?5 `8 J" D1 ^' y4 z - pinMode(_DataPin, OUTPUT); : G. E9 y/ N; x) j2 x
- digitalWrite(_DataPin, HIGH);
; B5 z( }' K( j) l8 [( m \, S - }& M5 L+ R+ N& j' K
- #endif // ME_PORT_DEFINED" o4 Z8 `( H' u0 |' T9 A" A$ D
- 4 b- w2 k' l4 Q1 V
- 7 V: |. u3 R- t+ |( I v
- void MeLineFollowerArray::setpin(uint8_t pin)# C& D' Y6 Y: c; T$ E
- {$ @) B* i, J+ g- t4 }) H
- _DataPin = pin;
]' {3 i" h" i6 {. O - pinMode(_DataPin, OUTPUT); 0 a* T) U& L8 m& q$ I" F z
- digitalWrite(_DataPin, HIGH);) D: O( e" U- l- {: I. Z* H
- ( d' {$ |! i6 r4 `& Q/ _7 i
- #ifdef ME_PORT_DEFINED5 J+ e+ C# O7 _# F/ J
- s2 = pin;+ ^5 E, c' X% l; Q0 ?. H
- #endif* s/ }( n7 u/ i" Y: }# S x9 T2 p
- }
* t. h" |; L5 f, U* y8 l. [
' s) z* e; q( [& B9 J& Y9 S5 m- uint8_t MeLineFollowerArray::getValue()/ M5 ~6 T- h3 G, c
- {7 X% P2 M8 y; B2 l, U8 f8 i9 f- O1 J, M
- uint32_t LOW_level_read_time;9 ]# z- f# S- T8 L0 ~: T$ k8 M4 Q
- uint32_t HIGH_level_read_time;
0 J* ~+ b# j# g y - uint32_t time_out_flag;* l8 ]& ?/ r0 J+ p+ g
- uint8_t Sensor_Data[3];1 E/ \/ ^& I9 G# S( u
- static uint8_t old_data = 0xff; }3 ]4 A0 F: g1 d0 Q
- 8 E ^$ k4 z9 m3 ~- s5 X j
- pinMode(_DataPin, OUTPUT);
6 S- L* t) Y% c$ Q - digitalWrite(_DataPin, LOW);
8 d( p& O1 v2 T3 Z3 n9 @: G - delayMicroseconds(980);1 |# g8 g: Z+ A! n, x
- digitalWrite(_DataPin, HIGH);3 ? d: f" K$ J- W" M2 _
- ! E) O c/ n0 M1 M1 e" M4 Q" h
- pinMode(_DataPin, INPUT_PULLUP);3 X; k. e. A+ V! S" }
- delayMicroseconds(10);
, C. x" p9 }4 e+ ] S
9 o, [) J! C2 V+ [- time_out_flag = millis(); L/ I2 R) ?, E2 d
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; c6 [: `: N8 s0 D - * C/ Q1 ~$ F% t6 A8 {
- LOW_level_read_time = micros();3 P& P K, Q0 O
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
1 d3 y6 E/ B' e% `$ \. E - {
: F& x5 h( ]; a- u4 e - return 0xff;( X; z2 N. c4 |# |; ~( x% U
- } e0 U( Z/ U, a
" @) A1 [ _7 y3 L, x. l- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 G1 a9 N) a, L5 A( \" D
- ! o* G( r* [) O) H
- HIGH_level_read_time = micros();$ w( E" X6 [ o3 |
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
) c+ h) l. r$ y$ E/ H8 }
' O5 S' c" O3 e, N# t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! I* u: G+ X# O& }* F F1 J7 D
- {
$ c/ w I2 G6 r/ G - return 0xff;& u9 i$ _5 a0 g8 s! P
- }( f. H4 k; {6 u3 k9 ~3 }; |
1 j# G; k6 ]1 ^+ l- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))3 W4 o6 F. X3 m% u0 i
- {
# O z* U1 A0 k2 B, r2 L - return 0xff;
8 R3 J1 p* ^: @. p3 y( t - }) H1 e" ~' P) S
- 1 _% d2 _2 Z, m% |! I
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, X7 m7 v. N' v+ Z2 ^' p, f! G
- LOW_level_read_time = micros();
5 R r3 S7 O2 U8 J e - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level8 B1 P, f, Y, J6 i e2 @
- , c1 [* x6 D- a6 z/ B
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out1 _1 ]* x) j( ]: h& f
- {
7 v ]9 }( h* |' I' _2 s - return 0xff;
5 s, z( v( u, l/ Q$ A - }
- R9 {3 ]& M4 G) q4 w/ p h
$ d( v1 y L- l5 y, ~5 E; Q- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))) w% E; P; \' w; m9 j# W8 T0 a1 x: f# ?! |
- {2 i$ k. d4 ^/ ^+ v8 e. M" j9 j
- return 0xff;3 [; E2 X! h- z8 j2 |: o$ t8 n# g
- }
; y+ ~2 P: M$ W* a% ]: o% v( {' @
, U; U" A4 M- h# d4 V& A! K( S- for(uint8_t k=0; k<3; k++) B- E |! \$ t7 Z4 \
- {/ ?: S C0 d3 P6 D% n* Y
- Sensor_Data[k] = 0x00;8 K- N7 G! W( b( @ T# B
& ?# x( ~+ s* p$ G! J" [0 R7 j- for(uint8_t i=0;i<8;i++)) ]% S) ]* D$ Z. b
- {
4 N) m% S* R% K - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
`8 h" m. w2 }. O - HIGH_level_read_time = micros();2 k4 Q' S; y, J; p% f
- LOW_level_read_time = micros() - LOW_level_read_time;
- [6 X* O7 |7 ? - , C/ p6 W6 K& G! |# U
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) X+ Y/ X5 t. P0 H$ h
- {: i' _$ |0 Q. j8 B+ d$ X4 h$ \2 m
- return 0xff;
+ _; W* `% ?+ I# U$ d - }) {/ e9 n8 |- N1 ^
* O8 P1 e0 s2 k0 l3 I6 c- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. F1 _2 y; G( G L c+ C - LOW_level_read_time = micros();' I6 X( X" E, ~" ?) r% _
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
0 S7 N0 X Y8 a3 O$ M - + }- w1 o0 I5 @1 X
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 18 S5 \4 G& D# ~# t8 g
- { o* o0 ~- l# O5 o0 t6 n' e
- Sensor_Data[k] |= (0x80 >> i);$ D' o0 y* E8 h
- }* B. G& a( z) \ |. F* ?
- else if(HIGH_level_read_time >= 100)
" t/ r+ ^, O; T a" v- Z. r% Q - {) e* W" G) ]# d+ l6 M. p7 t
- return 0xff;" o2 M8 P* }/ r0 `1 n4 p9 L
- }7 ^. i" ?0 E! \# W
- 8 o; j" Q; K) m" [2 v: R% X
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)4 p, L/ ]. }4 f- M6 S! A M# i$ [
- {5 r0 \* Z) O2 l+ j7 b5 _% W: o
- return 0xff;- _3 G! _, t/ Z' j; o9 X( G) A
- }5 t# ?7 r5 ~2 L; r B; Z
- }
2 L( m O1 b k: R" W! p - }
! i1 s3 Z) C ~% `/ L( u( n" V4 a
1 V1 J: K1 N* ]* V- q4 R( G- H- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
3 t! t+ O0 u4 u; I8 Q - HIGH_level_read_time = micros();
2 V8 m8 h( R- ^6 H - LOW_level_read_time = micros() - LOW_level_read_time;0 L- \ L9 L) K" `9 e# `
& R# h/ j& @5 u1 i u0 I- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ). k% x Z, [: K& M+ @* w
- {
0 j" O! V7 W! O! B2 b - return 0xff;9 m1 l* D ] v# d: E& C* W
- }
4 x- e2 e/ P6 O) S/ K& ~& H - & v [: N4 y( z7 Y. y; a( \
- pinMode(_DataPin, OUTPUT);0 o) n" }- p3 h ^ q4 V
- digitalWrite(_DataPin, HIGH);
6 R2 L4 @" Y; |+ a+ U, \% S' L1 B - ) _; Z+ R7 w9 U) x1 L3 |! R$ B
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))9 D# ^: ~9 N+ ?: M# c+ M8 k
- {
' E* E$ H# S# u( x0 b# Y - old_data = Sensor_Data[0];
$ |& c( l/ z0 t" o( K+ t- ]+ X2 y# m - return Sensor_Data[0];
4 b6 U# K9 V4 a1 W0 C - }, ]! ~3 b8 M+ y9 Y R5 y3 m! \+ _
- else
/ z0 r6 ?; P- E+ C: x+ d - {# @( {! b/ o1 c5 g: W7 O, \
- return old_data;6 R, i2 Z/ l0 G, G. T+ u Q; h& n
- }) ]7 E! j7 A, R" ~
- }. D& \% t7 @# j3 C m# T
複製代碼 2 N6 k" I M+ X" c* j5 M1 n
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
- |. g* z/ F% W: Y B( Q, S6 W O0 q, J p
|
|