|
|
- ^! _6 g. {* U3 w- c0 d; t- #include <Arduino.h>
9 Y- ~4 s0 w; \0 P - #include <MeAuriga.h>3 D% G K3 t1 D/ A( v4 O# t, E
- #include "MeLineFollowerArray.h"
, A4 d. ]" d8 I. g+ M2 J9 {8 X - / W2 r7 k3 Y) u; e7 U% e
- MeEncoderOnBoard Encoder_1(SLOT1);
' J% n& {: [2 Q5 A' M8 }4 N - MeEncoderOnBoard Encoder_2(SLOT2);
3 ?2 g U& t9 f3 @6 X - MeLightSensor lightsensor_1(12);( R2 V) V: ^$ s% X' G
- MeLightSensor lightsensor_2(11);7 k1 h5 \% M( [. R- B' r* f8 s7 K9 O
- MeBuzzer buzzer;
1 t$ z( Y8 y$ t1 y. V+ B, q - MeLineFollowerArray linefollower(PORT_6);% [) t2 Y( e- i5 C
3 D b+ b" B: }2 z) p- #define Error1 1
. g3 k2 {' x2 p; T+ r2 \' {( m L - #define Error2 2/ q) [' ]1 t" B6 i/ e: @2 C' }% [
- #define Error3 3
% J0 z6 x- }% V2 F) W( l/ s: C - ! L) b2 c) u9 K/ O- g
- #define Kp 15
5 l% G) q3 I+ G r7 C' H! m* Z - #define Ki 0.157 C4 y- A7 v- D0 `/ _5 }
- #define Kd 0.03
! ?2 q, M8 l2 X' E' p - ! Z9 b3 L# l+ ~2 t" ]5 u: }
- uint8_t sData;
$ c& N7 ?- _& a8 \, M1 Q* e - uint8_t D1;5 w4 g6 O. P, W) t- j9 }- O9 h
- uint8_t D2;
) @5 H5 Z* { J) ~$ n - uint8_t D3;2 }; }- H1 J! H: Y) N6 k3 ]% [
- uint8_t D4;2 y0 @5 z5 `4 s) n0 [ _1 d2 y, V
- uint8_t D5;) \6 ?; u' b9 [' Q3 R+ \$ N
- uint8_t D6;
4 Y2 ~) a7 g* k% b2 g5 L
; a- u8 V8 X) [% V+ F6 W- float previous_error = 0;
6 e/ _6 D4 z$ ^( L' O( M - float integral = 0;
- H6 H* R! r/ q% N/ ^ - float derivative = 0;/ ^! V3 F* `+ A' K, ]5 e! W
- int Speed = 160;) e6 H) J4 l$ ]$ v- t5 {
- float output;5 U. M4 H2 j! ]& t- v6 l3 n! j0 n1 t
$ c7 C. D' E; m0 _- byte Left;3 ^' _1 q1 p: v8 C
- 9 I. Y; A2 C3 A0 x: P1 B* U, y
- void setup()
: a5 t) s4 q8 H3 T - {
+ A ]- A4 `. M0 _ - //Set PWM 8KHz
* J7 ^0 u7 p0 I+ l! Z3 N( Y& e2 r - TCCR1A = _BV(WGM10);- k. x/ ^# ~9 @- h+ g8 g' v) u
- TCCR1B = _BV(CS11) | _BV(WGM12);9 n! W& `" Y) y ~1 I7 D; G- I
- TCCR2A = _BV(WGM21) | _BV(WGM20);5 z3 D3 X* M* I5 R v$ B. z
- TCCR2B = _BV(CS21);
4 m j% \# G: S2 R5 I% P - Serial.begin(9600);" C2 ?, | D* l4 E. E
- buzzer.setpin(45);/ |+ q5 p ~7 i% p
- }/ M6 ~4 p( }/ o0 d2 w3 ~
- 1 m7 N8 |% H5 A+ @0 ?9 g
- void loop()" K( G# N, K) W3 s C8 d
- {( m: p( t$ y. J$ H2 o( Q+ `
- while(!((lightsensor_1.read()) < (10)));" |. [/ C6 u3 V& y, q
- buzzer.tone(1047, 500);$ e! E, ^4 S4 `/ r$ p5 ]7 g
- delay(500); U- d- j9 N5 J% }6 C# V" B
- do$ [ N9 ]+ v7 j+ r# z
- {
% M) K5 `3 Z8 D9 S; c; |* o - int Err = getErr();6 `, t' P7 _: k4 ~
- if(D1 == 1)
|/ { K4 \7 R; W" e. y C - {6 U0 F+ o7 l# G8 [
- Left = 1;
3 O- X* Y: m6 u( D - }) `: l! \5 F- y I/ S+ f, u! i. w
- if(D6 == 1)
: H/ `" t8 M, m) I - {7 Q9 H3 D/ u1 j% W
- Left = 0;; }1 M- K3 S: A9 L6 D
- }+ o, d, j, o/ l
- if(Err == 99)
7 J1 q9 \1 Z9 L7 L9 C/ A* u - { A1 {/ c4 A2 H" s$ P& O3 z
- if(Left == 1)/ u% [! c$ _6 x
- {7 @! G( {- n) i% _1 d: \
- Speed -= 5;, E& ], a& H, G* E" P+ }; R, ~
- moto(0,Speed);# E% F; J; A1 [* d( \6 h7 o1 w
- do
& a: m1 ~% c0 h - {7 D2 Y f; u8 b5 p+ D* S0 _ y
- Err = getErr();$ L4 X3 j3 U* q0 y7 o3 P1 {& t
- }while((D1+D6) == 0);) v# j) \0 a. m/ x
- }
! F2 b( B7 C1 k# O. n - else4 N9 e# g# _ X1 l5 j B
- {$ i- U6 M. c. k4 v
- Speed -= 5;
1 @: h8 _$ |' ?& e) ]7 s4 W - moto(Speed,0);
9 k' x: g H, [: g- F0 D$ S - do+ p% E6 A4 i6 t' r2 i
- { W8 W; B _( t* F& g
- Err = getErr();+ M: h9 H; b" N
- }while((D1+D6) == 0);
( A/ }8 N. j$ j; f3 H8 _- J - }
2 S K( a/ g3 J0 b1 q: ^( ` - }1 [- W' o8 k) C/ ?9 h" E% S3 ]- E
- else
0 B o" H( z A# H/ C - {" z+ {; O" o5 z& S) I- D& n. p! T
- if((Speed < 160) & (Err < 2)) Speed+=1; - M- K; T0 r. T2 O. H; l; B
- if((Speed > 100) & (Err > 2)) Speed-=2; & {- c8 M* o; @0 F0 m. V. k/ B& H
- integral = integral + Err;& g0 b. i% t6 w, n+ d4 A) I" j
- derivative = Err - previous_error;/ c( A' w3 k: S% ^
- output = Kp*Err + Ki*integral + Kd*derivative;) Z, A4 S: z X) Y6 C
- moto(int(Speed-output),int(Speed+output));
0 B& `7 k; i3 ^0 |( N- F( W2 [ - previous_error = Err;
/ W7 b. c" o% K# g7 Z& o) V& C - }
; Z- \6 |6 C3 V - }while(!((lightsensor_2.read()) < (10)));
6 y% D* M" Y; k6 Y* k! S; B* c - moto(0,0);) ?7 Q4 O$ x" U. n- ?
- delay(500);
' g& j( T+ c, c1 u8 ]) r+ n - buzzer.tone(262, 500);$ u! M; T$ q7 y' [) t- \
- }
/ t; }0 Q4 A! \3 [. }
7 [; Y" u$ m! ^- int getErr()5 M# n" V7 T$ |- U, X0 |
- {
( }/ D& n& U! y& i - sData = linefollower.getValue(); B) J; a& a' x! z; d4 z2 A4 {6 A! [
- D1 = ~(sData>>0)&1;
4 l/ E0 X) E+ K4 L - D2 = ~(sData>>1)&1;5 w+ w7 [, s& {& }
- D3 = ~(sData>>2)&1;3 E+ t5 X. u' C/ ^; C8 W, S
- D4 = ~(sData>>3)&1;
7 o$ R7 \ D2 ^) f5 Y3 |( z) e - D5 = ~(sData>>4)&1;( y, u. [$ l' j; V
- D6 = ~(sData>>5)&1;
1 A. t! r" K0 U J6 q9 R - int downD = D1+D2+D3+D4+D5+D6;9 |" S( j# R$ Y, _1 I; z* f4 |% b$ h
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
5 O$ o% {& D; a4 |+ T - if(downD == 0); _- i" u2 d( l7 A6 S! j( Z8 V
- {2 A0 ~0 w( D% T, \
- return 99;
# a& ?& @6 V) O - }' u6 U. Y! O/ d
- else
7 u5 a2 A( ]8 Y+ j' l8 D7 G5 F - {* Q3 X3 u3 [2 C2 m! W+ D: a
- return upD/downD;
0 \6 P- c6 d- w* O+ ]# d% a - }
9 P, l7 I% c" ?. N, v - }8 v" X1 W( P6 k
! ^" H7 ]2 w' D0 ]# w. V$ S- void moto(int Speed_L,int Speed_R)( k% A) i+ J l9 g3 }5 \: I
- {: d* K" h6 j/ V. T. r6 D
- Encoder_2.setMotorPwm(Speed_L);3 y3 I" D. S% G" k) u* ?3 M
- Encoder_1.setMotorPwm(-Speed_R);
7 T W/ c! N$ L n - }
複製代碼 & e" _0 }6 s& n1 ]
MeLineFollowerArray.cpp) ~8 \) {% q+ o/ ]8 L
- #include "MeLineFollowerArray.h"0 Q$ D! U- t+ @: r% S
- 9 B: \2 `9 S+ f0 I) s& o
- #ifdef ME_PORT_DEFINED9 v2 `8 \- _0 |4 y, o3 _
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)5 k( v7 M, M( f% g4 w Q
- {9 f; l" g( {3 i
4 `: d, H7 m7 I9 \+ i- }
/ ?! G }6 p; o( x) O6 k3 Y: e8 q - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)2 \- B. h l5 ^+ Z; d! B$ C" E
- {
& R7 ~0 J/ f& }( G - _DataPin = mePort[port].s2;. c2 r; B0 K5 Q# S( L8 ~, B/ F
- pinMode(_DataPin, OUTPUT);
5 U6 G# r0 x& @+ V' E: U - digitalWrite(_DataPin, HIGH);
6 G. {* Q2 Y7 a* G8 g: I - }
" }3 `6 @' O9 ^( W( e l5 ^ - #else // ME_PORT_DEFINED
1 F8 `! d, p2 b+ B5 n7 ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
) f+ D0 K+ z. x$ U - {9 h, z$ @3 g/ Z
- _DataPin = pin;
2 v" L5 s4 i" S& R) E% @; J - pinMode(_DataPin, OUTPUT); * u1 [3 ?4 X+ p+ n0 X
- digitalWrite(_DataPin, HIGH);
/ V) O* R( k) l" a5 r - }
" s& X0 E# Z" {. h: P - #endif // ME_PORT_DEFINED
% J3 J" V, Q% N D' `! c) p
Q0 D4 c- }; H/ D" b
( h! O$ E7 r% E) J- void MeLineFollowerArray::setpin(uint8_t pin)! z2 Y" g- ]( ]! v9 f% E( t6 b
- {, A2 S# \5 `8 S( v: }$ a7 b3 W
- _DataPin = pin;% }& e2 R" S. ?8 k, p3 i6 g: _
- pinMode(_DataPin, OUTPUT); ' t$ p" l# E2 q) G6 f& [
- digitalWrite(_DataPin, HIGH);- D: F' [; I2 F
- + B1 J- ]) ?$ R7 o4 [+ V. u# M
- #ifdef ME_PORT_DEFINED& x: k2 U- Q- {7 E. H& a& U
- s2 = pin;' j: }# e' J& ^% J, S9 C, h
- #endif
7 G7 p. j: ` d" `4 y - }! C7 U. A+ Q' g" l$ N
- & v$ U$ P! g+ Q
- uint8_t MeLineFollowerArray::getValue(): P" _: d) ]& b) |. g( }1 p
- {8 ~; y3 g; ?" I5 @
- uint32_t LOW_level_read_time;+ }2 O& y. `: }, ]% ^
- uint32_t HIGH_level_read_time;
9 V$ k T4 k# o. h' y6 I+ r1 {% z" Z - uint32_t time_out_flag;* L @( [4 B) G
- uint8_t Sensor_Data[3];. j! I% ~+ W R3 J, e% a% o( F6 E
- static uint8_t old_data = 0xff;6 _* @5 ^% }. r
5 m$ ~! y! n* }" ^$ z( i- pinMode(_DataPin, OUTPUT);
) l. _7 P7 }3 i/ Q3 E4 l - digitalWrite(_DataPin, LOW);) y" M0 Z2 q* X r5 d
- delayMicroseconds(980);
- |4 L" `2 C1 }9 ?! E8 f - digitalWrite(_DataPin, HIGH);
6 Q' k( i6 X1 c1 c - . }0 D- ?% ~5 e* e
- pinMode(_DataPin, INPUT_PULLUP);
) v) ~3 `$ L5 U& k - delayMicroseconds(10);
$ w9 [5 Q ~! B - * J2 c! t: C, r8 H* ~: ?7 J8 `$ p
- time_out_flag = millis();
' I# q# B7 r+ a+ \# k - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, S7 i8 {7 b5 E6 j
0 x% y& N1 x; O- LOW_level_read_time = micros();4 E- C* \- I4 E0 f* ?1 ^$ [* s
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* ?' ?9 y0 i1 S" E0 l
- {/ f9 w0 P- Z+ P/ m3 v# u) A
- return 0xff;8 J3 b2 ^" Q0 \
- }: S5 u9 Y% ?4 E$ O! {8 D
9 G8 ?1 }) y8 T2 q+ L9 W) G/ Z8 h* {0 o- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* u/ A5 w# C2 W5 |. v( S - , I! ]8 z$ D: P6 T; J L$ E
- HIGH_level_read_time = micros();. k8 }. C% i# N+ n; G; `% {* O
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
+ _& b( W& w: `. x - + t6 M( ^( o& m% {9 B( S6 `% o* y" q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 y: H/ N5 m! K, n
- {
3 T4 F( K, ]& Q9 v& n5 G1 S - return 0xff;3 p! f# f) D% R) {% ~" c$ S
- }
( F; G6 r7 \8 K. z - # y/ P4 n1 H+ Z0 K, I7 |5 }
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))0 T/ v9 e% ^- k1 U0 y+ |
- {9 |5 e3 o' u$ A, @2 m$ m# Y+ r8 y; L/ O
- return 0xff;
; |( o% i) S! }# D4 `, ]( f: n - }2 Q1 U. `, v- R: v( a- h- F
- & U. [( z4 D6 H5 k/ T. Y! X4 x
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );6 H# A: j: ^! m' `4 T4 l
- LOW_level_read_time = micros();
4 i, e6 P% x7 n - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
1 Y$ L) m9 B; d- W3 i' F9 y' D
8 U+ }6 r# J: F# c8 U7 G- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ {, B9 C. g/ @; @; Q1 F6 ~1 a - {9 n; c! E# U; w
- return 0xff;7 Q$ N v' e" y
- }- R9 [& C3 ^" K
- 2 W4 c6 e0 Y: c7 f* ^; ~
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))- b+ _' i! N! a' S* ~% o
- {: t5 \' \1 U" T1 q9 n
- return 0xff;, V2 c3 `; L7 S7 v6 J) v
- }* [+ o6 G5 {- U9 a0 B
- % V& E, c+ g* x4 K6 C8 A8 t
- for(uint8_t k=0; k<3; k++)% N! e2 R* ?* u' ]) @/ ]3 }
- {
, ~& s; }2 v! v! }* A; s - Sensor_Data[k] = 0x00;* } |+ o9 R; h8 m9 x
- 6 R0 Y) s- M( c' j& c p9 ~2 t
- for(uint8_t i=0;i<8;i++)
4 _9 L/ N% d' a" w6 ^ - { T9 W9 z9 q* b1 a p
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 w! L, B1 |1 V3 W4 v( T5 ^) h
- HIGH_level_read_time = micros();
( o' A7 `2 y! A8 R5 q0 k8 ^ - LOW_level_read_time = micros() - LOW_level_read_time;
" e% h0 l) f2 J9 { - ) ?5 V0 U7 b$ g" j* I. Z' j- x! N( ]
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
! r% g; \3 \ p- N+ U - {
" i& \, F! Q8 ?! T2 y( p% ~ - return 0xff;
T- h7 B2 O8 K7 f5 I - }
! j: {- o! f. J7 E - - c! ], D) H5 u/ l* k0 b
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, V8 C5 \0 C1 l
- LOW_level_read_time = micros();6 p% m$ |" C% |% |; }' K, ?
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, ~# A' g) v; U* J; L$ N' n( L/ S
7 q+ ?/ \$ U. s8 c( K- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
) [4 @5 c7 E; o9 m x# Z( V3 a- v - {; U2 H9 P# B |5 w( J
- Sensor_Data[k] |= (0x80 >> i);/ g! [% ^$ J, N
- }! d' {" W- S" @1 u, }
- else if(HIGH_level_read_time >= 100)
; b/ ] n! `* K1 n - {1 }" z1 Q: x* [! @, B9 G- _7 x& [7 z" ~
- return 0xff;# O k8 ^5 A: H! y' G! A! G
- }$ t5 Z" ^! t8 |0 [, @" f7 s6 ~
2 Y$ M0 K+ h( l0 o% J9 ~" A- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)0 Q+ C! m/ h! g% U8 Y, m
- {
( g% w, o: d, A3 _0 ~ - return 0xff;
0 G3 N7 L7 ~0 Q! ~- d - }
: G) A0 E, A e7 ` ` - }
9 z. Y6 s& e7 W% {' r: B - }
n' ]/ J$ g9 ?( q
* D% l# n( N" y5 b+ y- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, b0 j5 H. k0 n# o
- HIGH_level_read_time = micros();
. \9 r( m5 [* T) c% Z; E. @ - LOW_level_read_time = micros() - LOW_level_read_time;
. Q6 n) Z' K4 Y
1 k! w k; n& U/ u7 d9 v( i- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# z: c$ C& \4 k" V+ M
- {/ I/ z6 x A7 n# K" F9 R
- return 0xff;4 G2 C, x/ q% {) `
- }
) F7 y# @) S9 ~# m) h5 B5 B4 x
8 y( M% l$ ^3 a: g$ T% E- pinMode(_DataPin, OUTPUT);
& p t$ ^! ~6 Q - digitalWrite(_DataPin, HIGH);5 h" ]" l4 g; \3 n7 u
- 5 N) w5 h3 O0 X* g" A( H7 |
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))5 e1 u. L0 q. D
- {; I; w0 W$ }+ F9 R
- old_data = Sensor_Data[0];
- d; l/ o2 U, M" t) u/ p6 d6 p - return Sensor_Data[0];
8 c& S3 L7 C# k - }
& H5 A+ t; H$ g - else+ }; c! m# j; d; ?
- {$ Q) \2 z& ~9 \! `2 m2 z# ]# m* l
- return old_data;
6 \; v7 ]2 y: W - }
R% F. x% u( G. c* w( i9 F - }
" H6 J# ^# z6 m& _9 [4 `+ \- N% o
複製代碼 4 r6 |& T0 u* C
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
- S' `2 F5 R; q( D% Y
. d o' r. V5 Y. J' t |
|