|
|
% k* G5 {3 X* ?- B$ K
- #include <Arduino.h>
3 ?& w1 C: Y P5 i - #include <MeAuriga.h>4 ~" s n V1 u( K
- #include "MeLineFollowerArray.h"% B$ M; x; w1 j5 ^: @2 a
$ j3 X% ?0 G" I" ~9 {8 O- MeEncoderOnBoard Encoder_1(SLOT1);
+ n7 _ h n& a" D - MeEncoderOnBoard Encoder_2(SLOT2);
8 D/ O0 Y# A4 X4 u$ z - MeLightSensor lightsensor_1(12);* {2 A* `$ R |0 }
- MeLightSensor lightsensor_2(11);. j: N% \) v) G1 K* R$ P
- MeBuzzer buzzer;
$ d2 g5 K# ^) b7 C - MeLineFollowerArray linefollower(PORT_6);
& h7 y) t R3 Y - @: ?3 t( m7 Q1 B
- #define Error1 1
1 }4 w# \1 W* I; N- G - #define Error2 2
0 q+ k; t1 \3 h - #define Error3 33 ]" t1 I, ]2 H5 L5 W- ^( t5 b
- , G* Y! w n9 }, y7 ~& T+ V' g' P
- #define Kp 155 V7 Q' u6 Y$ n. |5 A" Y2 P
- #define Ki 0.15
! B& ^; l% W7 M( _+ B1 ~2 z - #define Kd 0.03
0 w6 s7 `3 l8 P4 H' X9 B3 B - 7 T' t/ h( ?9 Y: N. L- y
- uint8_t sData;
; f$ ?9 p- _/ [6 E Z; t8 }) T - uint8_t D1;
" W; V3 x _3 I5 ^$ J, S - uint8_t D2;
! x/ F0 J6 T" m% `7 N; A" f- p; |0 K - uint8_t D3;/ W- s9 j8 R& R) d5 Y
- uint8_t D4;" P6 V8 x: G" o+ P
- uint8_t D5;
/ ]# r. t" F7 s8 Z$ v - uint8_t D6;5 ]. x" ]2 o7 k. a2 u0 j
. ` O: ~6 Q: K& n9 @9 Z, t' A- float previous_error = 0;( ~6 e4 c- K; w" P# i
- float integral = 0;$ ]9 h/ ?) D% c& z5 u& }0 A) v% z8 b8 _
- float derivative = 0;
: m; C7 r# N f - int Speed = 160;
; }9 N9 s0 {, \3 T9 U - float output;2 p9 z9 c! Y4 m: E1 P9 Y) k
! B D% t! j6 h- byte Left;
* N1 ?! U* Y; {. u, x, e9 s0 }) T
: g$ I1 W1 f- F" z6 t+ G- void setup() 9 l0 j7 C, O- p3 F$ L* e2 b4 y
- {
( x4 X' @1 U2 K - //Set PWM 8KHz7 @- e; q: i, V5 ]
- TCCR1A = _BV(WGM10);
( h$ {, @* b0 g3 f$ c# {/ } - TCCR1B = _BV(CS11) | _BV(WGM12);
3 c! M; z0 {9 |, B' N% c - TCCR2A = _BV(WGM21) | _BV(WGM20);
# Q* u6 U$ L/ d' j q2 M2 y - TCCR2B = _BV(CS21);
$ `, P) L7 y- M, q4 q; f - Serial.begin(9600);
- \# U( s; W; L4 }0 ^ - buzzer.setpin(45);
6 Q6 d8 O' z4 Q' w+ x5 g9 l - }1 _2 j4 M# b- y
/ J- V- E; b5 J1 n- void loop()
) d" v- g0 o* i4 @* ]) u - {. \7 q% H0 S& i) u% d2 N7 g
- while(!((lightsensor_1.read()) < (10)));
|& B3 i1 I* B( l6 p - buzzer.tone(1047, 500);0 o8 \7 g" |% V3 c
- delay(500);
* _2 i: [ O: d& @; ?: ~! o/ T/ m2 c - do1 l0 J; z5 `4 Z1 [
- {
. j5 U% J+ }9 U% t* x$ |3 `) v - int Err = getErr();6 M! r, V) _& Y, d o! [
- if(D1 == 1)
" G* T/ x/ U" O* h; I: s5 L# e+ q - {/ L8 P7 \, S+ M, O- i/ I
- Left = 1;
# e. Y2 q, X$ m, U - }
& b) W4 S* z; [5 r4 V# X7 k3 A& s - if(D6 == 1)
" J) D/ A8 U7 }- G, t& V) ?3 u1 z2 X - {
' ^& m; t" S6 ]$ k& A r - Left = 0;
( y' d9 `8 i0 o1 O5 a5 x1 u! G7 W - }' R7 ?, R: j( a. U
- if(Err == 99)
" Z' W0 y; S5 \( N( h2 G# \; w - {
' v( s. Y6 o) i/ x. P0 U - if(Left == 1)
6 F# Q. m1 K8 |, s; B - {
' z% m; e) Z: @. p - Speed -= 5;
, h% l2 k2 h$ Y# L) _ - moto(0,Speed);: U$ i; n* c# F% \& [
- do" g7 U7 _9 I! G$ k; b
- { F4 f9 {& p# G4 E8 o- C# o
- Err = getErr();3 w8 R; u3 F+ o) M/ P- r
- }while((D1+D6) == 0);
5 ~9 N' |4 @! q# l9 y - }
1 R0 C( R* ^& f- X - else
$ C$ N3 i9 K+ C1 F - {& {2 G2 Z2 i1 h- P9 m
- Speed -= 5;
! d$ B; u' Q m. R# N0 ^ - moto(Speed,0); & P5 O# X4 z% r
- do0 J1 B" P7 s7 r
- {3 E1 M! a% B+ c2 ?
- Err = getErr();+ j8 E1 d( n K9 J; j
- }while((D1+D6) == 0);
5 |' a* X( n* N* V+ C% j1 b - }: F! ? x4 |# {$ f
- }- y6 [' S9 J9 q! Y8 g- [
- else9 i+ e) V$ x) i( j( y
- {% Q$ b8 M5 x4 c
- if((Speed < 160) & (Err < 2)) Speed+=1;
$ u5 W0 A5 x+ A. G# P - if((Speed > 100) & (Err > 2)) Speed-=2;
8 {4 P* q/ a+ | m' K, P1 ~ - integral = integral + Err;
8 [3 q& A: N! ]+ ` - derivative = Err - previous_error;
9 K# Y7 L$ R7 O9 d) | - output = Kp*Err + Ki*integral + Kd*derivative;% c3 a' E9 d% N3 d: x
- moto(int(Speed-output),int(Speed+output));
. n5 E3 o1 C* C! g2 x* q' ?; A - previous_error = Err;5 x, G9 s$ [1 _% U; m+ C
- }
. F/ l$ _) v8 c5 W! S7 u - }while(!((lightsensor_2.read()) < (10)));
$ f c( m8 {6 @7 P - moto(0,0);$ n0 @0 q7 [& e
- delay(500);! u& n6 N! z, d% V" l3 y' ?8 j7 X. r
- buzzer.tone(262, 500);+ O) L. l/ J, q- [7 P
- }5 Z2 s- t+ i& `
. ]( B; }. _0 r- int getErr()4 j* |3 i- |5 |# {( m0 Z/ j
- { . i/ p( Z4 X) k' Q O( p( x& B
- sData = linefollower.getValue();
. [+ x( m N. A& O1 E - D1 = ~(sData>>0)&1;$ h0 p$ i0 K. Z: e- F" Q
- D2 = ~(sData>>1)&1;4 z6 L* M3 \6 s1 d& D0 W- X# X
- D3 = ~(sData>>2)&1;
0 I( J" Y2 S3 W - D4 = ~(sData>>3)&1;
! t; P3 p0 R" p3 f- ?5 H - D5 = ~(sData>>4)&1;, Y, ^( w, a1 r$ |, i& U+ |% I
- D6 = ~(sData>>5)&1;
5 G$ M: P3 z+ f - int downD = D1+D2+D3+D4+D5+D6;- _$ v7 ?1 R% O( Y- q
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3); z0 J* P4 e% Y9 K
- if(downD == 0)5 h! l) {4 f; M6 K
- {& q+ a% @+ s0 _3 Y k
- return 99;
, w6 p! d% c" B1 M* y% I1 N - }
! q5 C0 H" L3 G9 A! p3 S( r6 Q. e - else, l# m: z( k3 z. l) \; k' O
- { }8 k4 {' W* Y. a8 j4 P8 J
- return upD/downD;
! C( V' z6 V& H/ A0 O: ^ - }
6 f0 [- H2 a" n) p - }* T: `/ K) \' k) t' F
4 \ W- z4 \; I; P- void moto(int Speed_L,int Speed_R)# _: @: _0 H5 s- ~
- {
2 n8 |% Y) g4 j; F, U1 C - Encoder_2.setMotorPwm(Speed_L);
, K. _7 p m' t - Encoder_1.setMotorPwm(-Speed_R);! H V. W4 |: m2 t7 e) X) q
- }
複製代碼
9 g- i& t; [; {6 E. N y0 sMeLineFollowerArray.cpp! G% {4 D6 O* L8 P3 I: F/ ?
- #include "MeLineFollowerArray.h"
( B$ h# E2 z7 k9 A$ O* n) L* ^4 D/ j
, `5 |9 Z, U' G! S! `- #ifdef ME_PORT_DEFINED3 n' n( W- y7 c
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)% y4 @% N$ e! n0 q1 d
- {
9 f$ ^, l5 w" a4 D: l) J. [
3 V; o: C/ Q- y- Q- }+ R$ W! V( U' Y& L( L- u. F
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)0 M5 E! X7 i& _
- {
/ [; C% O8 S6 ~0 b0 ^ - _DataPin = mePort[port].s2;
7 F" p' d$ G, }! I. b - pinMode(_DataPin, OUTPUT); " ?6 ]& d* w. T& i+ F3 m
- digitalWrite(_DataPin, HIGH);& z- N# o/ q& u# }) @3 r% M: T
- }
+ g. S5 j2 w* ^. R: R$ H( w. [ - #else // ME_PORT_DEFINED
# w, g8 b( Q9 L7 Z% t9 m: M - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)2 T' t6 x# |' p: F
- { Z* [( {2 n1 N$ V/ n; x
- _DataPin = pin;3 N& P% f7 Z' V: a: G
- pinMode(_DataPin, OUTPUT);
3 k- k5 W& |" S' @& `" a' a" L* J# a - digitalWrite(_DataPin, HIGH);
Z3 h6 Y, U+ t8 |" D4 {6 \5 ? - }
; p+ Y. s2 H! H/ N - #endif // ME_PORT_DEFINED
/ R% i( ]- g( X2 X# Z& {* ^9 ^ - - R: d8 @" v# c+ d* X* @, o
9 ?4 ]( W2 e& @" |/ k1 i- void MeLineFollowerArray::setpin(uint8_t pin)( `. F0 F) y# O' C1 ]; p C9 L
- {
& E/ `2 v" Z I }% j$ x; T3 D2 B - _DataPin = pin;
' s9 F! @2 ~2 Q7 @ - pinMode(_DataPin, OUTPUT); 0 M5 z( o. w+ B: b; S7 G
- digitalWrite(_DataPin, HIGH);; m: A& U# m# A1 D0 L" M2 E
- $ R$ ]( D& x9 x( u+ C
- #ifdef ME_PORT_DEFINED
( W0 J8 r+ h& J0 x: Y - s2 = pin; @1 P: \+ p+ p8 P- }
- #endif+ r1 x6 Y; ?5 @7 T6 o9 E
- }" F0 c% L" U+ b/ k
- ! |7 _1 @- F% k5 m1 N0 V
- uint8_t MeLineFollowerArray::getValue()9 z9 c& j3 |( t
- {
$ u2 a9 P- R6 }+ u1 K3 P, y - uint32_t LOW_level_read_time;, x$ g9 ]% z# ^
- uint32_t HIGH_level_read_time;
. Y1 U0 k+ g9 b9 ]/ } - uint32_t time_out_flag;0 L- |. H+ r% u3 }, t
- uint8_t Sensor_Data[3];
% h- Q3 v& _- ^6 h& p& H - static uint8_t old_data = 0xff;% r- |1 s) h2 S
- 8 K5 G) S* Q% U7 @- K6 r
- pinMode(_DataPin, OUTPUT);
Y5 U- [4 W) G7 E - digitalWrite(_DataPin, LOW);: |$ k8 z! B- J7 x0 }& h
- delayMicroseconds(980);
" n9 ]1 I5 m) R: L! _ ]9 M6 I - digitalWrite(_DataPin, HIGH);
* A" t$ n' y8 R* _2 Y: H0 j. e3 \ - M& _& ]! e) R, `
- pinMode(_DataPin, INPUT_PULLUP);
& d1 K$ ~; M' @7 }( o; m - delayMicroseconds(10);
2 Z0 ^8 A* T( N& V
" R7 E; ^0 H* \) P L- time_out_flag = millis();
3 l2 T" Q- @# P# o, M. [) l& i( q - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# U; \8 [6 O: Z - 8 k4 C9 Y% b& j+ T* x3 Q
- LOW_level_read_time = micros();/ h5 y3 T4 k% m. P" J5 [' \( z+ b
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, g5 j9 b. D$ l \/ r% n& V2 V% A- b - {
9 V o4 V# R- Z7 N3 W - return 0xff;; F1 V& Z, k6 `4 c; f. c
- }
3 v8 i& u' ?4 t, {
. \0 C+ e" {$ Z6 k* [% h. K- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 S8 k1 ?5 {" P4 ?5 b' ^+ R: w$ G
% A/ t( ?& J. O4 u- HIGH_level_read_time = micros();
# a3 I" C/ }$ L+ a+ h& Y - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level' `! j4 j9 `* {: ^) F7 b) a
- 4 d5 Q4 l$ t/ Y5 ?
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 L" g8 b- s# r6 s
- {, r9 R; D0 V+ ]/ m7 m* m
- return 0xff;
) @2 b! S% ^6 |+ M L* T - }: K* Z, u" X+ Z0 ?& L
+ t- S4 w1 d8 c! \9 C6 f- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
. \. q0 N0 l0 F9 _/ ^ - {: X. K& S+ s7 m( s' w+ w0 _
- return 0xff;
2 j- q# S7 j) z& o8 Z. c: K6 | - }
; W8 \3 }9 Q. J" d4 }) D' F - 5 E, [+ F; `7 v5 u' E; e
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ r3 F: y1 A6 }8 P. S, c4 |* ]' k - LOW_level_read_time = micros();$ I+ g* K( H9 O+ q! [' C
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level( [( j8 @' _1 h4 ]* E! ?2 W
- c2 s+ s' J1 P; X4 g( x- Y7 N- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 q4 T4 X2 L) @$ H* Q! h1 ?3 X - {
+ f3 j4 N9 J$ F! \3 t. Q - return 0xff;, _: t- M. F, e: i4 ], O
- }
7 T j* s i, Z0 Q2 S
- z, y3 f2 G" F2 s- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))- H$ r- O8 G; ]9 \; p
- {
+ [9 X9 }( O1 h) |/ ^/ E - return 0xff;
4 y: E8 ]+ H, _+ p* H - }
' Z. Q7 K7 p' I
1 b: |% K8 c1 t0 F/ Y" H* }6 C- for(uint8_t k=0; k<3; k++)6 `5 K. R+ ^1 P2 n" F. `) {3 r
- {- o$ F+ a* L k+ c6 o9 f
- Sensor_Data[k] = 0x00;
) C- F2 U5 L; |; U$ E: c
/ W4 e* p e( S5 y! N7 a- for(uint8_t i=0;i<8;i++)
_4 X% p* g, H. T2 R - { `' Y9 b$ z9 A6 T; Q: B
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% {- d; S+ ~( d5 i6 i7 G i1 S - HIGH_level_read_time = micros();( C4 ?& Z% \ y& s, Y! B" C% u
- LOW_level_read_time = micros() - LOW_level_read_time;/ X. P% u8 k2 L% g. z' e
- + g2 q5 d! m) X! E1 y
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
6 L' M8 H: }$ E - {) n: J3 h" I- `: t; t) P7 v3 ~
- return 0xff;: ^9 F1 b% |' S) ~* |) I% z
- }% o, Z. C, u9 H/ U. X% M. @, i1 s4 o9 D
4 N& a G' N: X: T" P- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 y" G- |) }- p' b7 k* v+ a/ D
- LOW_level_read_time = micros();
& w5 Y. x4 M& V K" R - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
) k6 o8 @9 R, j3 W7 v - ) ~5 P! w+ F- D* u X; K/ ~3 o
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
8 C/ S* q, O7 _# v5 I( p - {
- W4 Z7 x6 o3 d. K7 P# X# { - Sensor_Data[k] |= (0x80 >> i);
y r6 Z' g2 H/ P4 \% l - }0 C% C+ X/ |1 R, e7 x
- else if(HIGH_level_read_time >= 100)' g2 y3 @8 u& o7 q, C. o$ N- ^( T
- {
8 L# L1 J5 u; `! i. ^, f- T k - return 0xff;
& d7 X0 |2 w% `! `+ [, K - }; b! ?7 q2 {( K
- , j+ s; ?: R' m. X8 o% S$ l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
; U9 v$ ^$ u& Z - {/ X; w# Q: m0 q e% h
- return 0xff;
3 d" ~/ `5 S, l. ^6 g9 D - }
L2 w' {7 V8 ^9 ?8 Q/ z6 M - }
" Q" J8 Q; E T* N - }
! q" l U! _0 w
2 A3 {8 ?1 g2 W1 q& g: [. d- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, {7 @" K% {% C. u - HIGH_level_read_time = micros();' d/ g* ]0 d! P. B
- LOW_level_read_time = micros() - LOW_level_read_time;
1 }- C' v: ~! R J7 b; r - * }5 x( o$ ^% A P: z. q. l, d% X
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) g& j$ V- T- d0 q( Q& j - {
& n; Y- H3 V) R2 V; [7 L% b3 h - return 0xff;9 |3 {! \* ^6 N* M
- }- X- @' X4 v$ E9 j1 a2 w, d8 A
- / Z4 e' J+ R$ `% E2 T& U
- pinMode(_DataPin, OUTPUT);
& S5 t. a* \" V- L, [1 R - digitalWrite(_DataPin, HIGH);
& Y& n- T: R( Z* t! u8 t, M- E - 3 V( j. `1 d- l _" C
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
% D; W+ k8 f0 o3 b4 R ] - {, e' P* b9 X( T8 j a: s
- old_data = Sensor_Data[0];
7 ~4 s( S+ Z. U+ n7 F( H+ ?& [1 t - return Sensor_Data[0];
. t5 ]1 d# l6 Q! ` - }
, p; Y& O: G9 ]+ Q% q$ P( f - else
! T2 X; k' H* ^/ U - {
" G# d5 B3 c3 z3 \) j H9 b5 v - return old_data;
! J+ U/ Z! }. m6 V$ e2 [& k3 `$ n - } D8 o, u- G7 D& C8 w
- }* \4 V8 p- @* q% u( i7 j9 G/ T
複製代碼 ; z$ [( c \: \ U8 z
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
# `1 L8 ~% J6 q4 H: }; k3 N, Y4 P
0 O4 e; c) }5 I* h( y& F x |
|