|
|
* y# p. @) g% r! c$ a$ ?% X) w6 W) f' Q- #include <Arduino.h>
! O" n- _& ~( q- _ - #include <MeAuriga.h>
3 J( I0 W/ I- b* f" O7 { - #include "MeLineFollowerArray.h"7 V" c# _% q6 n1 I2 N* ~6 N0 c
$ _2 D6 I0 U R* W- MeEncoderOnBoard Encoder_1(SLOT1);
]" O/ p9 {! P" y+ a* V! f! k - MeEncoderOnBoard Encoder_2(SLOT2);- D7 w, O! U7 W
- MeLightSensor lightsensor_1(12);- L/ b5 W; l4 ~9 O; ^9 ~
- MeLightSensor lightsensor_2(11);- l; b$ d7 ^" d o" c0 n
- MeBuzzer buzzer;
# V9 t8 w1 i# E - MeLineFollowerArray linefollower(PORT_6);- u5 Q) ^5 n, K1 @, d' r7 E p9 @
- % ?1 R% E$ ?2 G9 Z& t; w0 \
- #define Error1 19 Y) P% Q. ~/ `/ W9 l9 T/ \
- #define Error2 22 Y- M& U% C, T9 B* R" \" T: H# _
- #define Error3 3
4 r1 O! r- @- M
* m) @5 s: q) k5 F, u- #define Kp 152 }+ R/ ^( r. n3 u X+ U3 w
- #define Ki 0.15
7 I2 u/ f- Z$ A+ L( J - #define Kd 0.03) {6 w7 I% o0 u Y5 b/ W% a, A
- $ h( `: Y4 `( }5 A% R: }
- uint8_t sData;
8 G, ^! P/ |' `7 w+ A+ A - uint8_t D1;+ R1 s' R& H9 M* L6 j
- uint8_t D2;
( h. P( o/ X8 T7 ^% }: a8 i - uint8_t D3;7 W+ @% X1 \, {3 n" S
- uint8_t D4;
5 ~7 ~8 v7 Y! _ n - uint8_t D5;7 M3 R: H0 Z# ?0 ?2 O" `7 y
- uint8_t D6;
& e) s% D, U+ ~: F. d& p& ]
. t" }+ p+ B/ m! g; V- float previous_error = 0;
* E9 i9 ]5 b$ d6 W0 p - float integral = 0;
* @+ X: {1 ~: l/ x - float derivative = 0;
, M! Q' \8 e& K) Q1 I - int Speed = 160;. t- f( G* _4 q8 l' Q( z
- float output;
; [7 z- y) d$ @. |: {8 L
- S$ E/ R% w6 t% N, r s- byte Left;+ l" {; t6 N9 |6 i
- M. N' X$ ^( Y6 C$ s v& A3 X
- void setup() 8 h) ]9 g8 `) w8 i! a% Z
- {, ^: c; Y5 X7 c# I1 t+ i0 \- z
- //Set PWM 8KHz( g8 r$ X: C, n8 d/ C
- TCCR1A = _BV(WGM10);; N0 E% u' v3 U+ [0 l2 @$ h
- TCCR1B = _BV(CS11) | _BV(WGM12);
; f# O a3 S: ?4 p - TCCR2A = _BV(WGM21) | _BV(WGM20);5 Z/ Y \1 j; w0 e
- TCCR2B = _BV(CS21);
, x+ U- [2 F5 c+ T8 v3 i6 V - Serial.begin(9600);1 J! I% k3 V1 D9 @, z4 ?( T- n
- buzzer.setpin(45);
9 c) b& B6 H$ v' y H4 p2 y4 g+ X - }
- C: _8 r, `, C$ v L# q! }4 @. O* |. m - & T. f9 G$ h/ V0 S. C6 ^
- void loop()7 ^4 H; b- H& J5 s* a' x
- {
! [/ x& `' f' z7 p - while(!((lightsensor_1.read()) < (10)));$ V- T0 U. C% Y( I1 z/ P, @6 y& j
- buzzer.tone(1047, 500);+ I/ g, t: g, p* w6 i# a
- delay(500);1 y8 `. h+ c: @6 }: o2 S9 |- J
- do7 @5 Y' f3 I+ T# Y
- {8 |* H( x; v. a T7 O3 x0 {
- int Err = getErr();- |2 @% c9 ?! w! }1 l( I: s3 |" V& ^; b
- if(D1 == 1)
; C- D1 `4 s" F U/ T - {
3 M+ o S& [% O& I1 i. }3 ^ - Left = 1;
; A, l% r0 N5 v& v. ]( H& b - }
% _5 o4 @1 T$ \ [% j' q - if(D6 == 1)( _5 b, _* }8 G7 `5 D
- {! t2 c; z: t" {9 h4 f
- Left = 0;
" H! A. e7 C0 C3 E - }
. K; T% ^4 [4 X5 U! X - if(Err == 99)
1 Y4 [$ k$ f9 n/ y9 x1 k9 p0 { - {0 A5 ?3 x. x4 E3 N/ F4 }8 S
- if(Left == 1)
8 V- ]+ s* z# M/ ] - {. K2 s; L8 t, W7 b
- Speed -= 5;* O/ @& z. l7 u8 N! e. V
- moto(0,Speed);
9 ~+ M% o' N9 x, i" V - do( \: D& F8 S/ L% e
- {
& Z/ W% p+ c" e4 T8 S0 l - Err = getErr();
# v7 E }5 M8 C4 t - }while((D1+D6) == 0);
! h9 J2 r* {6 W7 b$ L- E - }
1 `9 U: X3 a$ I U0 V# E) ~ - else1 x! h1 J/ k! v6 A3 @, K
- {5 \3 g3 `" }: \* R# X- o" a3 a. t
- Speed -= 5;( U, S3 j: m* I
- moto(Speed,0);
k9 k1 x+ F/ h% w- v - do% `$ ]0 i1 ?& o# y5 n
- {
3 H9 ~9 z! l) X8 q E1 q4 G - Err = getErr();
; j. r7 @1 L; D2 w1 r; ~ - }while((D1+D6) == 0); {7 E9 C2 z) v0 i3 ^; B
- }
6 M1 P( A' r' Q" n - }' L0 P g. F& l% n) [2 O9 d* j( H
- else
# U2 R8 N9 A2 | - {, a0 G D0 O5 O3 Q
- if((Speed < 160) & (Err < 2)) Speed+=1; ! B5 M! C6 f/ [# s' A
- if((Speed > 100) & (Err > 2)) Speed-=2; ( `3 ~) ^6 ]+ Z
- integral = integral + Err;7 E8 c: ]/ t( `& M. s. V
- derivative = Err - previous_error;4 J% N6 S& k7 t/ w: C0 q& l
- output = Kp*Err + Ki*integral + Kd*derivative;
! j. m& q9 V. g9 p/ O9 ~ - moto(int(Speed-output),int(Speed+output));! Q E1 J6 Y4 B4 y* v" w- b7 C/ O
- previous_error = Err;" f3 ^/ Z, `" {
- }! Z0 Z( q" d5 s) _# Y
- }while(!((lightsensor_2.read()) < (10)));
" \0 B) c: y g* B - moto(0,0);, S( p3 E) w- q( t( p9 ~
- delay(500);& c6 s. @4 D- n) \9 B
- buzzer.tone(262, 500);
P6 `; ]" {" f1 I - }9 X( Z5 @& D( \$ w/ {& D$ m; b- w
) g6 _+ Y* @0 [: k' h- int getErr()
6 O# A5 q# Z$ t* {! o - {
0 D( m/ u) T$ z& I" u# E9 M. V @ - sData = linefollower.getValue();; P/ `- s9 {) E2 u
- D1 = ~(sData>>0)&1;; ` H5 x/ Z- O1 @
- D2 = ~(sData>>1)&1;
9 A) a, p# m) j - D3 = ~(sData>>2)&1;6 H( M' g& ^# r" U+ T$ h% q
- D4 = ~(sData>>3)&1;. D1 K& |4 X/ a* Z5 C
- D5 = ~(sData>>4)&1;+ ?* h, E! @& ?
- D6 = ~(sData>>5)&1;5 B8 G% m1 u# M3 X& B6 x+ _2 z
- int downD = D1+D2+D3+D4+D5+D6;/ b( e" w: B- _, Z! G5 S" v. n8 m8 a) f
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);# @$ p7 ?% H& l! O
- if(downD == 0)5 C, z, m/ z6 E
- {
* ^6 f0 k* _; `, R - return 99; H: H6 d8 U0 h# W
- }2 ~* A2 f- N. a. Q
- else- s' K0 ^" F$ M' z2 R t ~- S
- {9 i* z, w( |0 O0 W2 P
- return upD/downD;
0 a' j$ c: r) h7 D+ w - }
% C1 y4 n: m9 J- g1 Z. s - }
8 B* U( w1 q/ L3 g( r
* d0 T% r& {5 W$ u3 ~( T* o- void moto(int Speed_L,int Speed_R)+ l: A& k2 W: q1 h6 P/ [
- {; a% l( [ z6 v3 q$ i( }
- Encoder_2.setMotorPwm(Speed_L);
( K1 K' W0 z6 ^" t+ g& e% x - Encoder_1.setMotorPwm(-Speed_R);/ D+ M' {" g6 w ?0 l6 {
- }
複製代碼 1 O) B9 X4 ^+ y8 {) C
MeLineFollowerArray.cpp2 ?3 m8 k! ]8 ?: q- J. s. d: {
- #include "MeLineFollowerArray.h"2 f9 c4 _& W& a$ ?
0 H4 a6 C* i+ K$ |+ A8 [- \: E- #ifdef ME_PORT_DEFINED9 F$ V* h/ M$ j6 w
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)# U+ q1 o" Z+ n9 ^2 x Q
- {6 `& Z/ j2 l& |, a$ F. X8 P
2 ^0 b1 a# P3 h. g6 n& T- v6 T5 t- }
7 R: u7 z) s3 W# j u - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)# r% l$ m4 y% K. u6 H$ g+ k# e
- {$ ?; p: x, X$ x0 B( I
- _DataPin = mePort[port].s2;7 I) \; N) C- o3 [& Q, F9 e
- pinMode(_DataPin, OUTPUT); 3 ]2 [) D$ v1 [% V6 L* M( t6 g
- digitalWrite(_DataPin, HIGH);0 v: u& P6 t |% @1 g! x) g
- }
9 O, ]# y# D7 U3 f# G - #else // ME_PORT_DEFINED
- H! W3 D, p" p4 L( z! N - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
1 j: r$ H: O9 S* v0 U. r' e! R - {
, ]/ ^9 s$ r8 r* N3 W - _DataPin = pin;& f9 p! G8 X5 I6 O, c: Z
- pinMode(_DataPin, OUTPUT); ! z$ b) d1 ?% |7 e9 u
- digitalWrite(_DataPin, HIGH);) I3 k0 T1 v$ g, n' E
- }
' U$ r) Y* W' h- X/ L5 L% v/ v: P - #endif // ME_PORT_DEFINED( p! \+ J* x! \/ {: m& x
2 H4 q. f# _/ Q0 b8 T0 _
) u, t6 E- y5 k, H0 Y- void MeLineFollowerArray::setpin(uint8_t pin)
7 [# G* Y% {6 M7 y" N$ [9 S3 t - {- u# c" D# K. x _0 `
- _DataPin = pin;
6 f% i9 N; H+ H" N6 ] - pinMode(_DataPin, OUTPUT); # A9 ?3 S9 b" I- w7 i, X
- digitalWrite(_DataPin, HIGH);1 P$ O/ c/ q2 _
- 6 {& v# ^+ [' o
- #ifdef ME_PORT_DEFINED/ `9 A* q( H8 k8 K
- s2 = pin;+ t+ O% n/ h% i8 H$ D7 w. d
- #endif! I5 \9 X% E# S* J5 z1 D3 q/ g
- }+ K; V% |+ @: ]* }. f! s
- 2 l1 S2 @ `" G1 P; T
- uint8_t MeLineFollowerArray::getValue()
1 A A# U6 }3 i! l! h8 u! `, R! p - {, R; Z) K! N9 d( c, o
- uint32_t LOW_level_read_time;
# [& Z% O% a( k! E9 g - uint32_t HIGH_level_read_time;
9 k( R! H, @( f- @" b - uint32_t time_out_flag;0 N. j, d' E" [0 @
- uint8_t Sensor_Data[3];: Z* S& y( b. ^+ A/ V! f
- static uint8_t old_data = 0xff;, Z C- E+ @ n( `
- , O5 T( r4 K) l9 j
- pinMode(_DataPin, OUTPUT);, O T" }# N0 H# e$ d1 J
- digitalWrite(_DataPin, LOW);/ t9 C! Q9 C: z9 u* @' x
- delayMicroseconds(980);
1 N( g: {1 L. _/ |% ~7 r' i3 D! P' A9 S - digitalWrite(_DataPin, HIGH);
$ I. f5 S1 {2 b8 V - ) ^7 W0 ?6 b5 G, X& g" W
- pinMode(_DataPin, INPUT_PULLUP);
( d! ?" B' s% |0 G - delayMicroseconds(10);5 }9 Z# ]8 T8 L l; K
' |9 h) u- u- Q5 @( v! ?) s- time_out_flag = millis();
3 c! R% k* n, B6 [& |* e) H3 `; X; K - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& i. C1 _, n- ?# o: d* ?
1 @( ~" c* T) ` \/ N: q- LOW_level_read_time = micros();- j7 }% U2 L) k+ [2 D0 W
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 z; P0 s+ I. a; f' u( `0 d+ R
- {
, _( |6 l ?+ E6 k0 D) j - return 0xff;1 u4 [, H/ R( r+ z
- }
' \& n: s5 l3 g* \% Z; g# }7 { - 3 V5 v9 N2 _% _1 L7 T8 \3 r( o
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 T* Y% N9 r* G3 A+ v
" d% s% z9 A/ C5 X' d$ M/ r* Z- HIGH_level_read_time = micros();# F1 ]4 R V) y; K% H# k3 n
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
. `4 o+ i. C! M, W - / m8 H1 N% N* U6 j. a; Q
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 K9 l. @8 L5 t; I6 }6 V! W B - {& |& J, O4 K9 a, {4 D; A
- return 0xff;2 l' h( A. E0 q1 p: g5 a' h/ w
- }7 n* ^, K2 c; m- i" u/ k
, o# f. Z$ Y4 q d- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
/ M1 H1 p6 l% @% g, Z - {
" a" F5 f8 L" K- h3 U" V, ^1 p - return 0xff;5 b3 L* B7 W2 ]) e7 ]3 V& L
- }" V' X! h' l Q* E$ I
3 n5 _) \9 n$ P% \8 f9 v: G* |; f2 C- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 k y2 s3 u3 `: j* Z, ^7 m/ ~
- LOW_level_read_time = micros();1 u) z/ r, s8 q" ?7 C2 M" L
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
, W' g8 Y( E4 y. o" {2 P - . H1 [( T( D9 e w. {/ @
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 P8 J: D; a; m; {3 i - {9 M8 }& A" e2 Z* F/ x( Y
- return 0xff;5 Z+ o+ K/ U4 x+ l6 ?
- }' C: J) \! O- A( W4 L$ _! m
4 W1 |, s+ K3 L5 I+ v2 i7 K- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; \; D# {* w. K: D - {
* X+ w1 d l7 @" E9 R0 m+ r. ~) [ w h - return 0xff;: y' [; }: j/ E# K; }. [7 o: u
- }: C3 B' Y. \2 M& l: M" Y
- - E/ S8 g1 O: j k$ C. S* @
- for(uint8_t k=0; k<3; k++)
3 s/ E G& [. [7 j* \; G: n2 E - {
, ?* o0 L$ e; ]- K, r/ E/ @ - Sensor_Data[k] = 0x00;( N: s3 L3 U v% _
- 4 f }# l& p, ]* P8 A& U& {
- for(uint8_t i=0;i<8;i++)( Y n* E1 @8 N) s: j
- {
^9 o' }2 w/ q7 E4 } - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
- y; D9 H! Q9 `/ B% y - HIGH_level_read_time = micros();
' } P% v' Z1 p" n - LOW_level_read_time = micros() - LOW_level_read_time;& O/ L( o6 u k" |, w
- 0 s/ t8 p" t" A Q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
5 `9 ` b; G& U, } - {/ ?. D7 e( L) p6 K$ ^% j: v1 E
- return 0xff;* D0 G3 s1 w) U8 x& [
- }9 Q7 T' R5 v0 Y3 M/ L$ A
- r5 e' b+ \4 G, |+ p* Z
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- K' a8 F0 K) _3 p( l) {
- LOW_level_read_time = micros();, } f! v( R' M: A) m
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
: V9 p7 c# Y! ]8 A A - 8 { D8 a$ s4 V% J, G9 y9 Q
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
7 H# I* U0 S3 z2 m' Z% i7 u+ _ - {' x: |# _$ Z+ r" N
- Sensor_Data[k] |= (0x80 >> i);
9 E0 x, |: N- t4 Q% K+ D: H - }
( I* N9 u: l' ^- A; Z* J - else if(HIGH_level_read_time >= 100)
+ P0 f4 t2 u( c! t; {4 t7 X" o! t - {5 I- F. y( ^+ \; B0 ]
- return 0xff;
# f& U# f2 _* U" d3 ~* s - }
4 x! ?# E( ~: s* L* ]6 h3 h - + u2 J5 y0 f2 m3 _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
% |) G8 v' `, N5 @* }7 A( O - {7 P! V3 @% @; m, Z/ ~, ?+ V( x4 V
- return 0xff;
6 e$ w0 D2 W* \0 z - }3 d: x9 W6 E0 Q
- }) I4 }% A, N* y( v, w- w
- }
* \; T; y6 j- w1 V
# ]' o; b& M* K- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ W( ]- v( d, R2 j+ J9 c
- HIGH_level_read_time = micros();9 a% ]/ g' ]- N+ g% W- X9 A2 t
- LOW_level_read_time = micros() - LOW_level_read_time; {3 H0 k& Q* F% B3 |$ ], D
- : L. s* h: \7 E0 b! F# u3 S) ~
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )" ]+ u0 D2 M! A. K
- {
# C. k' Z/ m3 Q! z& Z - return 0xff;3 ]( j9 o y8 ?+ |0 X
- }3 x8 n+ R- Z: x ^6 f9 \, t
- & R5 G3 b1 j. _) E
- pinMode(_DataPin, OUTPUT);
% S0 v% T) u1 f - digitalWrite(_DataPin, HIGH);
/ Q$ M# b9 c% g2 ?" `
3 r+ h( m A8 ?: z5 K- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))* _2 V( h+ n$ C' @* }4 P
- {
% e5 a9 `2 ]2 y2 z; s! C! p% R - old_data = Sensor_Data[0];
; |" H& @% A2 L/ T* a - return Sensor_Data[0];& Z% H& f* c( G
- }
9 t }- ^8 X5 m! J) X - else- s9 ~' N4 z! p3 K
- {; v# \ d6 c7 Q: P! p
- return old_data;
0 E; N0 b. J8 `5 Q% J - }2 y- R: Q2 W( B8 t# f
- }
, Y5 I% y8 N6 a" S( z0 D' l- s
複製代碼
7 Y- V6 E$ j! S1 u# dMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
1 }. m1 k& d Z# j
0 H* Q" d% r6 j5 ^! r* l; \) a+ M |
|