|
|
3 [3 k; a. M( k
- #include <Arduino.h>9 w4 Y9 ?) X# Z2 ~0 }7 N
- #include <MeAuriga.h>
% |& G) j9 e0 {4 d, ?, K8 | - #include "MeLineFollowerArray.h"
0 o3 N C2 {9 D+ `: y
# k8 y/ }! ~6 n2 B( c( y, h- MeEncoderOnBoard Encoder_1(SLOT1);
9 @0 W0 ~" E; _# @5 b - MeEncoderOnBoard Encoder_2(SLOT2);
1 W( A$ `! S0 f) j$ i# z0 ^ - MeLightSensor lightsensor_1(12);
. |1 l; o: }1 g7 w7 ?0 f2 f5 ] - MeLightSensor lightsensor_2(11);
- W9 u9 I8 ~9 L$ A1 z0 B( u6 F0 n. M - MeBuzzer buzzer;
- W) {/ f6 ]% B9 H - MeLineFollowerArray linefollower(PORT_6);' `4 o* v/ A$ g4 |) e( B
- 7 o: {, c5 x& z/ y, Z
- #define Error1 1
! a& K, i$ _9 {4 D- m' @0 p( F5 F9 B1 O/ ? - #define Error2 2
4 L+ z& N f" ^% l8 y, o) V2 l2 e! j, z - #define Error3 3
3 E( k% U. L- ~
: j4 b" D# j2 q, k- #define Kp 15
( l0 m3 Z/ }. W3 P- f" { - #define Ki 0.15
: v. l' w! ^- q& _+ ~9 s0 H - #define Kd 0.03
) v3 r& g% D0 |0 |; j - 0 r" T: j& D: j3 g
- uint8_t sData;
; q- t0 J: f c - uint8_t D1;
( Z) c; A5 C+ z/ e# k* ? - uint8_t D2;
3 i5 @, q1 l' U - uint8_t D3;
1 p& u; z( Y; o# `4 c - uint8_t D4;
1 r6 `: F) m) w/ m! |% ~+ l - uint8_t D5;
6 C% w; Z5 `# B/ J% Z: q/ X6 ^ - uint8_t D6;
7 Z4 ~: I# r: F2 ^) N# @ - 0 r4 D! G3 z0 ~+ X
- float previous_error = 0;
9 m: w3 o/ N1 R5 F$ \& O - float integral = 0;
( T5 V; }% u+ y" A1 f' y+ y - float derivative = 0;
+ _; H8 a. P- Q - int Speed = 160;' [3 Z6 i) M4 t- Q3 n
- float output;4 M/ H. ?5 ?1 a6 H" g0 u2 s8 A& A
- % Z( \0 g- G5 V7 T8 O9 v
- byte Left;/ I% x: A! m) Y+ z) a) \
# h3 w& I! H( P) B6 R C) f; H- |- void setup() - n0 B* E7 |% u
- {
* s$ E2 K5 S9 g9 ] - //Set PWM 8KHz
* t3 N2 M5 `$ w$ L& s - TCCR1A = _BV(WGM10);7 O* A3 u) D @" d
- TCCR1B = _BV(CS11) | _BV(WGM12);
1 W' Z' V8 W; }: o1 P1 e/ J - TCCR2A = _BV(WGM21) | _BV(WGM20); T4 d/ J1 C* _7 y" L1 {8 e! [
- TCCR2B = _BV(CS21);7 M/ H! d4 r: Y/ Z9 K6 [. F% t; g
- Serial.begin(9600);! j/ A4 k$ o8 n0 X, D! H$ V( e8 ~# n
- buzzer.setpin(45);
: o4 [/ D' \: O - }5 W7 \! E. M* w
- + s8 ?# Z' ^- d" K
- void loop()
Z. W2 i) d" O$ i9 r - {
/ k& ^) ]' n% ~0 v$ g- N# O - while(!((lightsensor_1.read()) < (10)));0 j. l# a( p) _
- buzzer.tone(1047, 500);
. k& I+ ~. N L$ I/ @, _ - delay(500);3 G8 J" w- q0 v; g9 ~$ Z
- do
( U0 H7 d2 G; C: S" O, m - {
5 ^; v4 g1 m% K& h9 _; j0 F1 | - int Err = getErr();
}" |. {0 B/ [8 r- r! J4 L - if(D1 == 1)6 D* |6 A$ m1 T- n' m# }) o
- {
' N$ O: S2 y' p, i$ A( Y - Left = 1;
1 m# ~+ ?' H+ h4 r' C - }- @7 ^1 G% t! P9 {, c2 `) ^
- if(D6 == 1)! q5 G3 }) k# U9 p- P
- {
9 X. y; w: p. o5 Q9 N' M R% U - Left = 0;7 x( _0 W; a! [& G/ J/ l/ S
- }8 G! X8 v" w* H9 P; L
- if(Err == 99)
& O+ i: ^* c# l# z! @ \ - {
5 A3 d( W) L6 ? - if(Left == 1)- `* V! S9 P0 g$ s
- {
% E5 T5 s% P6 [. V - Speed -= 5;2 i9 R; }1 c( }) Y" z% m
- moto(0,Speed);0 u( T( P5 h# Y3 H7 N' |8 p2 s
- do
9 b: W4 U7 W& F- w - {# z) y9 q- c' d4 `6 i
- Err = getErr();
2 s* ~( ~* z# P$ X; N - }while((D1+D6) == 0);
% |6 v% b5 z5 `, ~! K5 ^! F - }/ l( U8 X5 }5 Z! i3 F6 B1 Q# J4 J
- else
6 T/ W) w$ \$ {: v4 M - {
6 D% t- M7 V R% z9 k1 U& y6 X - Speed -= 5;4 V8 T1 l, V, L( g
- moto(Speed,0); 4 V F7 A8 m. S# t8 b9 K/ s
- do$ C3 }' r1 v I* b
- {+ ^1 v: X3 d" [0 L+ ?1 a; G! ^
- Err = getErr();. B* O6 z- a& Y* F5 a: L/ O: l
- }while((D1+D6) == 0);0 e! Y- I9 i- a8 D! O8 _
- }
% Q+ g, f2 f5 Z2 U - }
' _$ O7 a0 O9 u+ {3 e6 Z - else9 w' S" p+ W1 b
- {" G9 v2 \) `) l6 p" }
- if((Speed < 160) & (Err < 2)) Speed+=1; & h/ e% S% @- ?5 \
- if((Speed > 100) & (Err > 2)) Speed-=2;
6 x1 s" h$ @) |; ? - integral = integral + Err;
; z k _. \6 F( } Z - derivative = Err - previous_error;
* }1 u6 k) U g* A* Y) v, j V) H - output = Kp*Err + Ki*integral + Kd*derivative;
; L! c3 n) N9 y, E - moto(int(Speed-output),int(Speed+output));# C$ l6 U, e' ^! G& M4 \4 T- D, w
- previous_error = Err;
2 n3 o r6 r H$ C- m" {# | - }2 Y, }5 e1 Y& G2 y/ l' Q, h
- }while(!((lightsensor_2.read()) < (10)));
( N/ t1 S; O" U7 X& s - moto(0,0);
3 e9 M O/ e+ d2 v - delay(500);/ S5 b; y( t& Z# G0 s( o5 k
- buzzer.tone(262, 500);) ^7 y# D8 v/ t. d6 V; j4 l
- }
/ S/ _9 a5 m4 Q2 Y F5 }
/ B3 M: b, m% I% T- int getErr()2 ]6 k- C% o# o, G
- { : `% c) k$ _, E9 h8 O7 s
- sData = linefollower.getValue();
" k# M# k- f- S8 [& ~ - D1 = ~(sData>>0)&1; t0 M9 w0 m, g! n, L1 Z
- D2 = ~(sData>>1)&1;& [: F$ [' R% T; L9 i) m& \- p
- D3 = ~(sData>>2)&1;
, I) G4 S, z% B$ C& j0 r - D4 = ~(sData>>3)&1;
% u/ C6 U9 Y# V! { - D5 = ~(sData>>4)&1;
$ p& `, n1 f; {8 K& w1 L+ ] - D6 = ~(sData>>5)&1;* j0 n8 Z D7 Q1 m& m5 [3 Q
- int downD = D1+D2+D3+D4+D5+D6;
3 h2 `/ I6 w i* h - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& [1 h) z, }& D2 C% X
- if(downD == 0)) f7 O& o: Z/ [& M! X
- {
9 H% Y4 R1 a; F$ J: E" c" | - return 99;% h& o; x/ Q x3 V6 D8 u) s
- }
- M, a6 j) M8 p3 X& C - else) C8 G0 T, [# x6 b! B' B
- {6 X7 ^& O) X$ B" ~' j
- return upD/downD;
6 @; u& l" O% f. w- p% B - }, ]2 @" w& T# z6 F
- }) Q" E# [6 ^1 p; p7 [4 X
- ! A2 P: v# ]5 R- x& F% H
- void moto(int Speed_L,int Speed_R)
9 U: i( N; w9 s9 R" b" f - {+ J A4 t N* |7 o
- Encoder_2.setMotorPwm(Speed_L);5 c0 ], \( \, d% I$ k3 a& m% Y
- Encoder_1.setMotorPwm(-Speed_R);
7 f5 A; d7 B: z: D - }
複製代碼
5 |5 J r" }' bMeLineFollowerArray.cpp i1 {, ~* |2 z0 M% B
- #include "MeLineFollowerArray.h"- |% l) ^8 z5 k9 ~0 V& g+ p* u. ~' t
) |; ]& ^3 z( s1 e) [! l% _# w- #ifdef ME_PORT_DEFINED6 H9 n' j. \6 }
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)6 h' R$ v' E* {2 z7 ?
- {! k \) v9 r8 B
+ U. v# v% ?/ h; i/ e5 C& ~- }
3 o/ s% J* T# h+ d% w - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)) C5 z5 N$ f: K! l# i( P
- {" B! z! ]" c* l% I; I. v Q
- _DataPin = mePort[port].s2;
' ~7 `$ g! h( W& o3 z% v" u' U9 } - pinMode(_DataPin, OUTPUT); % }! @3 f+ B( D1 _
- digitalWrite(_DataPin, HIGH);' X. b1 J- m+ u0 ~4 Z" R- r
- }
) D* d: I4 s5 p4 ^' Z* s+ W - #else // ME_PORT_DEFINED
; G6 C# | E- r+ @. z3 j - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)& g$ |5 q2 @8 ?# X8 y# o& x' b. s1 y
- {% R1 Y' ]- b% Y1 d( N/ G
- _DataPin = pin;% v; k; \: D; V9 J. U
- pinMode(_DataPin, OUTPUT); 6 V9 n7 ~6 n0 R9 u4 p% |5 I
- digitalWrite(_DataPin, HIGH);9 P& _2 Z+ f) Z) t) D- p
- }
) X' O) }4 Z0 x3 m% P% a: c- Z - #endif // ME_PORT_DEFINED
1 c- h) K( a+ {9 X6 n- D7 Y - $ h* i' X8 `, P3 {
( s9 d% }) B: z+ s- void MeLineFollowerArray::setpin(uint8_t pin)/ Z6 J" w( d/ G/ c
- {, y. j; C( V# i* ^8 M
- _DataPin = pin;
1 ]5 H, u: _- Z- J# M, h: f - pinMode(_DataPin, OUTPUT);
4 E& f; o5 [7 W% D - digitalWrite(_DataPin, HIGH);- N. e3 k: H* P# r% d
- . ]3 c; I- Z2 l3 U# _
- #ifdef ME_PORT_DEFINED+ K& B: c& W4 C- h" R
- s2 = pin;( L. C) I8 A+ H1 D, T( }" P( v
- #endif
0 v6 W0 E4 M5 J- z7 x3 S - }
9 o) y0 s5 o- D - 0 d7 u' B6 W+ k) T4 |1 p
- uint8_t MeLineFollowerArray::getValue()
% M3 t6 E# l, m& Z/ {* e! Z3 r# r - {
" t5 Z( S- ]9 G0 \2 t! E/ K' [9 X - uint32_t LOW_level_read_time;. j+ m* t0 v$ d0 r
- uint32_t HIGH_level_read_time;, K& ]" B R( d+ i
- uint32_t time_out_flag;7 O% w/ t6 ]( u0 r+ i/ y2 @
- uint8_t Sensor_Data[3];
. e0 b5 P2 g$ L; z9 _' f' K4 }6 v* [ - static uint8_t old_data = 0xff;
2 X: _9 P* e+ M& G
0 n) O2 Z: [" H8 W, g# s& O- pinMode(_DataPin, OUTPUT);
. E2 h! s9 ~0 m/ r - digitalWrite(_DataPin, LOW);
' Y0 _* H* P A - delayMicroseconds(980);
4 U2 E9 M( h9 |) y9 N9 H - digitalWrite(_DataPin, HIGH);& w: m% {- C( w
9 j* |# t) t3 c7 `' y3 G: ^- pinMode(_DataPin, INPUT_PULLUP);
/ ]0 |, P0 [8 i# h - delayMicroseconds(10);
: n5 Y" L; U$ R; s; N' F( F6 m3 M - ( A" j; d9 C/ R( G' _
- time_out_flag = millis();! ?8 z' m& L8 A" G( s! H0 v
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
! F( w$ v& L+ `5 L% H
1 l; m" U) G$ h) J9 N- LOW_level_read_time = micros();( b+ s# u6 a, Y8 E; K- B# t
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out- {; ]. A) P1 K& p! |3 t3 L
- {' a& p: s2 k }$ {
- return 0xff;
* a& B R3 n8 A+ a5 Q - }
' I, F1 C/ P, d& @- B6 P
( D) w# _ ~2 S0 f" l- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# K2 ?( G5 ^* z2 X9 n
- & c1 K% E8 M! _% H+ V {
- HIGH_level_read_time = micros();; ?* a* q' T4 T" M6 I( E( {! J
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level" u3 d$ ]% T0 c6 [
- 0 b7 C: W+ v( J3 W- v) m' J% D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
! C! Z, Q& ~- l0 p7 P. u; Q' H7 ?' ^6 P - {7 b0 @. i' \" J4 A* H- E
- return 0xff;2 [! n1 p+ Y: y% P6 j0 q
- }
8 L+ H7 P! z$ ]/ I) E
C- Y$ k8 j, f9 t9 W# G- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)), O. q3 ^+ [" H
- {: v0 O* k, D% q. S3 e3 v" X* ]
- return 0xff;
8 f$ z! Z5 R5 p) g - }/ y' r% E! g; ~% d
- V- e7 w$ U$ g+ M4 c7 g- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 S9 z$ {. v+ Y3 A* H: x - LOW_level_read_time = micros();
0 v0 k# [- U& J6 i- }/ F4 E - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level; K, f+ _% y5 s
: X4 O/ N# D) F: R. t/ F- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out- I: w; c# D) x- \5 V, M' ]' `) | \
- {, R4 K+ O9 {7 k% b" c
- return 0xff;
$ L2 N' d. ]2 S8 T3 `* P) l" L. `+ f - }
1 |! x/ {: c7 _* g
3 G. q0 R4 x9 ?. W( f7 ^, H9 v- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
2 S$ b, Q5 C) ^: G9 m, L7 x - {
! w3 L0 w, S2 X" ^* f7 b; x! E - return 0xff; N+ C2 S$ H; e$ ]% P& v6 ?
- }
K' p: O- r' [8 Z, g: t - 3 M. H/ ]! V, R* ~9 W, g
- for(uint8_t k=0; k<3; k++)
! Y: i1 \; {/ H# i! {& r! U - {
. S Z8 r6 e g& j2 E, s* y - Sensor_Data[k] = 0x00;+ l L$ Y$ L& r) [7 }7 y
- 9 n5 J5 k! h! O& @) c
- for(uint8_t i=0;i<8;i++) z- U; ^- F$ i$ H: |* w2 a9 \
- {; P! j/ I0 \- o
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% ~- `2 O2 d6 j' g - HIGH_level_read_time = micros(); C* M9 o4 s( ` j. \
- LOW_level_read_time = micros() - LOW_level_read_time;2 _" J2 J+ F6 x1 o5 e6 n
" v8 X; n. [9 d' S- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* c/ V( Z" [& l& d/ @ - {
. K3 d% D" q9 N( i0 { - return 0xff;
' @# ^$ o4 b/ ?3 c* n - }) j- m. q# b- D- T7 s, u
* k3 |4 q- [7 _4 M2 s; T0 ~- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: w; M8 H$ S% u8 ~- W' t
- LOW_level_read_time = micros();1 l h. S3 y- l/ {0 q* G
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level2 X0 C! U" c0 q# b P
3 d3 H/ O$ e3 ~" U7 o: [- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 u/ ?3 l* u/ I, D
- {, E# `& q9 W9 x1 _
- Sensor_Data[k] |= (0x80 >> i);
% y f9 k) q4 J" ?" C - }' `, ~% Y( w4 E* \& T4 f. C( x
- else if(HIGH_level_read_time >= 100)) P8 f5 @% g+ `) n
- {
2 X! t6 o' j2 A - return 0xff;# {5 Y" U S/ L
- }
$ @; H) O7 D6 u- y7 m
3 P' Y5 j; @; F) }: {6 w) @& G) B- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)% N+ Q6 t6 J8 x4 \8 {3 j1 b
- {
8 B7 M' H+ L" v+ {" ~, \+ b - return 0xff;9 P" G' B( A" {" w
- }( S" ~7 {7 E3 Q H) S
- }
! E) Y# }& K) \# z4 F& }! h( e7 ^ - }
5 V& R& |" b" _4 m
+ N' s9 R$ t, w$ ?; Q" I( I- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, E$ m! O: F6 C& x: f; R: g7 A9 E+ ~
- HIGH_level_read_time = micros();
' i8 [' Q+ A) [1 j6 P* ~% o - LOW_level_read_time = micros() - LOW_level_read_time;
' ]6 m3 {* i4 G) c& K
6 m. c2 g6 U% ^3 M- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )0 E- c9 J' c ?; X- C. n* z
- {
( b6 a/ ?+ n; }# g+ S+ Y - return 0xff;" q7 k9 k, q0 s- S$ N4 A
- }' A$ d2 h9 j' I- h& N1 h, P9 z
: V& E" k. i0 V4 |8 ^- pinMode(_DataPin, OUTPUT);
0 l) I1 \1 k6 c$ H* U5 U5 A: b% x# C - digitalWrite(_DataPin, HIGH);
% P$ H1 c- Z" |- ?+ j
; u0 O. O* O. z" }5 j. V4 J- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))0 }) k/ c$ C/ _" c/ W; v' X3 z
- {
9 o, a b4 ^& g$ z+ h" d) I - old_data = Sensor_Data[0];
+ B$ v% m0 k8 B/ G2 v& O4 i - return Sensor_Data[0];
1 x: P# I. j9 N) w# ~ - }; V9 A' g; U* U7 w
- else
) {* s3 |7 a$ L- h: U - {
/ }! Q6 T' Q O3 A) F$ E - return old_data;1 G0 g7 N1 Y2 w) J
- }3 z8 z8 p# L2 N2 b1 V, B" I6 n
- }: p% y! x# ~; x- P/ q( N! v4 }
複製代碼
2 o9 \. _* c' c8 g6 @! j4 {MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
1 X. x. Z. o7 _( V: B+ e B! w+ I4 c# V! K
|
|