|
4 R9 w: I3 N2 f( ?
- #include <Arduino.h>
: W$ O; R. h4 Q& |2 E/ \ - #include <MeAuriga.h>
8 Z* Q& c- p* E, y' T( A - #include "MeLineFollowerArray.h"
! n% J7 Z: x6 Y0 r+ ]6 q+ V2 K. U( n - : @; _* O) w0 L
- MeEncoderOnBoard Encoder_1(SLOT1);
8 h. u" k; T8 o2 t* P - MeEncoderOnBoard Encoder_2(SLOT2);
5 P4 C4 j9 E# \) ]* _ - MeLightSensor lightsensor_1(12);& {( G* }' T$ l5 I p4 s
- MeLightSensor lightsensor_2(11);0 A2 ]6 q0 r! \% m
- MeBuzzer buzzer;3 @9 a; q- \3 G% I& \) d
- MeLineFollowerArray linefollower(PORT_6);) _8 z1 b4 u* z2 C# h' x
- ' G1 [3 d4 S* ^9 I
- #define Error1 1
0 A) h2 n% }: e& Z( U - #define Error2 2
) _ p) Q. q6 F8 s- T - #define Error3 3# x7 e) e$ v$ ~4 h
, k3 ~& W( k7 O# f. p: {, D1 Q- #define Kp 15
. l; }% N* {* T- J Q - #define Ki 0.15
. m% r8 m4 h% T1 k+ U - #define Kd 0.034 e( J6 y+ E) I4 G" ?) C1 D( X+ z
; C+ ~( a2 L4 ]% e2 E' j. q- uint8_t sData;- x) ]2 l8 p9 t# C7 c7 A
- uint8_t D1;
) c) t6 v; p( R) q: B - uint8_t D2;9 X7 u' d0 d) t; b$ A- E# X6 h* p
- uint8_t D3;
/ v% [% ]& S% Z - uint8_t D4;
+ ]* G( U1 a' C, t# T - uint8_t D5;. B2 W8 j$ U; y* A q7 X4 e- ^
- uint8_t D6;
; k$ @4 @0 h6 I! Z* {2 j( t3 ^% h" l8 Z
5 l6 t" @" p! L* C, H' h9 ~+ T" z- float previous_error = 0;
0 [: f' r: \9 {( n - float integral = 0;
1 A7 Z# Y# {* t6 h; W3 P - float derivative = 0;$ z9 @8 I4 W/ D1 [0 T0 ^
- int Speed = 160;6 K2 E; i3 c+ m' Q
- float output;
. b$ @' r4 m+ o9 K7 d - : \8 } D' D+ o$ S9 A7 V# J) b& e
- byte Left;
4 n9 @& F0 ]8 _: W4 ~ - % z# ~' ~) Y& h! z, t* i V
- void setup() + ]% |0 [) n2 G+ N4 V A, _5 f) |
- {
9 I c. p# c( n8 J/ D - //Set PWM 8KHz- P" b- i( k' K- z! Z3 p
- TCCR1A = _BV(WGM10);
4 k; e9 V7 Z( G2 B! T! Z- y9 \ - TCCR1B = _BV(CS11) | _BV(WGM12);: P/ b! C& a4 X5 _. ^& T2 K( d
- TCCR2A = _BV(WGM21) | _BV(WGM20);
. W, q' h" j1 u# B0 K, O! ~! D - TCCR2B = _BV(CS21);
5 V% T3 O% o: R! V - Serial.begin(9600);9 v9 h1 N5 `3 H( B$ c& p/ P: T
- buzzer.setpin(45);, F! F& J6 z4 X0 ^/ X+ K! x/ n. E
- }
8 S6 \, O; e4 Y ^/ a+ z' _# n8 v: Y1 q - - ?, z" ^5 r/ J2 k# F% k7 P
- void loop()
O. d& a8 E4 T - {
& h- B4 A6 a* w4 H% q) S* L) L - while(!((lightsensor_1.read()) < (10)));, m7 d8 g" |. e; i( ~
- buzzer.tone(1047, 500);5 l* }6 G: T7 e4 @
- delay(500);+ l' _7 d( P* G3 S
- do
4 u% O; `8 U: C# C" | - {
; w: ^! m1 l& x: q6 E9 ~ - int Err = getErr();
+ K- L1 W/ l: |4 \. ^1 w - if(D1 == 1)* ?8 F. M& a0 ~) I! [" q% l4 E& C j7 B
- {
3 \0 K8 S8 E4 o: H5 T5 p - Left = 1;& T: G6 u- k/ I7 d$ r; w
- }" H9 }( p: `2 H4 y
- if(D6 == 1)
. ^6 {: C5 X' _ - {& O4 X1 |/ X& C# b ?5 K7 g
- Left = 0;
- x E' }* B S! i9 J7 F$ I - }/ a* {% c4 A: l9 Y+ r
- if(Err == 99)
9 @0 J' f4 M/ _ - {
0 u" \0 @6 Z$ ?6 J$ i5 s - if(Left == 1)
8 [7 k& n% _7 H$ T( O+ a; I- y - {
' F1 ]4 Z* y0 ~! z8 H# `" ` - Speed -= 5;
$ {) I" x4 G5 i" y. \: c - moto(0,Speed);# C& a8 |! s( p$ i) n
- do
" O' c5 a- \' x! U - {8 B' C( c' ]! {9 G
- Err = getErr();
. D* {0 [5 H) B1 D4 T - }while((D1+D6) == 0);- S, ^: g& Z1 X$ S" a+ C/ @4 ~
- }, W* {2 o' n1 g" ^9 E: g' B1 e
- else
/ U% |% X' G, M/ H* ~4 D - {
5 ^7 R) X# Q" H0 I - Speed -= 5;+ ~: l$ [- c5 I4 Q) l
- moto(Speed,0);
4 U$ I8 }3 f! V8 N - do9 \6 A' \" L- N4 K& q
- {( t. `8 b$ N8 G4 c# l, ^
- Err = getErr();9 j- K/ y$ W" \% W; {7 M2 V; B& g
- }while((D1+D6) == 0);
1 _! [: @6 `- _/ ^. B" j' C - }* `3 i! l& K4 j, N. w; S
- }
' o# H. ]# V* h6 I, R - else
8 l+ p6 C9 q# E% X) f - {
6 o+ f6 v$ h! l& O- h5 g X5 B - if((Speed < 160) & (Err < 2)) Speed+=1; : ?+ P L' H# L
- if((Speed > 100) & (Err > 2)) Speed-=2;
0 b$ D5 ~# P' z E+ q - integral = integral + Err;
$ n5 M7 ^. V2 f5 T. k4 `$ U! O& P - derivative = Err - previous_error;7 ~6 I, W5 m" B. o2 u
- output = Kp*Err + Ki*integral + Kd*derivative;
5 f$ N+ ]! p: f - moto(int(Speed-output),int(Speed+output));' J5 b# @3 `. I2 G" o- d2 g
- previous_error = Err;' e2 \) {3 H3 }! r# G7 o
- }
/ @: s3 `2 R. { z' w J6 M - }while(!((lightsensor_2.read()) < (10)));
' Z3 {' w4 S( g, v5 y* f$ E - moto(0,0);
, ?- W9 `* K+ J6 {; }7 M( m - delay(500);
8 c6 ^: O7 v9 V, { - buzzer.tone(262, 500);% y, v' G+ O6 g$ w3 @* B7 G
- } S5 C e6 F" w5 \! r% P
- . `/ P0 o: Z7 q# I% p+ p! E
- int getErr()7 y( i! q& ^& E5 }" Q1 i
- {
- _8 u# k3 _" P0 M7 h4 B# ~/ O+ J - sData = linefollower.getValue();
. H+ `' ~* [8 C1 K2 C/ Q# ? - D1 = ~(sData>>0)&1;
* [0 Q5 m1 a' c% u) Y - D2 = ~(sData>>1)&1;
) z$ ~/ g: r8 ^4 n* j - D3 = ~(sData>>2)&1;
( e6 W, L' k& F" F: j' ` - D4 = ~(sData>>3)&1;
" E5 @" Q& {! k+ A8 t5 O0 b2 _- _ - D5 = ~(sData>>4)&1;
1 m, L. ?8 J6 s' F: @4 E9 Q# S. j7 i# n - D6 = ~(sData>>5)&1;4 o+ K: [( T- A/ [/ W( ~* o% i
- int downD = D1+D2+D3+D4+D5+D6;) X' D6 p: h) X5 [* ^# d. j8 u
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
1 U: L0 \; b( s' f) s - if(downD == 0)) F1 `5 f% ]* x a; I: l$ A( d+ V
- {4 K d$ z7 ~/ A: s1 d2 |7 r
- return 99;
& x% }6 T1 D; u - }+ }% O. R4 a4 m
- else
) [! Y7 Z9 `8 `: K0 q& c - {
! M. \- }8 y2 s, M o: S - return upD/downD;) {1 I" S2 [, ^" w1 w0 ]* } F" E
- }& N! G% ^& a7 E$ ?, g1 Z
- }4 f2 `) W! h# N$ t- q) S
- 0 g3 M+ K% R6 q: V
- void moto(int Speed_L,int Speed_R) Q: T3 c! _9 n6 K% g/ O
- {% ~+ g8 s8 V+ T9 V7 Y+ d
- Encoder_2.setMotorPwm(Speed_L);# i$ S+ ]2 J2 _& |+ {
- Encoder_1.setMotorPwm(-Speed_R);- ?2 d3 Q! e- ~+ z
- }
複製代碼
5 C+ z0 l% q4 I {$ H6 p" RMeLineFollowerArray.cpp* L1 R) ~+ e5 i ]) S p6 E! k& s
- #include "MeLineFollowerArray.h"
' [. a0 ~/ p) V+ p) q
t3 `" B3 ?: f9 A- #ifdef ME_PORT_DEFINED
- X* E0 [- m3 h; t1 Y - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)0 e. M7 H; P1 U5 S; C1 N6 V1 X* O
- {
* v: C: V# i* \8 c3 i
. r* _9 Z/ q8 L/ Z! F1 j$ d- }
: l, v& f0 k& W- ]0 z) E" G8 E0 } - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
* g9 y. z4 X# j h) r - {
% i2 ?4 s* I+ B4 T9 ^$ j: w2 h - _DataPin = mePort[port].s2;
! s, V- \* f4 R1 n# ? - pinMode(_DataPin, OUTPUT); V& L* t& c4 l
- digitalWrite(_DataPin, HIGH);
2 ~ \2 E; C- l# t- m& J - }
5 k4 H& o9 p7 L" h - #else // ME_PORT_DEFINED
5 N6 j0 J/ N7 Q R - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin), R$ L8 r, \5 N# o
- {- o2 k0 }3 x. o; v l" g8 {# R
- _DataPin = pin;1 d7 A: B w7 p: ]7 v
- pinMode(_DataPin, OUTPUT); ) K+ b5 d! t) [0 a% z/ R
- digitalWrite(_DataPin, HIGH);, r+ z; D' J1 N: |. ?
- }
' P0 a7 S$ g& g' {0 w' ~ - #endif // ME_PORT_DEFINED6 M+ v7 o; r6 ]; c; w/ n( W0 p; w% Z/ N
( A g. B& [9 {1 j1 T7 K I; f$ f- ; p8 _7 C8 b; F& ^
- void MeLineFollowerArray::setpin(uint8_t pin)0 [, ]/ w5 a/ H" t* ^! w: C% n
- {- y8 m) t0 }/ I5 G) V
- _DataPin = pin;' j! ]. u& T$ c% J$ v2 G: @$ n- V
- pinMode(_DataPin, OUTPUT);
# K0 ~% p4 x/ k0 l! g6 [ - digitalWrite(_DataPin, HIGH);4 d% k, u6 G# {6 g
8 |5 O) W( |5 ], X- #ifdef ME_PORT_DEFINED6 z3 O; u( P/ b) b& _3 b2 n5 P3 b
- s2 = pin;9 l, X, g2 b* `! i5 q' h
- #endif6 D1 G/ e5 t" f9 [
- }% j" N G0 ^" t9 G3 n9 j2 V7 B
- % y( x( z6 o7 A+ c; \
- uint8_t MeLineFollowerArray::getValue(). a8 R. h$ e/ c9 r3 `7 f8 S
- {
) e+ o9 B- b. u* w$ L3 L; G5 l - uint32_t LOW_level_read_time;
; }( e: l0 G' F* g5 ` - uint32_t HIGH_level_read_time;! g& Z! q# X' v$ l) H
- uint32_t time_out_flag;
@5 v3 k( T# P- Z- H: [ - uint8_t Sensor_Data[3];
* l; X h8 R/ E ~! c5 u. F - static uint8_t old_data = 0xff;8 U7 I6 e5 L& @
0 `( b) r+ k! {+ L3 D- pinMode(_DataPin, OUTPUT);
, z( y& s2 y& j; G% W' }+ T9 E - digitalWrite(_DataPin, LOW);
6 u# ~1 O5 R( @2 L* @8 w1 p - delayMicroseconds(980);
. V; U6 u5 [; p2 u- u) V4 H. Q0 R - digitalWrite(_DataPin, HIGH);+ B# y w& V% G# n0 p% |
- " ~7 S# L$ p$ \( D
- pinMode(_DataPin, INPUT_PULLUP);* Z+ E; d) Y1 [+ N) w
- delayMicroseconds(10);' P9 e6 u* W2 ^' n8 j8 _' a
- % y0 D7 m$ X K
- time_out_flag = millis();# k: {7 @) U& e- S) ?8 n
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); n% t+ C; Y& H' p; ?4 N- i6 C
- ' Q0 Z9 O( y1 g0 b; |; p
- LOW_level_read_time = micros(); X% l9 i/ i! t: O- K
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 v& g" T4 T; m
- {: {" ` [! r; e" f3 L: [' N2 v
- return 0xff;, D3 p& l; F3 m6 R$ u6 |7 F) _
- }
* w7 f# g' C5 P - # E8 z0 Y% c3 s2 d
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( w) {; K2 w; g6 T8 V
3 L e% w/ l9 o: i- HIGH_level_read_time = micros();
+ ^$ |+ H8 O1 _! D9 v( a& T - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level( p. C: }, c% E* z7 e [4 k
, K6 Y% J; C7 @: e0 j9 ?- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. ~: Y6 D( C' J7 ^& P3 H: p; ?1 A - {
# ?& R7 a# Q7 q - return 0xff;8 q1 y1 u1 N$ O
- }' B) H8 u/ \1 _3 H+ D) f
- ' Z) m4 k( N9 ^1 }6 n( O# S
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
, n0 y5 e5 R) w' n* Z8 g* G - {
* o! J. h; F: S+ ]8 E7 O' _ - return 0xff;* Z: I& Z/ d: ?/ \ [
- }
2 y+ W$ V& G& ^1 P9 }6 W
0 a M0 L y! p( ~! z# v3 y5 B- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
; g9 g+ w% Y# R7 l - LOW_level_read_time = micros();$ D4 M, n6 X" f* x# I7 K3 x7 f3 R
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
* ~" b2 u9 v' I# x
# M* u; b6 H; S+ i- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 x, M- H; V/ R1 s3 I5 t
- {
# C- U7 C5 k' D( {) C - return 0xff;
/ v; A; I7 N% j6 O - }
4 W! v u4 w- N5 d
6 M. g, `/ ?0 G4 u: r- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
4 O% {* F2 |' o* f) W6 Q - {
& \5 V, k% i2 n# N- _4 x - return 0xff;3 N2 k" A/ ?( \
- }
* y4 q6 @2 r2 g9 e - - O3 W5 h: A% D' c6 p
- for(uint8_t k=0; k<3; k++)# ^7 x5 U# w) k. l
- {0 {) h) d/ V, d- k, h5 G3 t; U
- Sensor_Data[k] = 0x00;5 [- b! r8 w+ [ m( T+ R+ [9 @! R
- ' D) y# |/ A* d" K3 Y- j9 u
- for(uint8_t i=0;i<8;i++)/ f' N$ x3 g1 y0 }. z
- {% x; \9 n! c- Z4 q& g% Y9 b
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level1 _* [! Z2 q z* q6 y
- HIGH_level_read_time = micros();- F d1 |6 c! q [0 H
- LOW_level_read_time = micros() - LOW_level_read_time;' [/ ^% k* q! {0 Y3 F
- , q! e# y1 h! s. W# e
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! v5 m1 K$ b$ m0 [! Y
- {0 @1 G$ K; V9 I# M% m3 h
- return 0xff;- C# y* r( o* _7 D6 N; Y
- }
- B, r' C# Z' Z6 l w, ` - + {$ \% A- s2 o1 C* b8 o& H
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* L( e% i, a; g: s2 Y! U. J. L
- LOW_level_read_time = micros();
. @ O) N: X0 E( F3 y- o - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, p; }( j& f4 W I& R7 H
" ~$ u, J7 H# h) T3 _5 r+ c R) O- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
y3 I; P0 }8 {6 q - {: j5 ] y$ r/ u! v I$ ^/ J8 }, r
- Sensor_Data[k] |= (0x80 >> i);
3 x" k+ e4 c3 A) s- }! d$ A1 C/ ~ - }
& [4 } o+ j" N1 u H3 E - else if(HIGH_level_read_time >= 100)
$ ~0 Z" p, D' e+ g C, c - {
1 G6 H$ L2 j9 i" X - return 0xff;3 Y( O# X/ }8 l% i6 ]$ Q
- }% Q6 }+ v7 r8 r1 H8 ~
/ f) ?- ~) [0 M! X. C- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
3 b5 u8 p4 o: p5 X) u) A1 W/ b6 Q - {1 r* S1 e; m& y/ i
- return 0xff;! O: Z4 R2 p s% ?# k0 A2 x- m
- }/ y% [$ P% l$ C% | w. G
- }
# w3 z7 I2 M d7 \1 Z! n$ { - }
5 P! s7 V7 a' P. j+ b& F
( o9 C/ k5 H! {, R$ H- U8 c2 ]+ O- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level c& K/ \" e) ^- j. t
- HIGH_level_read_time = micros();
: s2 T ?% ~/ o - LOW_level_read_time = micros() - LOW_level_read_time;
( w* Q1 [8 J; Y, Z/ f6 D! E - 1 P1 q4 E) `) g
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
+ l6 S# O8 j/ Z6 t G* n# q - {. ]6 U) \# @+ z8 k
- return 0xff;
! r D; \6 |2 R- v- S; i - }! G5 |: \% A" q8 V
8 U' A2 g- \ A w3 I5 a" i0 n- pinMode(_DataPin, OUTPUT);
' b% L/ ^$ |/ [2 I1 l! U - digitalWrite(_DataPin, HIGH);
) t5 t1 o* O& _( K+ N - 5 ^! y* _$ S- e9 y4 Q" K
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ N5 b" y# X+ H+ f - {/ }$ A: K- C" V) }. I
- old_data = Sensor_Data[0];
% C# A6 w( k/ n. o0 j6 y1 d( M - return Sensor_Data[0];4 f7 W$ K/ ~7 U% r' F
- }
0 ]7 O9 m. C0 B6 S2 D% X - else5 ~1 d( i$ C& s! i
- {. |, z' H6 _. }) P4 [$ ^
- return old_data;/ A& t. G0 S% U7 _; _
- }* f" }) a0 M6 X
- } {( p/ d) a! L; c6 A2 t
複製代碼 9 q, c& p: V. q! [( N
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
% l( Q- K8 W" A1 f* \. o% }
: S9 d% b9 ]6 V# r* F7 @/ A( u |
|