|
|
3 D- L) C8 p4 d9 P$ y0 y
- #include <Arduino.h>, g2 j6 C7 g* V3 ?+ X! K: h E$ I
- #include <MeAuriga.h>5 I+ a$ y- N- L
- #include "MeLineFollowerArray.h"
' J. P! O7 b/ s! j8 j9 M1 h7 F( p4 o& v - - C5 O% m5 C, f/ n$ }8 X8 V
- MeEncoderOnBoard Encoder_1(SLOT1);' o) t$ m) K3 w6 }" ^7 M2 @2 T
- MeEncoderOnBoard Encoder_2(SLOT2);
, Q2 Z2 R5 j7 C% t4 |) Y - MeLightSensor lightsensor_1(12);6 F0 a3 P( P' @9 t; _ ?- B
- MeLightSensor lightsensor_2(11);
! \( @9 _ U/ \ - MeBuzzer buzzer;
2 L& \2 F: @5 z h1 u1 E% S L - MeLineFollowerArray linefollower(PORT_6);9 E; p! T9 z3 [( k" [- h2 _9 I. O
. C& U/ N$ S. X# Z- #define Error1 1- Y1 b- l2 s* G% c7 Y2 P
- #define Error2 2
3 r" ^2 N; ]+ S' C1 B - #define Error3 3% @! ?& n6 S+ y1 l8 b u/ X8 y
- # D4 r; }5 S/ {' V) h
- #define Kp 15
$ }+ O/ q3 J$ a; P' O - #define Ki 0.15& E* {! \3 S0 v/ _; _7 ]
- #define Kd 0.03+ e! |5 N. |* \6 {5 H% z
8 R. q2 C: n+ B7 h( G* H* {+ Z- uint8_t sData;( _, h1 Q, ~ Z$ q. u. S9 j2 b3 U
- uint8_t D1;. Z" D+ D' o' ^" ^+ [* A
- uint8_t D2;
3 D, E0 {5 v" ?$ b9 H+ n0 |/ s - uint8_t D3;: F; A! E0 P# R# d( s
- uint8_t D4;* U# L4 @* m H
- uint8_t D5;! G7 u$ J9 Q2 t2 x; I
- uint8_t D6;3 L/ R8 S6 \% w+ P
8 _9 h# L3 Q* G. P- float previous_error = 0;" p) U% w" s( l5 [! `5 p# Y
- float integral = 0;
! H/ G+ N- l; z; h - float derivative = 0;' e! Z7 o- u0 C4 ~# b' m( ?+ T% Q
- int Speed = 160;2 d! h* z; o! y Y# K {; K
- float output;
$ q7 _& G9 m& f& U
8 i+ O z* t2 w% Q/ R" ~$ A- byte Left;* f( X8 o7 Y* G: y
: [! P5 L4 d8 U: A- void setup() 7 a% X5 y5 i7 J1 X( g5 o) D2 g
- {7 l, S( j% c5 C, o* N4 G2 ~8 ]
- //Set PWM 8KHz8 @. p% s; [' F
- TCCR1A = _BV(WGM10);, \6 s' m& L. m5 o( V
- TCCR1B = _BV(CS11) | _BV(WGM12);
3 a z" s1 _- F4 N6 E3 M* X- d - TCCR2A = _BV(WGM21) | _BV(WGM20);1 E7 X% w6 j1 o, {" e% i M E) r
- TCCR2B = _BV(CS21);
7 M0 ]2 b2 J0 M7 o: q6 w; S% n* \( c/ J - Serial.begin(9600);; i( h: z( k1 V+ g
- buzzer.setpin(45);
/ G6 t' C% b: b( f# c2 z - }* E" L% E9 p, D) r$ k
- 6 n4 B6 w" l6 ?1 j; d
- void loop()
0 y1 V- E& h* G( n - {
- i2 h) ]* O1 d9 w' F3 t z - while(!((lightsensor_1.read()) < (10)));9 ^+ D4 C+ A! c9 `8 i8 |' y9 l
- buzzer.tone(1047, 500);/ G/ p8 s* b; Q. w" {4 M; o
- delay(500);) D9 m; q" q, X# M0 ?
- do7 t5 P; j5 A" C0 v% Y. H
- {
8 s; Q' `. m: G - int Err = getErr();
; T/ [, H+ K7 i- ` - if(D1 == 1)1 \7 R5 R3 L9 H6 M& n
- {
$ f4 i: B8 Z& T8 z; m) p - Left = 1;
! a1 G$ ? @3 W+ ^$ |, b8 U7 C - }
: t3 z4 C$ f; a. R7 a - if(D6 == 1)5 p4 U' a( ~2 u8 n* p# u
- {& S* ^7 f& X- d) }! o' e# N
- Left = 0;! X8 I0 z* A$ @4 S
- }( c. Z" d o' y' Z6 c
- if(Err == 99) T" k) O o4 S8 t- @( }+ N8 |$ z: n
- {
4 h3 }1 L. Q# `' _- S - if(Left == 1)
: V9 P: l& {( a) ?- f% N m - {
$ _! C, p& T5 l L% \6 r8 W - Speed -= 5;& m9 _" E! g* t4 |
- moto(0,Speed);8 F T1 V7 t2 ?
- do
1 _2 x$ T& K7 m6 t0 k9 ~! S - {
2 _ A5 I4 B9 K$ S! [' T- H" g/ ~1 } - Err = getErr();
7 C0 [& R6 L* A - }while((D1+D6) == 0);
1 p$ Q: @0 } J g; B - }/ _, l9 {/ D% a* D
- else* h' l0 o- v0 b8 Z+ {' h+ D
- {
4 {- [+ _3 E4 m9 g9 c; k - Speed -= 5;, w$ V' b! _& r% d
- moto(Speed,0); 7 B0 P% i3 K4 Z! b* ~
- do
. H: M& o, l( C8 s0 {' B+ Z4 ^ - {( y. T9 Y7 }5 W$ w3 ?( N
- Err = getErr();
$ _3 U; _9 i' f% w - }while((D1+D6) == 0);
. z2 y3 i L+ S% T - }
$ U6 A9 f' @' v+ l& v( d - }
6 x8 G% S, [7 @- |/ s - else
- ]. g$ y/ l& W - {
# S2 R7 j8 A. s! I( ] - if((Speed < 160) & (Err < 2)) Speed+=1; ; d5 n! ]' u% Q( u
- if((Speed > 100) & (Err > 2)) Speed-=2;
' ?' c* V1 ~1 v- `+ r+ J7 {3 e$ e - integral = integral + Err;
" m9 q5 p. P0 a |( K, Z7 I) h - derivative = Err - previous_error;1 W' L; L9 t4 X
- output = Kp*Err + Ki*integral + Kd*derivative; W* F, [2 v6 I
- moto(int(Speed-output),int(Speed+output));' Y: E q+ t( M9 B( W5 H
- previous_error = Err;
0 M) j0 }$ R% o* q: g. |% H# t& g - }
) A8 e; r8 `; r0 U1 V& L) w - }while(!((lightsensor_2.read()) < (10)));8 {! x g& w8 `+ i3 z. R) b$ r
- moto(0,0);
. @$ B8 Y) @5 v3 b+ G% i/ ] g - delay(500);
; L. [' x5 C$ K& c# X - buzzer.tone(262, 500);6 Q( y* `9 J! n/ E/ d& _
- }
2 Z! ~: r1 k" o2 C) d/ V/ R8 j
5 ?) O% ?; M- {5 h$ H- int getErr()
w, _9 T* y' a/ `- k5 a1 W+ h - {
& \8 g. q M, ? - sData = linefollower.getValue();# H5 W6 E* L! Z. {4 K* D
- D1 = ~(sData>>0)&1;" y, n" G2 R/ z6 o+ [" V- Q
- D2 = ~(sData>>1)&1;
; u) e6 F% n# G# u6 N - D3 = ~(sData>>2)&1;8 @% j( q3 W. T' [/ n" f- k
- D4 = ~(sData>>3)&1;
( p# R( f! v j" V - D5 = ~(sData>>4)&1;
3 y# k2 G1 S1 C" u! L, I/ N( Q - D6 = ~(sData>>5)&1;
& D4 u5 g# E# Z! i - int downD = D1+D2+D3+D4+D5+D6;. g! m* ?- n+ k3 ^% u' M; h
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
) f) i" y6 y, F6 }- b7 S - if(downD == 0)
; ~% I# o" U z B, h6 \- v - {
% J) e1 @1 n( t) J+ `0 Z8 D - return 99;) F9 w6 R/ M3 W \) V; M
- }- T: W4 P9 C1 k3 ~- V
- else
7 J& d/ U2 z, ]/ J# p: [0 R. Y6 P - {9 J& c! [1 H+ b# O, l: ^
- return upD/downD;4 r9 l! S% S# B
- }
) M4 j# J0 n# W) Z) v3 a+ I# C6 | - }, G1 Z" v) ^# _5 q: s5 _
5 J0 v k# u9 `/ \- void moto(int Speed_L,int Speed_R)
P W, A& x4 K$ e" g& k - {/ ]/ S: ~# w9 N* P0 h
- Encoder_2.setMotorPwm(Speed_L);
( q3 m) Y+ o# S5 r- Z6 r - Encoder_1.setMotorPwm(-Speed_R);. E0 }! V$ Q* g4 D4 `
- }
複製代碼
1 Q0 c& D8 l- x0 _MeLineFollowerArray.cpp
* T' r. H5 p- ~9 U# c- #include "MeLineFollowerArray.h"
% C; V( g. ^' G, B0 q4 L% s - " ?! v' {+ A# Y9 o* A+ m Y! e
- #ifdef ME_PORT_DEFINED9 l, G0 V) s2 | n9 z. ]
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)4 L2 i7 Q; \5 y7 W4 L
- {% T. l* V& R) y1 [1 e/ o) W
- # ~" g' J2 a4 t+ Z8 ?/ g
- }& s% l1 i c) I) W2 e, {; b6 U
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)( X0 t$ U: `- F4 J5 y( n# c
- {
F* Y/ L: O5 X9 ? ^1 Z0 n; M - _DataPin = mePort[port].s2;
4 J1 n: H; j& Y3 S8 H: O z3 ` - pinMode(_DataPin, OUTPUT);
( k8 R+ m' U2 ~ - digitalWrite(_DataPin, HIGH);
& Z9 F& C5 T, R) m8 g - }5 v, Q5 J% M- t5 a, _) V
- #else // ME_PORT_DEFINED5 w; N1 q6 E1 x! K2 [
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 O+ o( X# z: N# O
- {
' b" x$ F' @4 k( l. S% G! @+ R+ x. Z - _DataPin = pin;4 X" l, O: x" y2 z. y) I
- pinMode(_DataPin, OUTPUT); 7 P$ X* h' T- |+ K0 K
- digitalWrite(_DataPin, HIGH);
- [+ M: t- H* f/ z3 M5 H' s/ F - }
" i; f1 _" |2 Z" \2 K - #endif // ME_PORT_DEFINED3 C y" a t' v
& f- ]+ B' I) V/ U- % F, J; M0 P9 d; W
- void MeLineFollowerArray::setpin(uint8_t pin)
+ F9 e" v4 n: `0 T; ?2 ? - {4 o' |5 B! }% {# N# G: F; O
- _DataPin = pin;0 G \2 h6 Y2 c. z2 J
- pinMode(_DataPin, OUTPUT);
# c. a6 z; w) a1 Q - digitalWrite(_DataPin, HIGH);$ \; ]5 ^8 w8 A1 ]8 e
- ' j5 v) A- G0 F$ O. p) i5 w X9 @# h
- #ifdef ME_PORT_DEFINED7 a. e$ |; g+ G
- s2 = pin;$ X* g/ _6 c' G2 M6 y# a2 O: Z
- #endif0 K4 M; m8 ?3 k: M# |. [0 O1 K
- }% S' e! S3 C7 _* H0 X# [7 r. C
6 q9 j- j4 `; T- uint8_t MeLineFollowerArray::getValue()
5 c* u. i1 Q( P3 b* ^1 m+ ~: K( d - {$ v, u7 N v {( {- h9 z+ m4 K8 Q
- uint32_t LOW_level_read_time;4 S# g* D6 t% P- i3 ^; s& h
- uint32_t HIGH_level_read_time;
. j& Y; V4 K5 P0 U# b - uint32_t time_out_flag;
$ s' j: [ W/ E, Q7 C - uint8_t Sensor_Data[3];2 v3 j) X: I# o; {6 ~6 G
- static uint8_t old_data = 0xff;
! A3 t+ A( E7 m
' _! ^0 S7 L4 O' i- pinMode(_DataPin, OUTPUT);; u: E; C; |9 \
- digitalWrite(_DataPin, LOW);6 l% q+ Z+ F4 |. y. i& B% @; T
- delayMicroseconds(980);, a! I. v* O6 @& u. \
- digitalWrite(_DataPin, HIGH); T$ f/ m% N* b& x& X l
- P, Y! z. C/ I- pinMode(_DataPin, INPUT_PULLUP);
, s5 v6 x. u; ^: ]2 ~ - delayMicroseconds(10);
4 d6 D; B+ S% c% E3 @3 ?( R) H
* P! A5 @9 i. z' S& l- }, ]5 R- time_out_flag = millis();" M+ d/ D7 `% F! k& c( J9 p+ m
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ {: |: h3 y. s3 @
+ r m0 h8 Z+ n3 g$ z! N- LOW_level_read_time = micros();! A& l: n2 J5 a9 |& {8 D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; U, R# B* s) d - {
# T$ f' w0 R$ H - return 0xff;
7 \% ?) N9 u. I# S/ f% Q: Z - }
+ l+ Q3 x5 B( k+ i3 [8 F - ! a8 @3 J. w: z# s$ h" {
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- P. |7 p: Q J3 l" R' T
6 L, H. p- Q4 U. E9 F& V0 Y- HIGH_level_read_time = micros();
, W. A5 {& ?1 k9 O1 j! G# n - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level& m6 [$ ` V, g( }7 v1 d( S- z O2 l
- " p F* a" y" `& o9 C' i
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
0 ?: T5 I" b" S* j! P - {: Z+ I% E y& x4 z+ U8 O+ j6 @
- return 0xff;
+ \/ g5 B# H+ g. N# f. Z4 K - }
; U$ m# d8 g- ^9 a! l7 a! U - & Y3 F) v, a+ N
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))3 w* H5 f" C3 X4 C- b7 z* Q
- {
, K. F, j3 J5 y - return 0xff;
/ S) X+ ?5 m5 c1 G - }
- X2 `' f! C' i1 s1 o4 x5 D
3 R* `5 @! Y+ V( L% [7 v- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );3 X# }" B, _, P& N- F1 k( k
- LOW_level_read_time = micros();
# f8 L+ A1 d- H$ ]" a& P - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
m% }2 S r, _& E, L0 L0 _. {% @ - % ^$ |9 b0 H5 t7 ^; S
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 q+ }: Y3 @! l" X- s4 b
- {& ~- o# D" ~3 C; c, `
- return 0xff;1 n2 ?8 ~, o% p: v; o. }
- }
+ R0 X: {% q/ Q
1 e' \7 |1 }3 B: t; n* K( c& e- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
# V8 X+ r6 {; C' y' J+ f6 J - {: ]* e- y& ?4 r q9 ^8 ]
- return 0xff;/ z; O. @ a' m- a0 i% q; C
- } Z$ [( R$ y& ~6 g# C+ v
- + `! M0 ^3 T6 L9 J
- for(uint8_t k=0; k<3; k++)6 v9 J6 [2 Y6 p/ s
- {
1 ^( z( c; c8 ^( I4 x - Sensor_Data[k] = 0x00;
& r3 n: U) J, c6 x; d, t - 8 I1 ?( A1 r" N3 [8 `6 z
- for(uint8_t i=0;i<8;i++). M+ I& ]7 K: Y; C7 H0 ^
- {5 ?& |: U5 q7 K* P) d8 b
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& [4 B5 I) M$ ]% z! m - HIGH_level_read_time = micros();
) F/ M' r& F3 F - LOW_level_read_time = micros() - LOW_level_read_time;
( L, A+ V/ L6 @9 v' U
# l% K" _8 k4 b- p% w- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )% `7 ]7 s, c @0 k
- {/ d$ K$ X9 }4 w& S& V
- return 0xff;$ e' S4 R, ~! s0 A! V3 r( C
- }" d/ B ~# {7 y4 @
! c# d! ^* K* V* H* s- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
3 p( D7 F: Y/ ^5 Q - LOW_level_read_time = micros();" o! h) s' V; R m
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level2 Q7 t; g: R! h
- ( K7 L8 \( M3 |) `8 `
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
; q0 z8 _% B9 y3 B, b1 O8 z - {# f o# d* O/ s
- Sensor_Data[k] |= (0x80 >> i);# E; H+ [% L0 X9 V7 }( M
- }! h0 u" O) Q; m+ O% e& A3 P4 `0 y
- else if(HIGH_level_read_time >= 100)
& L4 y; `, [, i& b - {
: O! l1 L8 A( p0 F - return 0xff;
* I6 W/ Q0 M9 r1 ^! c6 r% g' i8 j - }- ]' `% I* M3 X0 _; i
- 6 F6 V$ J5 e v' b2 J( l
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)( d$ N$ ~3 q8 V
- {3 d' B5 p9 L# b& d0 L' M3 R
- return 0xff;! u( d3 J8 j( p1 }
- }
- \: B6 N! O+ _4 ^8 m - } s6 S9 g4 p4 \* f' {
- }! @0 R. w) S8 \1 P1 o
- % V2 h6 B! K* R1 X/ k7 w. Y6 S
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level2 b9 t! U& W: z
- HIGH_level_read_time = micros();
3 z6 B+ F8 {' w - LOW_level_read_time = micros() - LOW_level_read_time;
$ i; |" r2 I E; |8 e
6 k. e0 \9 y( n* S) d. j- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
: Q6 j2 i' Y* i. g, J: |3 Q - {4 D! i3 A1 G+ c7 G% f5 Z2 W; ?4 [
- return 0xff;! c6 N' }) f. q' A! _/ X9 o
- }. q& I% u1 I% i5 k( ] i
5 j/ D+ q9 }6 f- h8 |1 O- pinMode(_DataPin, OUTPUT);: x$ {/ i' M+ ~5 \4 F
- digitalWrite(_DataPin, HIGH);
+ v* d& _" l( N# E e - . Y) }, V6 y, S! N
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))): w" C; `! B) I
- {# u, c9 G% L$ x$ b w2 M i8 t
- old_data = Sensor_Data[0];
+ P3 c2 ~* C& n0 Q - return Sensor_Data[0];
+ _0 h, f9 r6 D5 b - }
9 O$ z; |+ A q/ B; {9 d, F3 [ - else! P. v9 f. K$ _) r
- {
; f: t7 M$ j z u' i - return old_data;
" f8 n4 j( z5 e4 M0 J - }: B, r* ^; i) n9 m, }4 D
- }
+ R4 y+ }1 Y7 P/ h1 l9 H+ _
複製代碼 7 s) k, `2 e, r! ^6 c. N
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
" T5 H% m$ i4 i& p
& U1 a3 `9 E) K7 T H7 c |
|