|
. ^1 O: t- _, K% B6 d
- #include <Arduino.h> i3 m1 G, H5 t) j6 {
- #include <MeAuriga.h> C1 X# e" x1 ^+ ] M5 A
- #include "MeLineFollowerArray.h"
M$ p& P- p# t% z' ^2 O - $ L3 q/ g# S# d( W# N
- MeEncoderOnBoard Encoder_1(SLOT1);$ T8 S* o o; Y" K
- MeEncoderOnBoard Encoder_2(SLOT2);
' O3 _5 P6 w1 i6 K1 Q5 [ - MeLightSensor lightsensor_1(12);
- `0 M6 E( L7 y0 P' P m - MeLightSensor lightsensor_2(11); c7 ?' B; |2 {1 F* I6 Y( K4 y
- MeBuzzer buzzer;
`$ a7 `1 E6 t: b+ | - MeLineFollowerArray linefollower(PORT_6);+ }% M# W7 @! G& x' s* v- V
! M$ D2 O, S# M- #define Error1 1
0 Y0 A/ j- I7 [3 i! ]4 M5 D - #define Error2 2- _, z; Y$ q/ d& h. z; g
- #define Error3 3
, x0 L5 l% ~" N, L1 P% N - + g7 Q0 `! O# }5 H
- #define Kp 151 r3 |& \& ~& h% K f% W
- #define Ki 0.15# ^1 A+ K& J {+ } ]0 f' N
- #define Kd 0.03
5 g5 p6 N. w/ K; L - ' `. }6 m4 U& c
- uint8_t sData;6 n4 a! t. K; S
- uint8_t D1;
) z0 U) b1 |: l0 r( [6 j - uint8_t D2;; P) w8 ?* ~$ g
- uint8_t D3;$ A+ D9 c, z2 [- d3 ?5 Z& d
- uint8_t D4;
+ [/ c, \/ u: }+ I - uint8_t D5;
% G" [8 Y( Z# Q; s - uint8_t D6;
) n6 B$ Y: z' b% D - * @$ _8 D4 S, y' k) K$ N# [
- float previous_error = 0; c+ x7 ]" M; Z ^* D
- float integral = 0;6 s: ^' C/ E3 g# a, {5 U
- float derivative = 0;
& c! M% g: D c( H, \* {' _ - int Speed = 160;$ [5 Y# q0 e0 ]! [; Z% Z7 V
- float output;
4 ]# q' K2 ~" Y4 L! u" E9 U$ j. W5 t
5 a$ C- R! k8 Q6 f0 D( q- byte Left;
3 S' w6 h- n, I. O
' g+ A8 s( m; w* B! X$ v- void setup()
& c/ A: P. \0 h* n) s( s2 e - {
" s: b; o; }9 Q - //Set PWM 8KHz
: R8 E# r' W) _1 Z - TCCR1A = _BV(WGM10);4 B, s( y2 I* |2 }
- TCCR1B = _BV(CS11) | _BV(WGM12);
# U4 U9 ^8 [3 K7 @2 A! h - TCCR2A = _BV(WGM21) | _BV(WGM20);$ H* X. F7 ~& O6 B
- TCCR2B = _BV(CS21);
# `8 }- F; p2 L! l& L2 S0 t" h - Serial.begin(9600);
$ o/ G1 [, a. c) J. N$ S5 d- U - buzzer.setpin(45);
P3 L9 n6 t. [" N6 Y2 y# D - }7 g8 u! K- \* S: M/ p
- ! a- v/ k/ k! X
- void loop()
/ c9 K8 J! y9 _9 k - {6 \0 ^, C+ z& X+ i: q8 U
- while(!((lightsensor_1.read()) < (10)));
1 {! q& w0 w8 p" J1 h N - buzzer.tone(1047, 500);6 C4 Q2 o0 Z4 S: }; l. v8 V
- delay(500);
* a7 L: o; M! F' Y3 O - do
! y6 N: F/ r2 ~5 H! Y# n' W; [ - {6 j8 D" g, f# A F9 W$ |
- int Err = getErr();
% v6 I" C3 A4 y4 f! u+ v - if(D1 == 1)
9 k. q: I# E' _7 b$ J7 L - {
7 c$ M, w$ y1 l$ A/ r7 D - Left = 1;; s2 L' d; m {! a" f. j" x- L
- }0 z8 D+ g$ p& r9 A0 S
- if(D6 == 1)/ D9 V, {7 y: q, q) b+ C0 v1 w4 [6 [
- {; Q# r W+ D/ {: C& R! Y% ^4 f
- Left = 0;7 J. {" F# P' k6 f
- }* w! [( Y4 D% s: Q& _& ?5 r1 X
- if(Err == 99)
9 m$ C! v6 J7 H1 s- X - {
2 c5 B! J6 S1 E+ F' V) f - if(Left == 1): _* e! ~2 c; V1 G! M( R4 l# S
- {
) s& M9 r/ f* o4 c - Speed -= 5;
) C2 G: \4 y9 P: h' H3 y - moto(0,Speed);
+ W9 z5 O- I' P" T/ v - do. ^3 o0 Z" ?1 B
- {+ v' \1 c7 Q7 f' j! x# {8 G& V
- Err = getErr();# D, S3 u) ~! E1 b0 g# E
- }while((D1+D6) == 0);
% l2 J/ m7 V/ t - }
2 t% d7 w$ _% }; \) a4 D - else. G( K; k Y* Y. y7 u3 f
- {+ T8 E( _0 ^# a& d+ x$ |! o
- Speed -= 5;
. k R0 P) O! H( k+ Y q - moto(Speed,0);
2 y, e, u( v% I - do
w9 S# G7 e* |6 z' C - {
/ D/ M( {0 Q! A4 U3 T/ t- v - Err = getErr();6 C1 o* F- |; x
- }while((D1+D6) == 0);
3 a& z( \, o9 `0 k o - }
* o5 k% D( Z1 u s+ E - }2 R) @$ Q" f: B2 y3 | h
- else- K& [( i. U0 T/ S
- {8 T, j M! A) F) _1 R. ?' v
- if((Speed < 160) & (Err < 2)) Speed+=1; & x1 b d! w" C4 _) {' x
- if((Speed > 100) & (Err > 2)) Speed-=2; 3 Z- I% Y! o$ r+ k( Y; S
- integral = integral + Err;' U8 f2 X7 z: T# B g, L
- derivative = Err - previous_error;
4 `/ p& F1 L# |& Z6 v7 w - output = Kp*Err + Ki*integral + Kd*derivative;4 L; y* I5 e) e, d" q/ x
- moto(int(Speed-output),int(Speed+output));
4 z4 G3 ~+ t6 }: ]* { - previous_error = Err;+ ]: G, c: s: A# q
- }
& n% ]" @/ m# O" J - }while(!((lightsensor_2.read()) < (10)));
0 M$ X) R5 q2 ~- P - moto(0,0);$ q$ [3 i" P. m5 s; Z& M# M4 q
- delay(500);# s9 ?* p$ K1 _- d
- buzzer.tone(262, 500);/ m9 o4 g7 s- Y. z {
- }9 h" p1 ~# `# T. u$ z2 Y" o2 ?
* x, j. K$ K9 U5 O% K+ t& N- int getErr()7 P# Q" e( P, L/ w! e2 T8 P
- { , B8 H2 ]4 ~# }7 \0 C# b
- sData = linefollower.getValue();
! `' v+ X. d0 A - D1 = ~(sData>>0)&1;
6 j$ g+ C* M* _ - D2 = ~(sData>>1)&1;
; |4 y4 U& _& }) ~) | - D3 = ~(sData>>2)&1;
; j; ^% W; i7 k7 E$ v; v+ _ - D4 = ~(sData>>3)&1;* K _' V/ E: _( U ]' r
- D5 = ~(sData>>4)&1;' _5 k; E$ D! b3 d3 K
- D6 = ~(sData>>5)&1;1 d- g4 C9 p0 v9 u5 k4 a
- int downD = D1+D2+D3+D4+D5+D6;- m5 H/ }( x. }0 R- H* Z9 d( Y0 Q
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
4 ~" F! M& c* E) S$ m - if(downD == 0)3 ~; x, |( ^' l
- {! S3 Z; O3 y! @6 q3 x
- return 99;
" g% G/ G. O( {* F - }
+ ~; r* ]. n- z. v* m/ Z) \" v - else
/ s* K1 f, G! E8 ?3 O - {; H! o4 V/ i1 \. D N K: ~. u7 k4 @5 R
- return upD/downD;
1 V) m9 j5 w! y - }
1 {6 y) _1 w5 U I. [* Z y - }
. n' }0 S3 U( g# y! T+ `3 _ - R0 u; ~9 u5 I$ e
- void moto(int Speed_L,int Speed_R)7 D, W+ V- A, R) Y' S# L- J
- {$ C7 Y6 }$ P" Y8 w# Y
- Encoder_2.setMotorPwm(Speed_L);
m' a2 R' g( E; Y. [" ] - Encoder_1.setMotorPwm(-Speed_R);
! l; H) c' I0 p - }
複製代碼 - K) x( o' y$ U4 y* M& z
MeLineFollowerArray.cpp- e( i. o f2 F- N y' q _
- #include "MeLineFollowerArray.h"; O1 k# R% m2 V
- , i: H0 a' E9 V# G9 z0 g3 x9 J$ X$ V, X
- #ifdef ME_PORT_DEFINED/ E: k# T9 @& K3 B
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
5 R' T9 a9 {$ y' S - { L% {: P; p! m
- 9 V) K0 J: I3 L6 ?8 N& l! |: m
- }; |' A C8 U& J% _( |; P; L
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)1 V, _! v; V6 C6 x! f4 \' h
- {
1 {- Q5 d* l* Q2 Q - _DataPin = mePort[port].s2;
* V" {& J- ~- T) g - pinMode(_DataPin, OUTPUT); z$ V T/ ~" M, F
- digitalWrite(_DataPin, HIGH);6 |: p" o1 x' ?% n5 t& M' C( s0 D
- }
$ Y% u/ v$ e* U: U. `3 v& R3 L - #else // ME_PORT_DEFINED. H& K% T; t7 X8 a# v% U' D9 s
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
9 |; A" E) I7 t0 ?( K' K. c$ b - {. H8 N: _ a! b/ T6 r- H
- _DataPin = pin;. M. A1 ?' Q" F# e4 F
- pinMode(_DataPin, OUTPUT); , f2 D" l' k4 T0 o9 {) b
- digitalWrite(_DataPin, HIGH);
1 [+ Q# L0 P: \ - }7 `' E7 n7 }* w) C
- #endif // ME_PORT_DEFINED
) T7 u% U1 o0 M6 R& p w
" b/ }$ K( B0 @1 v- 5 k; Q) o R$ g9 v1 E
- void MeLineFollowerArray::setpin(uint8_t pin)
6 w( f% k7 k& Y% M: K - {8 r0 e0 s' L4 r# H0 T) K: U
- _DataPin = pin;- D& H6 m Y7 K- q1 M
- pinMode(_DataPin, OUTPUT); 5 r; Y) T5 E8 H T/ k, _
- digitalWrite(_DataPin, HIGH);9 E( D7 M7 K ~ ]; M
- / {- Q+ W9 y* Z: Z/ s' z
- #ifdef ME_PORT_DEFINED6 c5 g: F/ B* d0 [& z7 ^! s
- s2 = pin;: ?. N# D3 ^8 n+ E9 K
- #endif z' @4 i5 g# {: ?
- }
) }. w, i% O h5 H% T) v# e3 H* o
2 z0 m$ x/ X5 b" H1 m6 @* @+ i& C- uint8_t MeLineFollowerArray::getValue()
, v: z& ?- [% q - {
; M F5 U# _$ n3 s* O - uint32_t LOW_level_read_time;: L# _8 {7 T+ f
- uint32_t HIGH_level_read_time;2 i+ m/ h: l. ?2 E8 _
- uint32_t time_out_flag;, w' P# D, Y& E
- uint8_t Sensor_Data[3];6 y/ M7 z/ j# K( {# x2 B! B8 v2 ]" X
- static uint8_t old_data = 0xff;
, I5 @. {, k/ X ? - ( p6 ^8 E) J3 \0 K4 c6 g0 A! W
- pinMode(_DataPin, OUTPUT);
' B ~5 W% u0 b5 U% v6 r - digitalWrite(_DataPin, LOW);
5 k1 Z7 Z2 T+ w5 j6 Q8 m - delayMicroseconds(980);6 i* h O3 {2 b& j
- digitalWrite(_DataPin, HIGH);
) g: K) G, q# X3 n& {
* @# f4 S4 s# f2 ^; `3 H) n- pinMode(_DataPin, INPUT_PULLUP);# h' ^4 K7 Z. g1 ?& P% h8 E4 o
- delayMicroseconds(10);
* q4 [# Z! K9 [4 H* m - 0 A) R5 V2 V( N2 g
- time_out_flag = millis();! e5 h2 H% ^ [$ L" O5 V
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ B2 j8 x$ R F; t8 [
4 @7 t& Y" r$ B4 O% w' ~- LOW_level_read_time = micros();
6 h3 X+ x( V6 D: d9 v8 ` - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' I5 @) M3 p) s* m8 {) L - {
8 y e; Y$ h* o; \! k - return 0xff;
" Y4 O: B/ z/ A) V) ~4 _& G# z - }! _0 y1 H, u+ k! r
- - M/ U _* C) k- L5 H; C9 g" ?
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) {2 T' B5 D Q: j9 Z" x
6 X0 a) r! d3 H3 v7 E, n- HIGH_level_read_time = micros();1 K4 ^0 O9 g4 o. l, [% F/ D9 n
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
0 b0 I/ Z" Y) z" n" r
6 c& ]- M+ W3 `# n: H7 J+ n- k- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. S$ M" p0 E$ c3 m8 u+ L: m - { U& d6 d4 J I& c. T( i% X
- return 0xff;
% d( s0 f" i& V* q0 q - }
( n- @2 x' [9 ?; r, T
) ]) g/ x7 y0 l- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))6 S) _( E, U* [* D
- {
+ v4 c( H' R$ }0 U! H - return 0xff;& F) U. i% Q' j- k& G
- }/ c* x/ m1 J; A
/ c' X0 y; ]( w8 P/ ^- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" X) U5 G- s+ m9 s0 b: t, H- p- j% _$ f
- LOW_level_read_time = micros();
6 k' c7 g0 _: F. a- r# m% {) x0 i - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
% m7 U+ q: g. p* ?5 u% P7 d- U: t - - t. W+ _. x! L8 r$ d4 ~& K
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- w- @8 F6 C- J" Q* z! ~ - {4 j7 Q" d1 X8 f6 J: F1 H; N; P# Z
- return 0xff;
- e; }' y! d" V( T) U0 N8 e - }* ^; x7 h( \, k8 T& @$ Q
1 H2 [, X1 [* I4 j- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
( ?. p0 y9 S9 I7 S% Y - {1 ~+ L, H ~; P0 j2 {
- return 0xff;
# e: J% @ a8 J$ z C4 m - }
3 |- Y0 n- I# ` - . ?: l4 B9 n& n
- for(uint8_t k=0; k<3; k++)$ p* v/ c1 b3 Y+ b3 Y
- {
& m7 B2 g; p$ f/ c/ o - Sensor_Data[k] = 0x00;
" @6 @! v# \, X0 p& E. |* c2 k* W9 d
( j; ^4 G) E9 [4 e' T- for(uint8_t i=0;i<8;i++)
+ F; c9 F0 z% @; Y - {
2 r/ [4 Z. y7 v/ v, C/ k - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ r# P4 z! V; k6 w0 t3 h
- HIGH_level_read_time = micros();
* V" J! J) e K2 n+ R3 a4 l - LOW_level_read_time = micros() - LOW_level_read_time;& O! f" r9 u; C( ]% n' I
1 ]+ K O3 n3 X1 y) J0 N* s5 o4 h- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
( z2 J( A1 }; M: l - {
% v+ g& f) y# W- X- K - return 0xff;
9 g" M, [0 n# O' N% `- t- I - }
, B! l5 I, T3 n" v
4 f$ t6 _) s& S% k5 w. d- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! f0 z+ C, z& O' y. a
- LOW_level_read_time = micros();. X7 Q: B: H2 [$ h& K
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
& e0 G3 y& @" v- p
! ~4 v3 p$ J" D) b* T/ E- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
0 e' w. \) C ^9 |7 ?3 t& p - {
4 x6 L& @& N) t8 m) z) k1 D% F6 P$ F - Sensor_Data[k] |= (0x80 >> i);
" `( T$ h h. v9 x - }
- R7 p, z; A* h- `0 H5 @! m - else if(HIGH_level_read_time >= 100)
+ u& Z) y6 N" h7 ? R7 M0 h4 ^7 F - {/ a" ~0 l" ~* X4 z3 o
- return 0xff;
* V* q- S: J: Q& F* q3 F! N+ F - }
( }% E D& x" Z3 a
) c0 Y; r* |3 y- z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
, C/ Q4 `& z7 M( v3 ~ s5 h4 q - {
+ ^/ d# s, h% {7 V - return 0xff;6 V6 \/ Q J# W; j s+ e8 p$ T$ m
- }
2 ^# m: A* a) M0 T+ g) k T" Y - }2 l# ~9 r( y* Y, \5 ]
- }
/ U* T: O+ S4 i& H" ^% e
; m0 z' ]4 S9 d' T' `9 y8 Z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ S- \7 y3 k) u; o' [) w% O
- HIGH_level_read_time = micros();) r4 h N3 @9 b& x& U* c; ^
- LOW_level_read_time = micros() - LOW_level_read_time;
# Q- ^) j2 k5 g- |4 U9 }
8 p h; i# {; v6 d3 I# }- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
5 C* a+ o& t; f8 F( }- M1 L- p - {
9 j& m" d! A8 @8 n - return 0xff;: x$ a K3 p! `$ f+ ^" w' ]6 ^
- }; V& J0 {1 x! K6 J$ S- l5 \
- 4 Q6 E- V# F$ d* C1 p, P! k' ~
- pinMode(_DataPin, OUTPUT);
) O7 O, _; s) S3 z0 h& Z3 W - digitalWrite(_DataPin, HIGH);
/ L. O& E, L+ g( F' } - 7 Z+ v/ x# U& I( n
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
" [! V: t$ E3 L4 [; G: A# f' | - {
4 w( }. N. k/ i - old_data = Sensor_Data[0];
5 R l$ ~$ {( u8 s+ a$ ? - return Sensor_Data[0];( i1 l; a! | C, @2 @8 x% o% {
- }0 [1 i, n, T7 S6 @* F
- else
! Z) @: Z& P3 e. z+ B1 _ - {; h2 h* ]' Z* t0 j! j
- return old_data;3 s; a- K2 u9 O2 k6 [
- }
( \9 c$ k" { ~ - }8 ]; [* Z/ O1 `+ z
複製代碼
5 ~/ Z5 x6 \: c% C) o/ @* D: k8 d7 UMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
7 h- o( s+ _7 \7 k
0 ^; H$ h Y) O/ [ p) a# A! e. p
|
|