|
|
" r* S& e1 T1 y( Y0 h( W& p- #include <Arduino.h>
, \& F; R$ s1 y3 f& | - #include <MeAuriga.h>/ e' q! R6 K. N! I5 @
- #include "MeLineFollowerArray.h"
2 d9 R9 c% ~4 Z) b8 i# C - 1 j+ g- B2 l3 a9 `0 A
- MeEncoderOnBoard Encoder_1(SLOT1);6 f- k% }. |" |) G2 c
- MeEncoderOnBoard Encoder_2(SLOT2);
$ A! z: r7 L1 W - MeLightSensor lightsensor_1(12);$ g& t0 Y1 s/ K1 V$ ]1 l
- MeLightSensor lightsensor_2(11);
$ n5 R4 }* _6 Z+ L* r - MeBuzzer buzzer;- V: k7 J- |9 Z3 f1 d. T7 {: v
- MeLineFollowerArray linefollower(PORT_6);8 _; }. ~- o4 U
; k# V: U6 r, _+ l- #define Error1 1
3 _3 x& }( }$ L" e - #define Error2 2
% O8 N( l8 V! u - #define Error3 33 v7 y: J- V/ i
- 5 ?3 z a# v' G& F! D, _, R
- #define Kp 15. j5 o) Y& U: K! R% f
- #define Ki 0.15
& l6 a$ X# {% k9 d8 c7 H0 l# L - #define Kd 0.03, G' _( d& z! G& S
- x& g. ~. Q+ g9 w! j
- uint8_t sData;1 y* u( `6 e% z# C
- uint8_t D1;0 J( Y# u/ Z9 I/ `1 q, z
- uint8_t D2;
8 G: j! U' M, L' k2 @( V - uint8_t D3;/ r7 r/ ` k% A$ ~; d' v" @
- uint8_t D4;0 i# M6 N/ i- {/ C/ f& P0 E5 R
- uint8_t D5;
: `8 T- s* T( ~+ H% [9 Y q0 G8 k - uint8_t D6;& x$ z' c% F4 y! q1 c1 E
o2 {3 g2 ~1 ~* x8 d4 N5 i- float previous_error = 0;- @2 o( n1 ]9 F
- float integral = 0;
+ B) Z& A" A$ U8 ~) n - float derivative = 0;4 B" G; J2 d3 V7 V
- int Speed = 160;$ w# ^8 ?+ ~ B$ v6 Q2 G T
- float output;
+ M2 _7 _' N: \ q4 B5 k - . _4 I$ }$ F& v; b7 u; s
- byte Left;
: M8 w5 I' E' Q/ W- s, R5 Q - / C( h6 n/ [( d; @! t
- void setup()
' s" D. Y' J# c$ n/ {3 K( {8 I - {, P- U3 l) P/ {2 c' e5 X3 T
- //Set PWM 8KHz6 g ^6 R% I& U9 f! ?# s
- TCCR1A = _BV(WGM10);
& h9 e+ ?) S0 D2 K. h - TCCR1B = _BV(CS11) | _BV(WGM12);
/ @6 s6 P, K: T- ^1 j - TCCR2A = _BV(WGM21) | _BV(WGM20);8 }$ [8 v M% A+ H
- TCCR2B = _BV(CS21);
* x, A5 p0 w9 c3 S* P - Serial.begin(9600);; X! J. S: r' O( D c" v2 E
- buzzer.setpin(45); m5 T& C9 F# l% ]( k4 h
- }
p8 X3 L, K @# Q
. o w6 T5 [2 x+ M) i- void loop()
& @" F- v( r3 R/ n1 c; u _ - {3 a& J! s, K6 h
- while(!((lightsensor_1.read()) < (10)));
4 \1 Q/ z- ^+ |, ?8 Y - buzzer.tone(1047, 500);) \2 y. S3 q& h
- delay(500);
! L2 q+ l' A- L$ C1 h4 E - do
! @: p- q5 ]) A" u/ N+ [' G - {) S3 p' E; z2 f6 j
- int Err = getErr();
8 t0 |1 F8 k" Z6 [" G - if(D1 == 1). T* J7 F7 x( T. F$ T; [
- {6 _+ P8 H1 N3 D6 w' t$ z7 Q
- Left = 1;% C/ J `' G& L/ |
- }
9 _! s4 [ x$ q- {& {: | - if(D6 == 1)' ^' q, v. l7 h8 |
- {
6 Z% L8 e/ C2 U5 [" M/ q" H - Left = 0;
$ M; z, F" O3 I& h, A* A - }
6 i5 H \1 C X1 R: f - if(Err == 99)
1 z8 r: R& t! _ - {: E3 N4 x! `$ ~ u) W
- if(Left == 1), K: E& h/ G/ j1 W7 S; f" i" r5 e# ?) ]7 l3 J
- {
2 _1 H) ~% a5 @; ^- H) V2 v - Speed -= 5;1 D& }6 J {3 O2 B
- moto(0,Speed);
! `; [' J P+ F6 d% z/ M0 b - do; [1 c* o8 D2 `% _1 P
- {" Q/ q/ k9 p+ x) Q$ C P1 ]
- Err = getErr();
) r7 K! _" ?, z* p - }while((D1+D6) == 0);6 C3 M- i0 ]+ Y+ j0 M2 C7 w2 X
- }
, ^/ S) m5 y& B }7 ?5 ~* q - else$ W( j* h9 ~* ^" ]
- {
/ E7 x+ @4 O& E" q( W - Speed -= 5;
7 g$ I( W: A4 K' K! _ - moto(Speed,0); # e1 v; A- A! B& s4 N3 N" l. k
- do
; v1 W- Z, X) z5 c1 F% J5 A9 ^0 Y, ` - {6 Q: O+ E- z* B3 B. d: ~, ?
- Err = getErr();
- s8 R% l% K; R# k9 J# d1 ?6 e: Z - }while((D1+D6) == 0);2 w3 R5 Z' `% L- H6 G1 Q
- }
" h# V4 C" e) J# \% V; O1 |3 I - }8 c7 h: ~! ~1 e. `9 L: O
- else
% R; `% c% r) d! _ - {4 Z$ L2 e: D$ w
- if((Speed < 160) & (Err < 2)) Speed+=1;
" A$ `4 X" M# V( G5 o/ D( K - if((Speed > 100) & (Err > 2)) Speed-=2;
3 G0 h; i& K3 S f" I6 G5 c8 h - integral = integral + Err;, x4 T2 C' p. U3 r3 f& T
- derivative = Err - previous_error; n- j2 Q1 Y7 ]
- output = Kp*Err + Ki*integral + Kd*derivative;
" M% `. M+ F4 Y8 i - moto(int(Speed-output),int(Speed+output));
# k1 S( y% D; a' f5 E" v - previous_error = Err;1 f3 L5 W" V# D3 H0 E
- }% k8 k! d# K$ P, t
- }while(!((lightsensor_2.read()) < (10)));( W# c' \. u4 z& \9 Q
- moto(0,0);
* V2 R% F6 s. q" y& Q: P0 N' V3 n o - delay(500);7 P( H) v: a# T$ j, c* p
- buzzer.tone(262, 500);
+ M% }# N) y8 V- v& t - }1 t* r7 |6 Q" `) u
$ f0 q! e5 q8 g v* y- int getErr()6 ? A3 ]& R+ `1 M: U! r2 t; v, V; K
- {
1 H( }* C3 C' e% ^& e& _ - sData = linefollower.getValue();
5 C# I+ Y1 V8 p* `: p/ r - D1 = ~(sData>>0)&1;
0 O9 }5 }" A) f, S+ }1 r - D2 = ~(sData>>1)&1;- q9 k7 B& F2 ~1 u3 f. s- y
- D3 = ~(sData>>2)&1;
- ?. K8 G) }1 w - D4 = ~(sData>>3)&1;
, S9 m! e* W# s* q - D5 = ~(sData>>4)&1;1 X" E: ], Q+ l* m8 n
- D6 = ~(sData>>5)&1;
; z/ l; T% ?5 R - int downD = D1+D2+D3+D4+D5+D6;
4 ?" G9 g) \5 K @9 J' C - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);1 j+ t" x2 W% j; V9 ]7 Z$ V
- if(downD == 0)
, @# _8 m2 s% N! Q# d - {% N" ^- O8 j) {( l. U5 O$ m+ W5 B1 i
- return 99;. H2 p+ h R+ ~5 [$ |! o* V
- }
; J+ W4 A8 X. b! g+ s - else1 o& G" r$ a) x9 \& c1 A% }
- { `+ G: L& D8 e/ y7 B- d
- return upD/downD;
- T k' }2 N+ l; z: p0 Q0 G - }8 E7 M' _* @9 {* `
- }0 T/ c9 x& w# |( d; X
1 Y p. \0 l# c0 {5 \5 ?3 |0 R- void moto(int Speed_L,int Speed_R)1 h* ~; q7 R9 N# a& {) D& R
- {8 q+ {) X, C- r) w8 J: r) ^; c
- Encoder_2.setMotorPwm(Speed_L);0 v+ T: T$ B( M h
- Encoder_1.setMotorPwm(-Speed_R);& h# W; Q+ N: S) n
- }
複製代碼
; j1 \3 Q4 ?7 {' P+ g; sMeLineFollowerArray.cpp3 O: ?9 ]* x" i5 g" h Y
- #include "MeLineFollowerArray.h"
* p! [9 ^) J" K5 }2 C) m5 S c/ j* s - & N% D5 Z$ G2 M$ T( h, |
- #ifdef ME_PORT_DEFINED! B9 p! K3 n6 E: g3 T3 H }
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)- M7 s' h# T9 l' j1 K
- {
# d( C+ n7 e. L- r$ D- {) w/ g - 5 B' F0 _" _+ F8 r( Y' y
- }
$ ]/ j5 s1 N2 c$ D - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
/ _3 T5 ~: m Y - {" \0 V' g7 Y! @( ^! d' s0 J
- _DataPin = mePort[port].s2;8 y- M9 ] u' z7 e3 [: \. A
- pinMode(_DataPin, OUTPUT); 0 Y0 s( h2 }$ K8 {
- digitalWrite(_DataPin, HIGH);- y' e3 H/ y j N- {6 y6 Q0 O
- }* |! |8 j5 |) ~, {* E
- #else // ME_PORT_DEFINED# }6 }. }; O! _/ l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)- d7 L, W* J4 U/ E
- {$ {9 _1 Z- U. d$ ?" {
- _DataPin = pin;
- |* c1 _- {6 i: {3 _& _ - pinMode(_DataPin, OUTPUT); & I6 S' O2 ]8 B; p1 m9 W
- digitalWrite(_DataPin, HIGH);! k( {' f& m- S- j
- }- ?3 K% y% N. ]+ {3 N" [
- #endif // ME_PORT_DEFINED
$ W) a# A5 ]( u$ z - 2 n7 e9 T$ l: A+ C: x
9 j( n3 E, ?2 L- void MeLineFollowerArray::setpin(uint8_t pin)/ n( g) k8 t, l; o
- {- o% a# R1 H" a/ @; w) Q
- _DataPin = pin;5 l% C" @4 x& e2 }
- pinMode(_DataPin, OUTPUT);
& X- `6 u# I$ O2 n. T5 V - digitalWrite(_DataPin, HIGH);
8 \6 {0 a+ o+ O$ {) L
$ M2 q) u, b# k5 x- #ifdef ME_PORT_DEFINED& P8 S/ h) x! @- D
- s2 = pin;4 t! }5 p; R& f2 a6 z4 v4 y6 A
- #endif/ M5 S1 z. w( n1 I1 L9 m9 \
- }
s4 U# ?1 L% o" @: U0 A) U& _
- l9 A$ V; h+ m" ]9 S- uint8_t MeLineFollowerArray::getValue()
; w* T ` I; C) v' |+ i( b0 c, O3 ?4 f - {
( E* `0 V b6 [+ v) O! K - uint32_t LOW_level_read_time;* b1 r$ D! ]! \, e; B* t! E6 b2 Q4 C
- uint32_t HIGH_level_read_time;
' |) ~! u. H: d$ D5 Z# k. G - uint32_t time_out_flag;) {5 H* |# p4 d
- uint8_t Sensor_Data[3];1 i' s6 P* ~! h8 U7 h" O5 F
- static uint8_t old_data = 0xff;
( s( i/ |8 P* t
4 c# }6 Y0 x7 s- pinMode(_DataPin, OUTPUT);
& I- \+ V6 T& |2 h+ I4 b* v - digitalWrite(_DataPin, LOW);" `4 N; Y" y- v0 n V* h! g# l& o
- delayMicroseconds(980);
2 t1 ]7 d; a3 H/ N1 p8 b9 ^2 | - digitalWrite(_DataPin, HIGH);. @# k* A' t; F; p# G ~
1 f. o# F9 z6 \9 `' C( [- pinMode(_DataPin, INPUT_PULLUP);4 `9 M9 G2 k' s- E. N/ t- Q
- delayMicroseconds(10);
* C5 p8 l2 z* i5 H - % C- E" R) A5 k, X
- time_out_flag = millis();% L1 J# X K7 C8 R @9 d
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% c3 s5 }+ I& | - % H. z. R) n1 G: F: M, G: F
- LOW_level_read_time = micros();; i8 m5 z4 m* L
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
9 i" O1 C6 f' F k+ A' p; ] - {
5 v/ [6 H6 |! h+ a# v8 m: y6 S - return 0xff;$ J" { I1 E) ~8 t D. d9 v0 H
- }
, ~ `. ~- b8 O4 B$ C5 {! r - / J$ ?# y. ^& A8 s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 t/ x" T; @1 T8 P4 _3 g3 w: J
2 G, o/ H8 w5 A4 z' g6 Y2 m- HIGH_level_read_time = micros();: C5 j% N9 J% Q `( E7 J
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level5 p: ~4 ?7 `2 g+ E1 \9 H+ U
/ s- |8 T/ M W- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; w* o: B5 `7 M$ V - {
0 Q+ \+ `+ p4 k+ b& b! } - return 0xff;1 x, z: t# {$ L/ d+ z
- }1 b8 Q# h5 {; t( e
- 1 x) s; B$ y) |
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))) R* B# j* Q5 r5 H5 U4 P: C" i. @. c
- {
7 G# @+ d5 I( J# s! l' V/ N - return 0xff;: j! R' }0 `5 m' j; ^0 q& S
- }
1 l' @; ? r/ \2 O. c- } - , b- c7 K4 u- J+ P: v
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 @) ^( [# T+ q6 ~6 y, _ - LOW_level_read_time = micros();
+ i( B$ o; v2 z; z5 E! }- d - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
$ Z t- B' e! \/ H% B" }8 D - 0 F0 z- |( O' d( B: h5 a( z1 c( e
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out) j# e( i) b# ?' Z( S2 d! a
- {1 Z& j, _9 u; q
- return 0xff;
: J$ Z7 _# p7 @0 Z8 {3 \ - }* G( c2 M7 G1 C, k5 G6 o/ Z1 \
- # J6 @2 G2 w- O1 W( l* g1 s* M
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
9 W9 I! c4 z) v* W; I$ P) F# S - {& B) r- `6 ?8 F
- return 0xff;
( ]6 G6 M/ P( P- u% _4 I - }
, C" M: M( A+ q9 v2 D
! x G% C. `4 X4 b1 M* I3 }9 I- for(uint8_t k=0; k<3; k++)
- [; d1 {' o3 B( \) d - {4 w7 d& e7 Y! W
- Sensor_Data[k] = 0x00;
+ \4 J7 E! g+ a# a
/ p: q. @# `5 `* }- for(uint8_t i=0;i<8;i++)7 O* p$ |8 ~( J9 z* ?
- {
4 D3 f& b% j6 S ?; z - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
; g# n! s7 M7 g$ I$ T - HIGH_level_read_time = micros();
( v2 N9 e9 i2 U5 z- R5 P - LOW_level_read_time = micros() - LOW_level_read_time;
r; D$ c6 H, B# V' Z4 d
& b3 b& f, Z* `' j L' e$ L- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) a( \! o6 e. [: w: s - {; l( P" r) _9 s C" G: v
- return 0xff;
. ~5 ] r! L' y' W" s! O2 Y - } F& C' X; ^ |1 ~0 s' {0 n
- ' M1 `4 T/ S8 g& v/ a% n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
& L5 p; {1 p! `; X9 M - LOW_level_read_time = micros();! W# T0 j" J; ~7 m
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
5 S+ X# W+ t2 a+ q7 X7 [
( M2 n, s8 q: l4 H. l8 M( n" G- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 11 b7 i# h( i- q
- {" W5 Y* o( ?- [7 r- L2 Q
- Sensor_Data[k] |= (0x80 >> i);: a6 u( ]4 y( x x
- }% _- [. c# @- U- c- _
- else if(HIGH_level_read_time >= 100)$ Y, {- b2 T, O' F
- {
# o( f' i* j I3 [1 }1 F - return 0xff;
+ D' Z0 @* h5 c, E - }. _8 S1 l a/ [1 d6 d
$ }. {7 B7 m4 i) b: J0 W% w/ y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
' }/ d3 I& k D2 @0 z- n1 J - {/ l( x; T3 X. b) G0 M3 v3 w: ^% \
- return 0xff;
2 f" ?. [% g+ Z6 k% y5 B - }
. [9 z& A- A5 \! x' W) R - }9 G* _ W) m& g+ x' B- A
- }* Y% J1 z+ T [- z( [" a8 G
- ; h) u C3 c# G
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level1 ?# U8 W' n3 v3 v+ r
- HIGH_level_read_time = micros();( }1 k( m" D$ s1 h) d% G3 i- o
- LOW_level_read_time = micros() - LOW_level_read_time;
9 n" E3 q5 o+ t( x3 `6 C
) f5 `8 E! n, X2 p# u( j; W- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )' a' R0 ]4 z0 z/ p
- {' P. O' U8 d5 Q2 W+ Y1 E Q2 Y
- return 0xff;
! b: ?2 v6 }- c: H5 F: i) _9 ] - }; U5 B2 a6 r$ r2 u
% h0 Y8 b2 S3 n8 K$ M: c7 D- pinMode(_DataPin, OUTPUT);
1 o' E: u' a, `/ V% i/ S - digitalWrite(_DataPin, HIGH);1 p6 p$ N# V' y9 Z
. S9 J% l; ^) ~+ I- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))7 v1 y$ Z1 {$ Q. L4 E" Y) B- B
- {
- T3 g" W' b: Q' @8 G - old_data = Sensor_Data[0];, h- V! f7 q: S+ @5 \
- return Sensor_Data[0];7 |5 w% d' }# P$ _7 g8 ]
- }7 x5 z, {7 c. \) C7 v+ R; t
- else8 o- F ^# z( f* E
- {5 R- D# N7 ^) H
- return old_data;
/ e' P7 ]8 u5 [+ w) \. ` - }5 F; B) d/ z7 `
- }
! I1 I: h5 G8 }3 c
複製代碼 $ W: D' [; E4 C( K" e' n% D
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
2 {; V4 R# m% i% m# Q* ~% l$ ]7 Q) |. y8 E9 t( q9 q- i! F. f
|
|