|
|
, ]2 i; [% @5 n' E- #include <Arduino.h>) H) f1 ]3 G% @" Y; M0 w
- #include <MeAuriga.h>
7 {+ l. J; V3 v - #include "MeLineFollowerArray.h"1 t1 U$ T# \( z8 d) y7 g7 W- {2 r. p
- U* Y5 R5 J) }. S) p. l! S
- MeEncoderOnBoard Encoder_1(SLOT1); M* ^ N/ x6 M+ x
- MeEncoderOnBoard Encoder_2(SLOT2);
9 d9 E2 z" Z' \, {6 i' G" [ - MeLightSensor lightsensor_1(12);! z( ^; K" \5 H6 V
- MeLightSensor lightsensor_2(11);
, E! F. c. X; r! Q! C3 S+ d& r P6 `: p - MeBuzzer buzzer;
* ?* d5 B' ]- N6 {* p- l$ d8 z- } - MeLineFollowerArray linefollower(PORT_6);
# d ^- k9 @+ c8 n3 ^9 @
3 D# K$ I( v$ x) Y- #define Error1 1
( k8 D* w2 B4 C0 a' ?% U- D - #define Error2 2
: E4 l3 E/ v& S0 u% V( j - #define Error3 3
1 t1 a# S4 ~- I. |) \ - 9 S; g9 X- `* q
- #define Kp 15- }( _: u# k i1 {" {! Q" s0 J
- #define Ki 0.158 m# u" x: R5 F. W0 x0 F
- #define Kd 0.03
% a+ r X6 q5 o, h- F* m0 I* K5 w
* W' X2 r+ I# f& s# t0 x- uint8_t sData;0 ?2 |0 q+ c6 T0 X
- uint8_t D1;
4 O8 t0 J9 n3 I - uint8_t D2;, s3 n4 S1 Q9 O6 K' Z6 N
- uint8_t D3;. Y3 ^# B: i) P2 N: ?: P% F
- uint8_t D4;
: b) ]' c+ m8 T - uint8_t D5;
Y$ A* A% R8 {' _4 E7 v - uint8_t D6;9 ?$ d% P4 ]3 Z9 A
- " P1 Q! I$ Q' x$ V; o
- float previous_error = 0;
0 a) S2 a2 U, |, ^7 L - float integral = 0;
" m& j% ?0 Y0 W- b - float derivative = 0;
" O1 l5 _4 W4 V - int Speed = 160;3 R& \7 z9 R. t
- float output;
1 u8 G" F+ H4 ?) S f9 R9 P! z9 @ - ; |! W6 L7 r {* N
- byte Left;& Y# B& d$ N* X% l: j( v1 Y" P$ q
- 0 j! c6 Y: A4 @' ^5 U% b
- void setup()
_0 I- S D3 S/ Y% H" \4 D: G - {: U. z. J+ {2 O& G5 s4 V& J
- //Set PWM 8KHz
# g; M! T: e6 V- \ - TCCR1A = _BV(WGM10);! |& M1 j9 l& i- x* C8 G
- TCCR1B = _BV(CS11) | _BV(WGM12);
! J0 ~: y, G& H) y% t - TCCR2A = _BV(WGM21) | _BV(WGM20);
" J8 \' ]6 y M0 b. `/ |3 F5 T! N - TCCR2B = _BV(CS21);
; u4 r: a9 b4 ^$ U: Z - Serial.begin(9600);
3 H& J) h+ v. c F) q - buzzer.setpin(45);4 @" W: w% Q. d5 O/ P! S
- }
: `4 U. q2 `2 x' e - ) K: l& W6 d/ d* q+ _6 {
- void loop()- {7 L0 b" t) z9 ]
- {
8 s- T+ K# X/ S" f: G# V1 Y; g! d - while(!((lightsensor_1.read()) < (10)));+ S9 s% H5 m. t0 B4 d" T7 C0 S
- buzzer.tone(1047, 500);
$ B6 m$ |$ p8 g - delay(500);7 {8 m8 }8 I' ^/ T2 L# O. x2 m+ M
- do
" R6 E3 b$ |* U- x6 Q - {2 @. ?% v6 Y/ f
- int Err = getErr();
6 K4 L) l t3 X) r7 Z; T m6 q3 M. m - if(D1 == 1)5 W9 Q2 A; @( O" a: ~
- {2 v. j, m1 A: Z1 O7 O7 J
- Left = 1;7 U3 d$ u' {+ C/ w+ B5 |' r' y% @
- }
0 E i2 Y! m7 W9 d1 O- w" \( h- b - if(D6 == 1)8 I0 }6 N$ `! F) w- e
- {# y/ Y. i1 o1 C- D9 L5 j2 e
- Left = 0;
$ M: u6 v) A7 C1 v2 L5 Q. ^& E; R/ }+ y" k - }
7 X+ _' ?. i, |& H1 w8 l: b - if(Err == 99)
6 o, a S# f# B - {
5 [$ P% T( t. W - if(Left == 1); y/ `. t1 \, F) [# ^& c) ?
- {
! G0 v" w9 {& e/ w9 l1 n - Speed -= 5;% H1 ~: B, k2 o9 s3 z3 r
- moto(0,Speed);
' ?: }% i: ]7 s+ b3 E: q - do
( X' z! |+ f& e' r2 Y, H" w7 P0 ` - {
E/ C6 E3 k' i# d* ~1 Q - Err = getErr();6 L" ]- v2 M( }8 N1 h1 a, g
- }while((D1+D6) == 0);
7 ?1 v% _ E. F0 z& }9 q5 H1 W/ G& R - }1 H: z8 A! `+ ^- B3 v0 K; N* |
- else6 \) N* w9 |. e- L; O0 L
- {
8 d' ]; o4 H: r - Speed -= 5;
) M2 w; P- T) Z* D - moto(Speed,0);
4 V) `% w! e j4 i' \ - do
9 q3 `- h0 j9 J( x: ^. u - {
: H" l: X1 R6 D5 P4 K5 n, [ - Err = getErr();
. @' w7 ^/ P( [ h8 O& r - }while((D1+D6) == 0);8 Z! ~# `3 z! M2 s6 D
- }
6 o5 Q% m8 I; O! \8 d6 l - }
0 _# k; v- R0 f( _9 z - else. R( {& s* m/ H; H6 L! @
- {- |. m8 m( Z5 H
- if((Speed < 160) & (Err < 2)) Speed+=1; 3 I1 H' F" _7 ~
- if((Speed > 100) & (Err > 2)) Speed-=2;
3 l7 K: g) ?) }+ l6 t, V, { - integral = integral + Err;2 f& a$ o% t( y+ S
- derivative = Err - previous_error;
* e* Y: \: v" C s - output = Kp*Err + Ki*integral + Kd*derivative;
$ W1 c7 W# j4 l - moto(int(Speed-output),int(Speed+output));
7 v/ ~$ s) ?/ N$ p8 r) t, J - previous_error = Err;
5 B# Y* H! U2 L2 v0 Z; x; M! r2 V - }9 @2 q( l+ n: n0 I
- }while(!((lightsensor_2.read()) < (10)));' S' p* ^6 Z( t9 H. @1 ^' t& W
- moto(0,0);* f( k# a' X+ d3 t
- delay(500); g1 X& {) {7 [2 d6 Z! F
- buzzer.tone(262, 500);
9 p+ f) |* G& {- I# V5 P' E$ X - }, S# v$ }0 D) ?- u, m6 U1 I
% Z, [' R5 |. h- int getErr()
8 a) |+ P) ^1 G, t7 \1 C. H - {
+ g$ R# a. \. o - sData = linefollower.getValue();
E( _) n, {: k0 r6 k - D1 = ~(sData>>0)&1;+ X0 D+ p! M( D. N0 C
- D2 = ~(sData>>1)&1;
/ [8 e: A! H5 Y8 [ - D3 = ~(sData>>2)&1;
* H, O) o- S" |9 O6 w7 o - D4 = ~(sData>>3)&1;
9 E2 I4 {, L, e7 d - D5 = ~(sData>>4)&1;
" \2 Q1 ~8 ]/ q! b1 h9 R: { - D6 = ~(sData>>5)&1;
! {1 l! h2 U* ]9 c V - int downD = D1+D2+D3+D4+D5+D6;+ J" c9 x/ ?" L/ a* b0 R
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);, L! |: Q7 M* M4 }! }/ L% y
- if(downD == 0). R+ G" s$ w3 e0 H
- {
# c1 J6 j: m$ z. G' o - return 99;6 |. N7 m1 e Y' j% v
- }
5 G) Y# I" ?! s6 A* S# q5 j0 O - else
( K" C, @. s1 K2 U! A" p2 |6 | - {
5 {) w2 v- ]' r4 [' U6 h0 z - return upD/downD;
e# E) X9 g: s+ s, Y - }
' `1 J3 k% s3 G7 L a) i5 o8 j - }( |/ z7 H6 E8 H. }) T7 a& {# W" z
- , y3 d* Y& E D
- void moto(int Speed_L,int Speed_R)
1 \! G/ _( r/ M( G2 g2 s( v/ I - {% _- N& j' X" x3 J ~6 `5 G" s
- Encoder_2.setMotorPwm(Speed_L);, S' m' b y( Z( U% G: z
- Encoder_1.setMotorPwm(-Speed_R);
) _( q% g- e$ ^9 g; X3 e; ~( ? - }
複製代碼
- e$ w% b5 N0 z M/ k6 ]5 b) l" XMeLineFollowerArray.cpp6 r- d: A# U! ?- N4 w* Y5 s
- #include "MeLineFollowerArray.h"9 n3 Y' C% v; s
( r+ \; Z {, s8 j- I+ d- #ifdef ME_PORT_DEFINED
% v n" X/ S, V% q9 I - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)' n4 t4 l+ j5 s, H& g6 ~
- { j* J- S3 K# s( ?6 w
. H Z+ W& V* d2 }6 r- }
( v5 Y. p& c3 |# u: U - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
( L. Z, z. O1 M& S. m" M - {& s, [- \4 Q" S0 B. U
- _DataPin = mePort[port].s2; @6 h" G- i( _
- pinMode(_DataPin, OUTPUT); * F, k- v, @/ e- J* z. ]! p9 e. D+ p
- digitalWrite(_DataPin, HIGH);
% L+ a9 ~3 j4 @# h' \2 M+ X - }8 U7 z: p+ R5 O/ C& x* z2 }8 q
- #else // ME_PORT_DEFINED4 h' P+ @% ^" p. t7 t
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
9 ^/ _ q. X% p/ V - {/ q7 b! d) ?2 q2 ~0 W" Q5 d3 o
- _DataPin = pin;3 D) h. M+ O+ [" z, I6 S6 Y. Q# |: B
- pinMode(_DataPin, OUTPUT); , s4 O. l; c; a
- digitalWrite(_DataPin, HIGH);
8 n( Y% ]; V! x! D% D7 E n' V - }
! o) X: K6 j8 A, }/ {" z8 k - #endif // ME_PORT_DEFINED6 S' k5 Q2 \" Y/ N* m! f; i' G6 T
- - B1 R) p# X* d K8 z! p
. h1 H4 b- k* j7 s- void MeLineFollowerArray::setpin(uint8_t pin)
* h# W( y( g! T) `1 W3 m - {9 U! i3 k) m6 t9 z
- _DataPin = pin;; o# L# v$ Q8 r& P. N( z
- pinMode(_DataPin, OUTPUT);
3 Y3 C ~" _( W - digitalWrite(_DataPin, HIGH);
1 r. c- Z1 ]7 ~' K! J! ~6 z
% b( e7 K" ^- P) ^+ f- c3 z- #ifdef ME_PORT_DEFINED
3 A# h# n* {) T. i$ t - s2 = pin;3 G, C' t Z3 W' f( T1 P
- #endif) S4 m9 N) L; y
- }
* Q/ F+ V0 k O - 4 c( p# b4 @$ S2 b) M G
- uint8_t MeLineFollowerArray::getValue()# Y8 _1 `' W0 {- y& ?5 N$ C
- {, w4 B6 E: ]# }# X: T1 C. V
- uint32_t LOW_level_read_time;
) _2 i7 O7 A# |; Y4 K6 \! F - uint32_t HIGH_level_read_time;, o" I# H1 Q+ Q' G/ x' ~0 S; e
- uint32_t time_out_flag;
% ]7 l3 y1 z) o: X6 P - uint8_t Sensor_Data[3];
0 {, c$ @& ]% [2 |6 f0 S- T1 ^; A B - static uint8_t old_data = 0xff;
9 e8 S" c! M2 Z0 G1 N+ X$ x4 o - 2 M: K# {- Y! q! O( ?1 m( y
- pinMode(_DataPin, OUTPUT);
" v7 s# c7 t" X$ o1 i - digitalWrite(_DataPin, LOW);
: u/ W' H( s* Y, }: j! O9 O - delayMicroseconds(980);% _: T! F* e* v( r
- digitalWrite(_DataPin, HIGH);3 j8 K7 b7 ]" j$ p4 m3 o0 H% E( o$ F
- ' |5 M% G$ ]: z# M1 y+ ~
- pinMode(_DataPin, INPUT_PULLUP);4 K3 F. M4 ?3 X& u' m
- delayMicroseconds(10);8 x8 V( a/ }9 k
* B5 R8 ]6 y+ U; _- time_out_flag = millis();' n7 F9 m0 R3 `2 m3 ]/ q' u/ A; z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) K. \( h- x( G$ @
w% L: c' w! s& F- T- LOW_level_read_time = micros();
' p6 T0 a# w0 E3 w) Q( q0 S - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) b" [7 W. J8 ~$ E8 T* i' ~1 y
- {! M k: I Q# N! l% @/ q
- return 0xff;
7 t9 ?# ~: k6 \* E" `/ U4 c' E# F - }: F3 a% d. k7 [- ~0 S
7 \; T. t; r2 x. e) I" T4 L- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# h8 ^" t, U7 @! v
; k/ C% l2 q! b, G& J- HIGH_level_read_time = micros();9 t6 `6 @* g: U4 K
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
3 H) `% c9 l( K! k8 q; f! v. D - 4 M3 W& B" n& m7 ?+ H' y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ N, v8 L0 C& W) Z2 `5 ~- J5 p
- {- k8 c2 s6 W8 V3 F% y" x! D
- return 0xff;, ?3 J' G' ?! {2 D- \& U
- }5 j8 P0 m8 n% G6 g. J2 d
9 D/ |" |' R; F, P- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))- g% V" C' ^. Q9 s9 E; A9 R
- {
. g. d' H6 q, C0 X7 s( H+ v - return 0xff;, W8 _) R+ k3 W1 Y8 Y1 \
- }' m# t9 _4 @2 ^# ]$ X9 e( `, ?" g
: q5 z% j. N3 W# Z- d" `/ v8 k- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 R" r2 B' ~0 M3 z
- LOW_level_read_time = micros();; F! j+ R$ }+ u0 R3 C$ H
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
& V( y1 m& T! E+ d& K) j5 X - 7 I5 U6 l* C, b; D2 S l3 y0 d1 ~3 m% ]
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ }, f. I- J3 q6 T3 B$ ?
- {1 q) N# v- q g+ ?
- return 0xff;, X E* F7 c8 f3 i% p
- }& _4 ?$ O1 m4 _: T( z, n3 M; h, Q; B
* N9 z3 h) b# _; v/ C2 }+ E- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
+ h& \0 i2 Q* W+ h5 D# O - {2 D- @# H$ Y$ d# b0 t8 q8 E
- return 0xff;6 w, v( I" s! p2 `
- }) A# a: A4 ]1 Y i
- * \ r! @- ?" h7 }5 t
- for(uint8_t k=0; k<3; k++)
: i, t* k9 E q9 k - {
; S2 A8 Z- l4 Q9 _ - Sensor_Data[k] = 0x00;5 C* a4 Z/ i( F t% y; j7 T
- - f; t" g+ y H) [
- for(uint8_t i=0;i<8;i++)& V. H; C" L8 |5 @9 K" S0 M9 y
- {
0 l `! n& I; c7 U* w5 a' f% S - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ Q+ J |9 {! s! J+ Q7 d" S+ @0 b
- HIGH_level_read_time = micros();$ x$ `! ?- g3 W! L6 F
- LOW_level_read_time = micros() - LOW_level_read_time;
/ w s7 U; x; W% F+ w, {1 M. T! \! I
( H% T! l, V/ b( m8 W- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): q6 [4 G; t9 a/ a" @3 Q$ o
- {
- Q* ]. Z9 G8 k+ E* w7 Y - return 0xff;
) x6 {; m/ S5 [8 s - }! R% `2 Y- j) a3 F4 G0 o+ y( V! ^! ^$ B
- # [9 K& b9 ~7 N- m. k5 p( f
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# y. C/ l- G! R" o: ]- Q - LOW_level_read_time = micros();2 Q$ D- \7 S( s4 R' A( C" l: O
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
+ y" ?- F. I8 q& y! {
/ A5 i2 m3 c4 |) Q/ G+ ~" c1 W- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1# `. [7 S8 T% A6 S
- {
9 \/ F$ Q/ }& j# ?$ d - Sensor_Data[k] |= (0x80 >> i);
4 f8 M5 f: S3 Z& Y$ l8 k - }1 E9 Z) ~- Y: ?; f. J3 s
- else if(HIGH_level_read_time >= 100)! i% Q0 D1 M" y/ E6 N
- {
+ f3 A: O/ B+ G8 [ ` - return 0xff;6 P: N9 ]1 O3 Z
- }4 B8 \+ B- \3 a I7 x$ M! C# |% G
- 5 y& @, A2 O- r* K& _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
- u" W/ f+ @ g: C) i - {* j: q$ `( Q2 a7 k7 g
- return 0xff;1 p* R" o6 n4 ^) Q' H
- }
0 O8 i% W2 I# ^) \& f) v - }
2 i0 t% u, J+ J - }2 i1 k# l# c4 p! I- A
- ; ]5 E6 N9 ~1 h$ a) p$ y
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( c* v" |" s4 ~ - HIGH_level_read_time = micros();
( X. w# y5 t9 @2 E - LOW_level_read_time = micros() - LOW_level_read_time;2 _9 Y9 T, B& K1 s" R' `, C7 ]
3 w( ]! h2 X: L1 M4 _' `- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- i) ?$ j* a2 o1 Q - {9 G5 |2 `. k" X+ Y" w! |
- return 0xff;
& N6 @- |, z( a$ T' M' x' M - }% k, }/ B7 _: K" V* h
- ) k0 p1 [: n( {
- pinMode(_DataPin, OUTPUT);% C0 w9 l7 B# B; f; u0 A9 p3 n
- digitalWrite(_DataPin, HIGH);" Y6 a$ ~0 D+ H1 t& k2 D( j' v
4 Z9 m4 ~* Q0 B5 C! F- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
: S: N$ S% P5 O0 k2 M - {0 `/ x) Y% {: p+ q, @8 O
- old_data = Sensor_Data[0];
# g2 E- u% ^& v) J q - return Sensor_Data[0];( v1 u, z2 J2 o# ]
- }
5 R" ?) ~$ a2 [0 |1 y" C+ H - else3 o4 A l& i5 f" T# k# U8 B
- {9 M! s- f/ A$ E/ N
- return old_data;. Y2 \1 y$ r+ E. `1 Q
- }/ O- v. a6 `( Q6 J K |+ g
- }
4 S5 ^6 ?9 _4 |! w
複製代碼 - O) D' m. ?% _% h# ]
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' _2 n9 ~0 B' j1 w
5 A7 y5 }* T1 G8 c* ]6 r |
|