|
|
8 W/ g: {- G: x, X- ]+ i: z) ^- #include <Arduino.h>& c0 _9 T) O/ b) f4 r) ?
- #include <MeAuriga.h>' H" E# R$ q$ t% m* q
- #include "MeLineFollowerArray.h"' }6 Z' U4 x( }, h5 R4 f
- 8 {# `7 _4 v- ~& g
- MeEncoderOnBoard Encoder_1(SLOT1);
$ B$ b+ c1 H5 [. f - MeEncoderOnBoard Encoder_2(SLOT2);. R# P2 n! P0 O7 X3 I0 C
- MeLightSensor lightsensor_1(12);* ^' J+ v, z' l+ O5 u
- MeLightSensor lightsensor_2(11);
7 z$ O5 s5 u$ f. o - MeBuzzer buzzer;
0 a |6 [; ?" a0 I+ c - MeLineFollowerArray linefollower(PORT_6);- ]) X7 C& G. e- X1 b. p3 c0 p8 l! \
: ]2 f2 ]" q4 R R- #define Error1 10 r& `9 L0 s) J3 e7 U7 @* i! S
- #define Error2 2
2 z0 t! Y2 {# a4 I+ Q" ~ E7 t) | - #define Error3 3
% @* ]7 B5 k5 t9 u5 t9 i R
' L! I8 M+ U3 `8 z( N) k9 O1 x+ J- #define Kp 15) K- v" u G8 j) m
- #define Ki 0.15
- ?' E1 e7 p3 j* |) `: s" U4 l, | - #define Kd 0.03; E' ~- d+ j5 V6 L5 |
0 M. L4 R* y' h- s& L- uint8_t sData;: ~; j! h' p0 I5 E* G& v
- uint8_t D1;8 a, L# e1 J0 P: \' {+ n: X
- uint8_t D2;: A8 B( P) M+ P+ Z& Z2 @
- uint8_t D3;& \; ^; W# q& a0 [& K. @/ c) g& u
- uint8_t D4;$ y( Q7 e! B( H9 q, y! p
- uint8_t D5;
9 A$ U( q& h, ?6 G - uint8_t D6;& e; g C9 n8 D. S# [$ p" g# L# P
0 v* J1 W1 v1 g3 P1 a' Q- float previous_error = 0;7 L- S. i$ Y3 \7 ?
- float integral = 0;
/ z8 C$ h6 p6 O& s {$ g9 X - float derivative = 0;* I. k1 Y. G4 O+ z8 q
- int Speed = 160;1 V2 W: F. O+ W/ I" n' C
- float output;5 ?* |: ~" S5 E N4 t7 \/ k' ~) \
- C2 u7 u* e, f' T3 X
- byte Left;
0 z7 G+ `4 [5 y - - q# A- }- J2 ^; H( ?: I4 c3 X
- void setup() 2 P: y0 w8 e' |7 q4 X* L6 L$ U
- {
7 L1 _4 p) L- m) B - //Set PWM 8KHz
. x8 V( o g, x6 M, }5 [ - TCCR1A = _BV(WGM10);
~2 v% r) v* G' c( y0 o# n* Y8 V; i - TCCR1B = _BV(CS11) | _BV(WGM12);( Z1 l; E" U' ?: z
- TCCR2A = _BV(WGM21) | _BV(WGM20);
/ L7 m' B4 f% }, v - TCCR2B = _BV(CS21);* @3 N6 [: l/ @1 I. z3 s N
- Serial.begin(9600);; n: s! U& b+ N# W: o
- buzzer.setpin(45);- K% g7 f# b1 }& O
- }/ R/ Z5 a3 ?, l8 t
) {/ Y3 R+ P- m) c: L9 o# O- void loop()
) I [8 O: q, v% }/ M5 f, n x6 l/ r - {7 N9 U5 j k' d6 F- G
- while(!((lightsensor_1.read()) < (10)));
0 }: m% T i4 t: X5 t6 T - buzzer.tone(1047, 500);
) R1 _7 X/ s! l' R P - delay(500);
; E* P- t: s' W! z3 y! ? - do
- \7 w& l$ [$ J8 h* ~/ p0 z - {' Q8 O" d, Q6 g. [/ [, }1 m% M
- int Err = getErr();3 ~+ I& K- O/ |6 D. y
- if(D1 == 1). j$ q k* ]" V; Y' h P u+ }% I
- {
8 x2 j5 a; j6 I Y3 [. [, z+ I - Left = 1;
' }: f( a4 U' t9 s+ J' a/ G - }! D' U' u6 e7 \& f& E v1 }
- if(D6 == 1)* C- Y( e5 f0 j- x- H/ \
- {; S1 Y# r) H! u8 o. O: r y
- Left = 0;- u# Z# k: i7 U% r3 D
- }
9 O' {* U1 |+ r+ C( f - if(Err == 99)9 D: C7 k8 ?5 g [
- {9 N- a! j6 f+ e
- if(Left == 1)
$ r: o2 U& ^5 G- u8 P - {
: A1 |' b! x5 i$ t( i# y! D; c - Speed -= 5;
' `. ~0 d2 I: m# D; d9 `2 m1 g! T - moto(0,Speed);' N' _2 [. \- K: a
- do
$ }! H1 q3 ?) h0 a1 P+ `3 ? - {4 z$ T% R2 V/ N
- Err = getErr();7 ^+ w2 [: v$ k; Z$ q: r
- }while((D1+D6) == 0);
. ^' W* `* p3 h9 t, f! y - }/ p7 ] C# v' w8 K# [
- else
4 r0 B3 I* K8 D2 d - {
) B' q2 V& I( r6 `, q8 N - Speed -= 5;# f; ^- N# f2 f3 P2 T5 N
- moto(Speed,0); # J) \7 M4 n2 I% x' G/ p
- do/ x, u5 s N) F; C5 X
- {
- @% N& }$ @9 n' C0 g$ x& F - Err = getErr();. x& e+ u6 B. y5 o6 Q& X" n& ^
- }while((D1+D6) == 0);6 l2 {0 w4 }8 ?8 j& N6 l
- }
5 ^$ I: q; E1 X1 w - }* ]# `- _* v. [" a( x( }! X
- else
* c- X" n* _5 N" _- u$ Q4 H9 S r8 m - {! m; ~( L. ~3 T9 Z! h8 r
- if((Speed < 160) & (Err < 2)) Speed+=1; " N K( d$ d n$ t; ?
- if((Speed > 100) & (Err > 2)) Speed-=2;
( C/ |; R3 c/ C8 B - integral = integral + Err;
% [, D; G8 C' V - derivative = Err - previous_error;
. Q( h, l7 y, J- Q5 v8 A - output = Kp*Err + Ki*integral + Kd*derivative;
* ]( i( Z! w/ s- Y' [7 K3 X/ ` - moto(int(Speed-output),int(Speed+output));2 B( b& I X5 [
- previous_error = Err;
& Q( _! G! }& x/ o - }3 I9 ]2 E, C+ N% f$ L
- }while(!((lightsensor_2.read()) < (10)));
2 p0 {; i& {' o' D - moto(0,0);2 Q' [1 v" X% s$ y
- delay(500);
- n) Z& u/ k, `3 {" b9 i - buzzer.tone(262, 500);% Y* X p) m+ h- P
- }
! E7 r8 M4 S, U - 9 A! Q! C3 t6 P; e7 X
- int getErr()
: @! w7 Y3 d8 m4 r, c$ c- | - { 0 _) n/ { z" X1 X6 v4 N3 S
- sData = linefollower.getValue();3 U8 W$ N4 K4 v, @
- D1 = ~(sData>>0)&1;
+ r' a9 A7 ~2 N' s& Z - D2 = ~(sData>>1)&1;
' w8 e0 z# F2 C6 z6 p# [. \ - D3 = ~(sData>>2)&1;/ v/ O8 b$ p$ k9 X; V( L v a
- D4 = ~(sData>>3)&1;2 W- L& p( o6 K8 f5 ]+ w
- D5 = ~(sData>>4)&1;
) s- B) v! D, `5 P9 `: }6 m& g) n - D6 = ~(sData>>5)&1;
7 G- V- h$ k* i; [ - int downD = D1+D2+D3+D4+D5+D6;
6 T8 X- n& T# d" z# `# F - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);/ `: i2 a* s9 l: a7 a
- if(downD == 0). P! Z. u6 s+ U: w/ C
- {. K2 Z: ^$ N' j- e6 D
- return 99;
& L* z$ b6 T: \# D; Z - }* E" n+ z, P9 b1 V" J
- else
; L& R* n7 {/ t* ^0 ~: S - {# |1 T0 a+ p3 a2 F
- return upD/downD;9 u: ~) Z" V4 V" {0 g$ z3 ~
- }
- V: k! W- i& o t V - }8 _' a8 V- X1 @# `0 Q! W4 _
. ?; L4 y, O9 D' D2 Z2 H/ ?# {- void moto(int Speed_L,int Speed_R)
$ [4 L+ p g% X# ] - {% ~( { b/ F# a9 | n
- Encoder_2.setMotorPwm(Speed_L);" | a% l; O4 @7 t
- Encoder_1.setMotorPwm(-Speed_R);
' V( j) } x" g+ n1 W - }
複製代碼
* m8 T' z# Q. [3 ]* p4 E' {MeLineFollowerArray.cpp' K9 d3 h8 q; H# E4 P: `
- #include "MeLineFollowerArray.h"2 L" K5 o& U& \" u) S# _3 l
- * j6 F: o% m/ }! Y
- #ifdef ME_PORT_DEFINED+ D0 Y; H- `5 o) f6 N/ ^6 g4 Q
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)9 O- q) V3 g# F& J
- {$ r: V* ^. c0 @) ]
- / l$ Q# ?% n2 T: h4 F
- }6 ?( r0 X5 v2 ^ x z* @# W) V
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)/ i2 ?. b1 x6 V8 `# T3 j4 {, _
- {) g" E9 L' i0 K) F2 o& C
- _DataPin = mePort[port].s2;
. u+ [: r- e5 ~8 w: g/ k# B - pinMode(_DataPin, OUTPUT);
- u/ D; {( A2 o - digitalWrite(_DataPin, HIGH);4 l- B( p8 A0 A$ _/ V3 Z( [5 O
- }
7 h' H8 G+ W0 R: b% F/ r! A: a) J - #else // ME_PORT_DEFINED" v6 h4 u1 c. y" g4 @& {0 \
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 ]& E( h# h& M$ b. V5 v
- {
# w2 }6 [9 ]% O3 C - _DataPin = pin;+ V6 V$ j0 Y- d$ n8 q( P/ P
- pinMode(_DataPin, OUTPUT);
' i* C7 b$ a) A' [1 b6 {" O - digitalWrite(_DataPin, HIGH);$ y9 e/ s3 C5 U4 \
- }
) s6 ~. T; ?; [0 X# Z6 X( K& l - #endif // ME_PORT_DEFINED
% y( u' `) a0 X4 ^/ _
2 ]! l, {% r$ o; I6 J5 ~# ^6 n
* Y8 Q1 |9 m; s6 n6 o$ ]- void MeLineFollowerArray::setpin(uint8_t pin)
% U! m$ K8 Q2 l5 ?" |+ E - {
% L, O# C3 X1 y7 { - _DataPin = pin;
; X8 w( G$ K! Q" n - pinMode(_DataPin, OUTPUT); 9 }1 H$ O6 A" ^) C
- digitalWrite(_DataPin, HIGH);
' L% ], K7 b' E" a
$ R$ H! o8 r6 A0 e# V- #ifdef ME_PORT_DEFINED
' k/ T; k" a- m6 P% D) K4 j - s2 = pin;" U. ^/ j# M! y( X' Z* B! ]
- #endif
; N" n8 X0 F1 c! S# V' m. X u B - }
( w8 w$ ]2 B' P' r5 i
' s/ A0 t# _' C6 h0 b. u% ?3 Q- uint8_t MeLineFollowerArray::getValue()
7 z1 n# n+ y. j4 L: y - {7 P3 t; e5 M) ]# W/ S5 {- N
- uint32_t LOW_level_read_time;( B3 X$ d# n1 u$ A# T4 y) ~
- uint32_t HIGH_level_read_time;: m+ h2 M% c4 h
- uint32_t time_out_flag;; O- g! f5 _/ I* i
- uint8_t Sensor_Data[3];
# e/ e2 d) m, u - static uint8_t old_data = 0xff;. i8 f3 X+ c2 s' |* c
* v' c/ G( y, u& ]* j2 {# }- pinMode(_DataPin, OUTPUT);
; p, W- k$ ^! N5 d - digitalWrite(_DataPin, LOW);
, [/ _ D. @9 Z' i3 J3 b3 q( [3 o - delayMicroseconds(980);
6 o1 C5 j! n: {9 ?8 D7 P4 b - digitalWrite(_DataPin, HIGH);
7 v' w5 g1 n1 t2 X# S" ]8 N8 q$ s
$ n- ?! q1 ]- C! l b+ G3 F- pinMode(_DataPin, INPUT_PULLUP);9 \; n" J# t2 q' c# [! C7 }
- delayMicroseconds(10);( U& L# W" ^2 o# W6 f. z( l. p
- : V5 M W e% |" r6 h+ k
- time_out_flag = millis();
3 c1 N, Q9 ]- E& ] - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ _% b0 r* }6 L
- ' h6 z( G3 i2 J$ I9 c
- LOW_level_read_time = micros();6 n9 h, [8 S% q4 f( d0 F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out' d* Y* _; Q8 Y/ k
- {
& {$ H; m% A9 b* V7 @. S0 W& L- c5 z - return 0xff;
" Q& l* I- g8 p a# l$ ]3 P, H! h9 D - }
5 o. l9 ^, L: j3 G2 H; ]
) Z% `1 w; U/ r+ n- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" \- I8 G1 L& P, M9 _ o - - q& _1 ~! W5 F8 t
- HIGH_level_read_time = micros();( d" t8 y4 O) r5 u2 B. N
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level! |; Z0 j6 B9 C# P( G5 e
* @4 n( E) D! Q/ }( a9 U3 h- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 q; `! ^# d! e* [% ]& E( I/ q5 N - {
: o6 E" e, @+ }1 w* n) m0 ^1 G - return 0xff;
' y0 x' k3 {7 w# o8 K: v$ s - }, c9 H% E7 t8 L4 x" O6 l
+ u' _5 G: Z5 ?+ w9 g* A- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
5 G: J& R2 a$ k; \! R) z - {
& b; Z) C+ h1 ] - return 0xff;
$ w: H8 v/ V6 n o* L( J9 ^/ ^ - }
: k& C2 }- A& D* |" o7 Q5 }+ _" G - + r0 d; k( i% ?
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
/ a' l2 ?3 a, D } - LOW_level_read_time = micros();
* i) o* w) w( N2 M# c - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level$ b% h4 A+ r3 ~" t' G* ]
: K B& [* |3 ~ x$ s; f- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, U% Y) L Z ? - {
2 m( W$ J, E+ @: f0 B8 n' p8 z - return 0xff;: n, p4 O* Q$ F* V% N5 g. W& ~+ ]
- }3 s+ m [# @* |8 G8 @
7 [3 N# ^. z; R r- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))# n- J4 X; U8 H G# ?+ K( z9 R
- {
- e8 V% U/ p- t1 A - return 0xff;
7 i1 b4 r4 g# o/ _. X; p4 ~# _8 J - }
; v1 d2 ~/ t/ q: s
5 w3 w" F: Y7 ]/ r- g# w& w( M- for(uint8_t k=0; k<3; k++)! M+ l8 g% C' H: i1 ~1 H8 s+ f
- {
# f- S; x# M0 u$ n4 N - Sensor_Data[k] = 0x00;
# N$ `0 n, G3 `' i - 1 V9 f6 M/ ~5 ~
- for(uint8_t i=0;i<8;i++)
. c" r# m# y+ @ - {7 P' ]; |+ L3 H
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level, P# a$ f/ c; m) `2 f1 w2 I! Z
- HIGH_level_read_time = micros();
( G) R& i! p; _" Y: x" n; | - LOW_level_read_time = micros() - LOW_level_read_time;/ E1 y; t# ~ c+ U
( V; }- ^4 c2 `$ V" m5 \0 l- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 } u2 T$ x0 B/ k8 Z& |" ^ - {
' Z% C3 }0 j# [, g - return 0xff;
# \% \3 b1 O( p5 m - }
" t8 l" q2 p) o0 @$ J# X% b! c( l) }
" ^: G/ \, k- ?+ @- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 J) T5 R' t0 P
- LOW_level_read_time = micros();
; _. C1 F* Z0 F: x7 b& }; ` - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level" @8 M( k' l5 y% H ]/ S0 q
/ D' q M7 z0 h" X% [- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
7 R7 a$ F* ]$ d* U# w! q - {: A3 O, d$ e( m3 U8 g# L
- Sensor_Data[k] |= (0x80 >> i);
) b( A2 Y- F, R: o( d - }% I# V1 N A& P" }" b$ d
- else if(HIGH_level_read_time >= 100)
& F" ~* M/ v7 e2 L/ R( t - {' `6 E% x7 f3 j0 { g
- return 0xff;
) i2 [* {8 Q# F! {+ _) V - }
* Z/ H) @; E# M0 ?9 L
/ ^( X3 y2 b* U7 m' B" l9 t4 z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
+ P! s O; Z5 C: P - {
6 C7 M) w8 j" {, J; x. ^ - return 0xff;6 t0 J5 t# h# `- t3 u; `
- }
7 r9 R: n, p/ ~! p - }) N R+ E- x; J- v
- }
3 W$ \: {* n5 X) ]2 [0 C+ ~
' t5 a& j) G6 O7 b1 ~0 |- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
4 M# `4 @* w9 Q - HIGH_level_read_time = micros();
0 I- v0 a0 k8 Y: _& @* u; u - LOW_level_read_time = micros() - LOW_level_read_time;4 O% E8 K& U* r- O. p8 |( \: t& ~
- ( z+ [! C4 H% I, C; a
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ K$ _. o0 x9 n" k
- {8 @8 z/ u# S6 c3 n, e3 P" B% P, c
- return 0xff;
* b, y% U. _6 X- E6 U; j - }
4 ]8 W8 U. N. c9 `/ ~# y. ?) d
) d3 [! g. @( P3 X) }- S k- pinMode(_DataPin, OUTPUT);+ M7 h* ^& Q) H5 j; U
- digitalWrite(_DataPin, HIGH);
+ V4 C" r$ H$ V/ W0 l3 S! C2 Z
9 a. X1 ~2 H; O- J5 w3 R% x" I8 d# X- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
. O) g2 g1 B& _' U - {
& N1 B: G5 z( b( p( p. X - old_data = Sensor_Data[0];
# L0 |' E$ b; I5 s* y, J" Q5 y - return Sensor_Data[0];
' k8 }) p, P2 Q9 M( v% l - }
L b6 W* N1 a2 e - else
. B8 |0 W% p# g5 y1 U/ w1 f - {
6 @. t& c6 T6 @" D - return old_data;
( u1 w6 I1 n' ~' @ - }- B/ o" c7 e# ?3 u( g
- }8 r7 M1 Z6 h0 G _0 q
複製代碼 0 y9 L2 q8 S2 ^, \" v
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, Z% i* l- ~) M* ?6 k/ z) f8 t
2 a5 r7 R0 {) x6 R
|
|