|
|
2 j- ^# x$ C0 ~/ x( h
- #include <Arduino.h>9 j' X- V6 {& n
- #include <MeAuriga.h>
$ @0 I# }* Q8 ], \ - #include "MeLineFollowerArray.h"
- T* }! a" ]5 l" u0 q* P2 A - / H( W2 e C7 I$ o* P! |
- MeEncoderOnBoard Encoder_1(SLOT1); q3 a0 p* a) L7 [& X0 N. ]1 u4 Z) N
- MeEncoderOnBoard Encoder_2(SLOT2);6 ]; }( a5 t- q- j1 [# `
- MeLightSensor lightsensor_1(12);
! I- b. ?, K3 t9 i# @7 D/ p8 | - MeLightSensor lightsensor_2(11);
1 L6 u; ^. o: e/ Z' r - MeBuzzer buzzer;: d+ X6 G/ X, V% W4 u% ]! q
- MeLineFollowerArray linefollower(PORT_6);
8 `3 o+ _3 C z; }' `5 i - 9 C$ y3 [! }/ }7 x
- #define Error1 1& t9 U: @2 }7 L& P! _$ u" j2 b
- #define Error2 2" X4 r0 ^$ j- y+ A0 d1 m
- #define Error3 3
5 V3 j2 L) {( Z9 q - ) R( H" i% o: u" c
- #define Kp 15* C$ K9 A7 G3 ]& Y; K0 h; N
- #define Ki 0.15
% L# Y N. Y0 c T2 _ - #define Kd 0.03
. F0 @7 J5 W* Q
G+ p* `3 A% `9 |, O- uint8_t sData;
8 d3 i+ X1 x( v$ N4 L; I - uint8_t D1;9 q+ o) G2 N2 ?+ H0 s5 @
- uint8_t D2;/ b$ g3 n( v i
- uint8_t D3;
1 m- Y6 P. G! ]0 t2 n - uint8_t D4;
+ N" i. y, Y* N8 D7 W& Q - uint8_t D5;
& l8 |! u) T; {8 ] - uint8_t D6;0 c; `4 B+ s* |' ^6 ^8 |
- 2 o4 z3 b9 }( p( V9 D( @
- float previous_error = 0;
( M/ p. x, O- p* Z. ? - float integral = 0;
2 w" X" H' s% f. s; t/ E - float derivative = 0;; B2 c8 X7 l8 ?7 f0 F
- int Speed = 160;
- @- D+ K' B" h$ Z. j3 X - float output;" _ Y0 o: f# |: t
! s4 ~: T4 V( q, F Y: q8 O) o- byte Left;
" X7 K( P Q' f6 ~7 N - 4 z" N9 c: g% {! b1 z
- void setup() 3 V+ z) r- A# C! ~0 J' ~
- {
# l B) x( W' n - //Set PWM 8KHz
0 ^* R n) E& s - TCCR1A = _BV(WGM10);# Z$ m; D$ c' U* ?8 j
- TCCR1B = _BV(CS11) | _BV(WGM12);
6 g' e+ m6 o+ W - TCCR2A = _BV(WGM21) | _BV(WGM20);
5 w) [( _7 g# |$ ]3 Z- P - TCCR2B = _BV(CS21);: j# ^7 y3 O, a9 ^
- Serial.begin(9600);: H6 S9 K; N- o4 r! d
- buzzer.setpin(45);' {# s! U) v1 D2 |/ n' P9 Q
- }
- A% v+ f- d2 u- e- W
* g: ?. ^5 z9 N* Q) M( U- void loop()
: D$ |1 J J- t: t8 Q/ d; u3 Z% x - {0 T. O3 @% B* Z9 j
- while(!((lightsensor_1.read()) < (10)));" ^& L A( U) r& K1 Z# ~
- buzzer.tone(1047, 500);. R( h* @8 v" G* h, z
- delay(500);9 r* Q/ X* V9 v
- do4 u, T9 p% L" l9 ?% S! w0 k0 l9 W( v! ?. b
- {
6 i! r; a2 G% T: [* `4 J. F - int Err = getErr();
7 g8 ?4 o6 p6 e6 D$ ?9 y7 \* u, I Z - if(D1 == 1)
6 s8 D& e! V! Y7 L* j7 S - {. U, e! w1 V3 n2 \
- Left = 1;7 g, Y* r, |9 f% F- P$ G
- }; N5 }% W7 Y; l8 n
- if(D6 == 1) ?8 a6 I3 a) z! r6 y. p
- {' @1 I5 w* i# A( c0 S% R; |! \ ^
- Left = 0;3 O0 k$ `8 m( y; s. @
- }
* D! K6 ?4 L, X Z! M - if(Err == 99)
: V. F3 G0 ?; ] - {
$ g" n: F$ R/ V6 `; k5 S - if(Left == 1)# v, P8 J, P: w
- {
- c |/ a, k- u& z m9 ]5 t9 q - Speed -= 5;. E3 t w7 L5 O0 a
- moto(0,Speed);% j1 T c/ O5 X7 N% |0 O1 n! Z! {
- do; q: K2 v- {2 e! W1 A
- {# C8 K4 d5 s% G! D8 M( m; d
- Err = getErr();: m& q# L0 i2 E
- }while((D1+D6) == 0);5 O1 y- x( k9 {' t( d
- }% k. N+ V$ T+ h; H' m$ M
- else& ~- z8 r. F1 L8 q# t, O1 q; h" a
- {
- ?3 u4 e& X/ { - Speed -= 5;- ^! y# ?9 R* j/ y S0 @
- moto(Speed,0); 6 ^4 m; b' p& C$ J
- do- h" N$ Y0 I5 n0 w0 G. E7 i
- {$ h9 ^3 ?* \ l
- Err = getErr();
: b5 ?$ z ?% [ - }while((D1+D6) == 0);
: |! Y, J/ x L* _4 ?4 j" Q9 g$ G a - }
- A- ^4 }) G& S: s6 ?1 C$ i - }2 w$ U1 ]% _8 x1 A
- else+ R& K1 `! v7 T: c% I B5 F
- {: I6 u6 e- @6 ^6 D5 B% B1 @) h4 g
- if((Speed < 160) & (Err < 2)) Speed+=1; * J" Z" T9 c) O+ I4 d9 L
- if((Speed > 100) & (Err > 2)) Speed-=2; 0 S( s: I% y7 B0 W/ X+ t0 s
- integral = integral + Err;
( P" a! t8 l6 _& S7 ?5 U3 y - derivative = Err - previous_error;
/ Y3 Z3 z' I8 ?( M- Y( X1 t- {3 x - output = Kp*Err + Ki*integral + Kd*derivative;! A& P2 ~; X5 g
- moto(int(Speed-output),int(Speed+output));" U: x- w; i1 }/ M( K9 e/ s0 {0 {
- previous_error = Err;
: s' z- L8 g- _ - }
& l' F" D9 L! X, _ - }while(!((lightsensor_2.read()) < (10)));1 t* d! G- [, E9 d1 G8 h! P
- moto(0,0);5 e1 N- u6 h% A/ J0 `3 _2 O
- delay(500);% G( m: O6 \; y! r
- buzzer.tone(262, 500);
- e8 d5 k5 v+ [ - }
+ Z3 f3 v% u3 U9 |2 ^ - ! h `) o" c( B! [6 k. Q! h
- int getErr()' \; v( S+ j4 G; | \
- { - a# @9 q# s; c, f
- sData = linefollower.getValue();, S, j6 U# i$ C- W; g
- D1 = ~(sData>>0)&1;; p. |9 g# d' G+ c% t
- D2 = ~(sData>>1)&1;
$ f4 y! {* I! c- d - D3 = ~(sData>>2)&1;
9 V7 H. X7 o$ ]% H; y - D4 = ~(sData>>3)&1;( |2 T- m, A3 [8 G: ?: G7 q
- D5 = ~(sData>>4)&1;+ d4 O' L( `$ j1 X9 C" o; w9 {
- D6 = ~(sData>>5)&1;0 l1 E- |! z$ o' @
- int downD = D1+D2+D3+D4+D5+D6;
1 T! Y3 U1 f6 |8 H - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
% [& [& Z- ]9 _5 d. B2 Z# {5 \ - if(downD == 0)3 J, ]! p5 @6 C
- {
3 V _6 X# {' [ - return 99;% o$ s8 r. T- v
- }4 P+ A+ X2 s) |
- else2 b" G g0 z& E y- j/ {
- {/ y. _. o, R8 z% D
- return upD/downD;; i% K4 \% g8 i8 h; T4 n- m- G
- }
- {' h6 d5 g* o - }, b6 o' p. z# N4 Y8 r
- % J- F3 u9 ^9 y( Z5 b7 |0 y
- void moto(int Speed_L,int Speed_R): _; A2 _+ v% G. I. ~; _
- {
4 y5 q" V; z; B; L c0 T# a8 L" M* x - Encoder_2.setMotorPwm(Speed_L);- b! R+ W! {9 {* {' U
- Encoder_1.setMotorPwm(-Speed_R);
& Z V# h" L" K8 P - }
複製代碼
0 a( W- w2 [* I; v b$ HMeLineFollowerArray.cpp0 p n$ y( \% B& |. @& @0 w: p4 G
- #include "MeLineFollowerArray.h"5 Z1 J4 _7 B7 ?$ c0 N
- 4 z' g" x$ G0 Z Q+ | D. F% i! K
- #ifdef ME_PORT_DEFINED8 R- e9 O' d2 ~. g
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
, }# o+ M3 M; W* k3 R; B - {5 t& t0 d) Y, X/ a
- 6 K/ ?4 l# I8 [2 W/ Q
- }/ p4 ?! r8 h R* i; q& G2 c$ ^
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
: \% S- d6 q- R" q6 {& |$ I8 O- r - {
4 c; L1 A6 ^6 o1 [; v: w4 M, U - _DataPin = mePort[port].s2;5 z5 E. B' s. g5 m" U4 B2 S
- pinMode(_DataPin, OUTPUT); ; c9 z1 p3 P8 s- l8 a3 Y b
- digitalWrite(_DataPin, HIGH);
6 w0 [/ ]9 x3 O* I7 n5 ?- S! q! E - }
; J& a. d7 {2 I9 V; X - #else // ME_PORT_DEFINED
/ B# O; E# I; s% T - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
6 f; `3 M9 a2 Z; g - {
' X. F, a( d. [( @9 x( X2 q - _DataPin = pin;% m! q' |) [, l
- pinMode(_DataPin, OUTPUT);
A$ R/ ]8 n' {! _& \) b0 N - digitalWrite(_DataPin, HIGH);
! ^: N) R& U$ y5 H" h - }! T( D {; G3 r
- #endif // ME_PORT_DEFINED
) F3 i$ z& x3 P: p* {# c0 ~5 t% t8 _ - 3 y9 a; }3 [" p7 b$ h" z& }; S$ `
$ ?7 D( X0 ]! y# U3 v- void MeLineFollowerArray::setpin(uint8_t pin)5 K+ \' J5 O1 A8 C( m! f0 V5 k8 I
- {
4 {" K$ D5 T0 @# g - _DataPin = pin;
0 Y; R: @# |1 p l - pinMode(_DataPin, OUTPUT);
3 q4 k! H9 v4 l. b# | - digitalWrite(_DataPin, HIGH);6 G F3 x6 y. u- f8 u) M4 d1 J
; b0 j7 u8 s) b: b$ @& _$ M4 [- #ifdef ME_PORT_DEFINED
! ^, L: Z, ?: W6 \' V - s2 = pin;
: v% y, z( a+ K0 x - #endif
# F1 u! X- ?# ?: O+ F2 H - }/ B4 A, K: z8 C1 k- F: a9 H
- 8 y! |, D9 O/ M* }7 c
- uint8_t MeLineFollowerArray::getValue()
& p0 T) [' o3 d - {
* [+ f. F' t+ V6 { - uint32_t LOW_level_read_time;
7 P- u6 I2 h% q' x+ S) u+ A0 o1 H - uint32_t HIGH_level_read_time;
1 Y' ?& }: o' B: E0 H0 f - uint32_t time_out_flag;
, ~3 X" J1 e2 x4 a d9 q9 d - uint8_t Sensor_Data[3];
1 x3 p2 M/ {) D4 ^ r - static uint8_t old_data = 0xff;* N+ F1 S6 X4 \/ v4 m2 [
- # z' u& d# {+ u! l7 F6 U; c# ]4 K
- pinMode(_DataPin, OUTPUT);
f' _; K4 u+ g, L8 l* x4 o - digitalWrite(_DataPin, LOW);( x4 O" _4 u- y9 [2 F1 E% |! p c9 b
- delayMicroseconds(980);5 o( h1 U( W8 D4 }& L9 F2 e
- digitalWrite(_DataPin, HIGH);; M4 e7 p4 S3 ]' ]+ c0 d
- $ ]0 t. o7 b2 ~: _- _# J
- pinMode(_DataPin, INPUT_PULLUP); o, ~: r1 A- [- E2 a) V* _
- delayMicroseconds(10);
9 U% U& c: i) ^- X% s
8 v* Z9 k3 o, Y' s' o* X3 d- time_out_flag = millis();
* c, G \6 V# n1 X; j' M8 t - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% J, c7 s8 G. i* o. s# ?( J
- : \ l% k# \# d1 ^0 X# t. |6 P3 f" `& B
- LOW_level_read_time = micros();
: y4 V; D3 i5 J- g - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
: ^4 W! |4 q$ d8 x! d! }* q$ M - {
0 b" m, b" x) n: [" A7 e - return 0xff;! ?2 w& e/ V: p' g4 Y) e+ s
- }1 G% q7 e6 J# H; }/ H
3 w- W" _7 V6 Z6 P% m7 T3 r4 d- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
4 u. D1 `1 u2 u7 Q2 R- { - $ j1 A2 d1 @& i
- HIGH_level_read_time = micros();
" r8 Z. \( d; _& |4 P5 N - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 S/ F( D: s( E" M# L% M
: g. C& W/ q0 O8 D+ F- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% l! Q) u s/ N - {
4 a; y, h/ V- Y' p% `; z - return 0xff;, K' @9 \9 f# Z. q+ }5 F( i
- }
- W! _% L' ?' j3 S - 3 w$ Y1 |: H/ ~4 f$ G; C
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
9 t4 W; l1 B5 M6 X7 y - {! A+ q# P4 ~$ O& u% p# ~
- return 0xff;
; r4 Y! T! b' C* F - }9 \, O/ o( c. S, W
S4 e2 o9 a1 A4 y( f* v+ {2 e- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 g0 ?# w! ^( j0 g - LOW_level_read_time = micros();
4 E0 V- L& u- f6 f H# \( w2 y0 O. j - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level8 K% D7 E& U( {" M9 ?
$ S A, _% X& z5 S( k4 x- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out+ ^* @+ a) r5 K) a& ^
- {
7 M) x# O/ e) i* @ - return 0xff;
& }" o$ u7 g8 \8 @3 F6 V - }
% m# E. a2 Z& a8 _# s- p: i- b - 7 _8 w/ [. j! V: B# t
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
/ x9 H8 J+ g Z+ R7 t - {1 e( H" N/ `* k' N- F* b J9 j
- return 0xff;# k! {: c( ?) }6 F4 B2 Y* E- q
- }1 M' W: N% U/ o5 P9 _# Y2 W! [
- 5 O3 f% ?3 |; w% ^+ e3 u1 p* o
- for(uint8_t k=0; k<3; k++)3 R5 y% S# l% ^' L% c
- {
% f; u* M% Z% J9 q/ @0 r - Sensor_Data[k] = 0x00;
0 Z" a I W8 |5 ~8 |- j0 X3 d, ] - * ~0 t) p/ m6 h
- for(uint8_t i=0;i<8;i++)
1 H/ ?7 Q8 \% ^( o6 f" H; H9 p - {
3 K( D4 t0 N, n. Z2 c - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level7 u$ M' w* d6 M0 \* } U4 X, T
- HIGH_level_read_time = micros();+ ?5 _8 L$ }5 ]* q
- LOW_level_read_time = micros() - LOW_level_read_time;
1 |4 M- v% w2 E! Q4 H! ~0 c5 X" A& i
: Z1 z( H8 c9 D5 s" P- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )( E1 {9 y f! ]2 T1 h
- {6 t; G/ \* }# [$ W3 D7 v
- return 0xff;+ w7 ] c6 a# E" f7 I
- }/ M* q9 Y0 A: s4 v+ Z
- ) `9 X$ C& T: D. |7 Y! f1 k) |
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 t7 W% }. a; n: W
- LOW_level_read_time = micros();
# R2 }) g' R8 |% y% V6 Y3 M - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
+ M2 r( b1 o' p Z/ X
4 |* i: h4 |: f9 i3 o( M+ V- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
! m; Y: n" M# e9 b. e* W - {
. w* {+ {8 H0 n( g1 `% d( Y - Sensor_Data[k] |= (0x80 >> i);* a2 m; j! `2 I, N+ s
- }
$ y3 V' d0 D+ H- ~ - else if(HIGH_level_read_time >= 100)
6 g+ c3 l- x" X9 L8 q7 Z - {
) i) v4 s" q7 N, ], Z/ }4 m7 {+ Q - return 0xff;
9 A- G* C0 a1 q: G" h - }
& Y S: N% @$ o( t4 u
6 L7 S7 V9 t/ k( ]6 q( {+ l8 v- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
W% j3 ^' k4 M: U+ t* n - {
' K5 k+ C0 ^9 }: {8 D, E7 \ R - return 0xff;
3 k$ s2 \# b: `6 Q - }
$ U0 k4 n- ?; c# T$ X% z1 x8 | - }' j0 b) Q7 w( ?( H. k2 }
- }
# l k, E& t8 D/ g - 9 M* O; l5 V9 |$ e3 x1 j
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
z# Z: m: K0 a( q" z: X# h5 u - HIGH_level_read_time = micros();6 ~, A' m) R$ {: c" _3 ^
- LOW_level_read_time = micros() - LOW_level_read_time;# G ~2 n( h2 Z% z: T& r* @
- - N( Z {' F6 j+ Z% s
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
2 [1 y, o; D+ D6 T# A& y - {' H) |0 ~% V1 Y. G v
- return 0xff;
7 o: B5 B! k' }+ d a0 `; x - }
2 Q( }# D H) L/ e5 r# m - 1 E$ T5 R0 a1 c+ c2 z' _# q& d
- pinMode(_DataPin, OUTPUT);5 ]/ ?9 v( G& H1 g h% }
- digitalWrite(_DataPin, HIGH);( t* F* F+ ~2 G
2 y4 R# K+ k) s. p, V; l7 r% \1 _- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))+ s( i- `9 ~; b3 E
- {/ c& ]% A. A9 `. a* q! a
- old_data = Sensor_Data[0];8 h0 o6 ^ ~- Y/ j& i
- return Sensor_Data[0];: R# I" b$ p% L' y# ]8 d0 K
- }0 Z1 f5 I0 j! e; ^
- else9 |( u' v Z# G; b% b
- {
3 R- E& e- z) y T! A/ F - return old_data;
- M. s5 V" m, ^" S7 u, a - }
, Z# O+ _4 E/ { - }
& z: ~* e! H# I# _: [
複製代碼
2 V' I/ y9 K0 n' \. `MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
5 a5 q* Q, Q8 H0 X' E2 j: b: G! y, @
|
|