|
|
5 V/ x }$ G% h! S7 T1 \- #include <Arduino.h>6 \: Z: Q* @, f! \* R4 E$ D* x
- #include <MeAuriga.h>4 ?& f: z- G- O9 f/ A2 F& \" u
- #include "MeLineFollowerArray.h"* e3 ~5 |4 h7 Q' I/ z
; e: |, U5 e& w* L( i- MeEncoderOnBoard Encoder_1(SLOT1);
2 O T% b& @4 z% S+ \ - MeEncoderOnBoard Encoder_2(SLOT2);$ |" k0 o# i) ]
- MeLightSensor lightsensor_1(12);
3 ]( V1 ?, V1 C - MeLightSensor lightsensor_2(11);
) c+ ~# G, f. t* K! W - MeBuzzer buzzer;8 s# L( F' W2 A5 K& Z# y
- MeLineFollowerArray linefollower(PORT_6);
4 z" T% J. p3 N
/ P. E5 s+ `3 o h+ I# K$ l- #define Error1 1
3 c3 I) K8 n1 Q( q5 s+ m - #define Error2 2+ o) B+ u; m4 g8 \% n4 {' G- ]
- #define Error3 3( V' @, |' L& E6 o3 P
3 \6 }+ l7 j3 {' ^" @- #define Kp 15
" {0 D5 A- E. F6 L% t; I% c3 j3 _ - #define Ki 0.15
, ^* W! t; @7 w+ L6 V* N8 d) F7 F - #define Kd 0.03
% E i$ m; v( Q7 U
/ b& @8 v* Z& T; q( n8 L8 _- uint8_t sData;
: M! G: j% B' e3 } - uint8_t D1;$ m* V0 c" V) Q$ B: t% h
- uint8_t D2;
% j! u! f' j1 s9 d2 K - uint8_t D3;6 Y1 h% ]: M5 Q% Q* m! }" r
- uint8_t D4;0 d, R, ^2 D1 t( n* g
- uint8_t D5;7 g. x4 u4 \8 o+ ~! g
- uint8_t D6;
4 y8 l! U5 u" ^3 p/ P - 6 M- n( a) N' O4 p4 p' n7 Y( d
- float previous_error = 0;
& i) _( x N- {. q! F( U, S& Y; F( t- Y - float integral = 0;
/ H9 F7 e3 u, z+ h! s - float derivative = 0;
7 l8 S: v. q5 r( y - int Speed = 160;' Y$ ?& h/ p! z$ c6 w
- float output;" B& c* s; \0 v
; Y5 W8 [2 O z7 r, O6 u- byte Left;" j4 z: M4 w) J N" p4 I5 M
! [( S' r) l5 L- void setup() % n* r; d) x0 Y* ^! ?
- {. K* s' S8 B9 P" M% ]: n
- //Set PWM 8KHz
+ c7 M' }+ M1 o" L - TCCR1A = _BV(WGM10);
5 ~6 R3 _8 p3 x - TCCR1B = _BV(CS11) | _BV(WGM12);0 `* W5 l" ]' U' C$ _8 i
- TCCR2A = _BV(WGM21) | _BV(WGM20); e8 a- Z% }5 c2 r) @* `
- TCCR2B = _BV(CS21);+ n) r. d8 Q+ k
- Serial.begin(9600);
k! ^3 y1 q2 E. `3 D - buzzer.setpin(45);
# g. [7 P4 M7 _6 U, O8 n9 E - }2 M/ x A T$ o$ D: T/ v
8 j- I( j6 G, o, S- void loop()
' H r. L1 Z9 m& ~& {: ]: A" W I/ o - {
: U( B" B& p5 x, i3 j5 ]! _! P - while(!((lightsensor_1.read()) < (10)));
7 ?/ q" H7 j- N+ u4 i" W! U - buzzer.tone(1047, 500);" H6 x- h1 b, Z! O- t6 }# x
- delay(500);
. r+ A2 J2 _% K1 z# D7 t6 C3 W - do
+ N2 `" I) d0 D/ ^ - {
+ J) S7 Q! Q/ G& V* H - int Err = getErr();+ E) x# d2 O2 F, G! a. C; _
- if(D1 == 1), c( p- q5 t" B6 }
- {0 ^. c8 a# g# H" k+ ]; d
- Left = 1;4 ~( Z. c5 O1 T0 A
- }9 Q6 J6 ~ X4 T/ F
- if(D6 == 1)
6 F1 [8 v+ m7 O# j3 [1 y - {
9 P4 w& u* @# Q - Left = 0;4 H0 r7 o* |6 ?; w
- }
, l# U7 M% ?% v8 N4 d - if(Err == 99)
{$ F3 f% x y" I - {. h; @, e3 l7 `$ v- v: K: A7 h1 A
- if(Left == 1)' o8 ^2 }& c2 e0 S0 F
- {
9 L w) g P, }* s - Speed -= 5;
- O! ?) ]# _/ q - moto(0,Speed);
2 E( V( D$ X/ M - do
8 G3 G6 H' ]* O) b( M9 C - {
) k1 g) X7 }3 h# T+ T/ E' s5 o - Err = getErr();
7 j8 R* G) n8 t- b( z+ I - }while((D1+D6) == 0);3 x p* A8 o4 \. m- j
- }) w1 O- L. R: _9 I
- else' _3 `9 h0 `8 q
- {
+ j% n! D6 z' X" g7 C - Speed -= 5;- ?2 Q& C' O* a" M3 G. X$ P
- moto(Speed,0); ' a4 a3 c' d: _2 }4 U& B
- do. d/ c9 E) }7 @7 L i
- {
4 v; ~- j9 I" V0 ]' { - Err = getErr();
6 o+ f$ w6 G! k) q9 v' D9 ^ - }while((D1+D6) == 0); v. Q$ M. M( k( O2 m" Q% V5 b( d
- }
1 h" J+ |5 o8 _: j. ~ - }
* a5 _8 |& h6 n' }. t - else
! {+ C' D- E% D4 R4 d1 | - {
- d y: f) Y9 I1 Q4 P6 T - if((Speed < 160) & (Err < 2)) Speed+=1;
k% r+ T0 e9 M2 W, w - if((Speed > 100) & (Err > 2)) Speed-=2; 1 p/ I* G8 ?" N0 N' s8 {5 n. ~
- integral = integral + Err;
# {7 e8 k3 d2 s: U$ B: ` - derivative = Err - previous_error;: C0 h/ t9 p0 Q* s) k- S* b
- output = Kp*Err + Ki*integral + Kd*derivative;( C/ F1 B- i& A
- moto(int(Speed-output),int(Speed+output));
1 |9 l: F; ]9 s% n$ |5 B - previous_error = Err;
) s' t; }, b; Q1 o' a. q' S }6 Z; u4 e - }
# j8 @0 p% @9 {# y - }while(!((lightsensor_2.read()) < (10)));
& y, m" V- n; A( u - moto(0,0);2 \ x3 _1 ^# ^
- delay(500);( y' m6 b1 p! I# g! e" ^$ `, C- X
- buzzer.tone(262, 500);
5 W4 v. m. @ b* A7 P% x; B - }8 c$ Y0 u2 C- x6 [# F
- . q2 {1 I* ~2 L9 f0 D
- int getErr()4 v. ` g) D( j2 c E1 Z0 |8 y9 M
- { , n( o: f: r& m3 P3 y: B$ l
- sData = linefollower.getValue();" Y' G$ W M1 a) d
- D1 = ~(sData>>0)&1;
" c- h2 C7 }4 |) v) N& q, B - D2 = ~(sData>>1)&1;3 M9 e3 G2 g3 O1 \1 W p
- D3 = ~(sData>>2)&1;+ ]# w+ O y# g2 X, J0 q. X
- D4 = ~(sData>>3)&1;; c: F- M- J% S1 f- `5 \ F9 z
- D5 = ~(sData>>4)&1;* _- `+ c! C% M. p6 ^5 B
- D6 = ~(sData>>5)&1;9 V1 `# F+ E& i( F
- int downD = D1+D2+D3+D4+D5+D6;
& o) }: a; j( x9 a - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);5 l/ f& @/ s4 a) b$ K4 Z
- if(downD == 0)
1 W2 Z0 ?+ D5 g; X - {
* M& o( h) F6 B/ [ - return 99;. {* D8 z( Z/ z- m* G8 K) l
- }
( \- G9 b3 \/ q4 ^7 ^0 A - else0 C8 Q6 j' @! U
- {
* l3 i* | w% e" O$ b - return upD/downD;
, Z+ |( y- z7 Y - }
" a3 l! T% _; c - }
' ~/ I# H3 X% _2 v4 a- s: h6 b X( V - . P' R; y' C ]& M
- void moto(int Speed_L,int Speed_R)6 J! s8 D1 [4 P3 q
- {( B1 p* c7 |* d& x: W' m
- Encoder_2.setMotorPwm(Speed_L);
: H9 F/ Y# U: `- A0 \3 c2 L# _ - Encoder_1.setMotorPwm(-Speed_R);
# o, W, H* o1 N; \ - }
複製代碼 , A4 P8 `1 P. O; L: r! _
MeLineFollowerArray.cpp
0 ~3 M# `$ ~( `% u- #include "MeLineFollowerArray.h"# `6 g2 b0 [9 u5 c- V3 p( C
6 ?0 k5 E) `% b, g. B5 Y- #ifdef ME_PORT_DEFINED# |" ]. Y' c% V9 o3 `. z
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)( o* X3 V2 E$ r3 O8 h' W; K$ f8 d
- {
6 F' d$ y0 U" h* I
) T% @# |* [8 _/ a7 W S ^! ]- }
& p' m( ]" c) q- ?1 H/ g. O8 D - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
4 |8 ^* }! }% O" _' J - {
* P' c2 i0 P+ ^. R; ~, w - _DataPin = mePort[port].s2;, K9 B y1 J6 A {
- pinMode(_DataPin, OUTPUT); ( m, j0 L$ ~4 @8 m
- digitalWrite(_DataPin, HIGH);% m$ A5 `4 P: d$ g$ t
- }$ [( `: M' h7 D
- #else // ME_PORT_DEFINED
0 C. O. d' A: A0 E) m9 ^ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)' L+ L9 b! ?. K
- {
3 @- \$ ^2 _4 Y5 K - _DataPin = pin;4 R q; B0 o I6 t A! ?, p; ~
- pinMode(_DataPin, OUTPUT); ; X8 r9 F6 X5 j1 W9 q0 H
- digitalWrite(_DataPin, HIGH);
* `4 N0 X1 Y, R( Y8 h2 J) ~! W - }* \- k H( Q' ~, ?
- #endif // ME_PORT_DEFINED: ~7 I: A) U" w5 }9 S; j
- ' q9 k, j. _& N1 A+ r
3 P; ?1 {* c! w- void MeLineFollowerArray::setpin(uint8_t pin)
& @+ q7 P. A9 b/ c - {& ?( L4 \! b+ e9 `+ A
- _DataPin = pin;8 l; w, H4 m5 @$ X: Y z5 _+ T7 U2 h4 m
- pinMode(_DataPin, OUTPUT); 0 ]- A; k* N4 w) o5 `" @
- digitalWrite(_DataPin, HIGH);
( d i% S: t. E' T - + [1 b2 u# Q1 {, b) ^
- #ifdef ME_PORT_DEFINED( q8 I/ u; f$ J% P$ G0 w) X
- s2 = pin;5 d. n: u' ^4 Q
- #endif
1 v0 h I, W7 l- y" Q - }; `' ^- \9 j# B& c0 i
& f, o- o5 X) e9 a2 y) H! \6 G- uint8_t MeLineFollowerArray::getValue()
6 w8 g& a6 t" K - {; f0 {" O2 s1 @3 B1 V- J
- uint32_t LOW_level_read_time;0 b" M2 A3 ]$ y" g+ u& F9 A
- uint32_t HIGH_level_read_time;
: S9 R9 Q9 Z7 D! O& M- g1 P - uint32_t time_out_flag;
q+ @6 `- G" S3 t& z - uint8_t Sensor_Data[3];' `# L/ Y% L i
- static uint8_t old_data = 0xff;) T( v, ~4 B: i5 r$ B9 ]8 D& P
- 5 q5 Z/ L4 u$ T. g0 }
- pinMode(_DataPin, OUTPUT);
$ I6 p t9 z4 \* `! k - digitalWrite(_DataPin, LOW);& G" r+ v- w% \3 Y6 C
- delayMicroseconds(980);
; g/ d, @; s$ V7 d% | - digitalWrite(_DataPin, HIGH);
2 x# U3 w: `. r; {4 m - * Z c- M- @+ a! p* V1 w
- pinMode(_DataPin, INPUT_PULLUP);7 z$ l% a# C" I$ I1 c/ i0 U# u
- delayMicroseconds(10);
1 S; K) D$ P3 [1 v$ w
$ `1 y |; k! p1 I5 v2 U- time_out_flag = millis();9 \* H7 x: a7 \% }2 h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% ], n, f$ O! e: q5 Y0 p7 g3 ^
- ' h" s) `3 {' I# y3 q
- LOW_level_read_time = micros();# r8 i4 a( j0 d! M: k/ e! [, g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 z! w* G4 _" T5 r# v
- {
: r( m" B' N* L% `0 p' d - return 0xff;6 n; u. w i6 J4 U! X! v' Z
- }
2 _, l* O6 F, T' B, b0 D! M$ ]
. ^2 D6 b1 u1 W% M6 V/ |$ E; N8 I- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ f' f; q+ J; e$ r- H
: k- Y+ ~$ O& |6 [) H4 {; J. [$ ?- HIGH_level_read_time = micros();
, i* Z+ _- x ]5 W - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
' }" H- C# u+ m7 e - ; F ]) T- ~5 A g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
P6 z' P3 F9 d- M9 H6 R - {! I3 }: V6 M% N7 ?
- return 0xff; v/ `* }# P P+ j8 E) x% a
- }% c% c* Y; Z5 W+ t' I* _
- 6 n y5 ]2 z) @
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
5 d$ K: ~3 }5 g - {0 K8 I4 o- i2 H N4 R$ W7 T$ q: p# Q
- return 0xff;
, ]2 h3 A6 o' u% A& g" N - }
, j9 g& n: l7 X% _" s - 1 _$ l4 V" a3 R. Q6 ?; e) U
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. ^0 g7 ^% o# x6 Z
- LOW_level_read_time = micros();
( `* v3 G8 N- \* g - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
& P$ X% s2 A4 w
! A# W, }8 l! E# ?* K' f- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 c) E% q, F; v; S0 Y! {, {
- {
. I0 b! O, p& G - return 0xff;
9 b" n0 g9 @6 S2 R- l - }% V/ B d7 `! v) a4 B/ ]! _
$ ]* ?+ O9 R- U* k2 @3 J( C" z1 E- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; [* M" T# `; K - {
0 b& u! u: c+ {/ c - return 0xff;
8 k8 x5 J7 J& \$ t3 h4 w' e" o) H - }
9 q3 X! j4 J, L6 T: s - 6 c, n5 X1 V* ^. A
- for(uint8_t k=0; k<3; k++)
" j; k6 b Q6 h) G$ n* ~; G6 ^6 o - {9 L9 ^3 R7 U9 B; g$ @6 {- G1 \" \
- Sensor_Data[k] = 0x00;
+ d* E0 Q1 X e/ a& ^ - + z( b9 ^ \, h& _4 Y& Y* Z; V
- for(uint8_t i=0;i<8;i++)
9 P: X; A6 |$ [, K - {4 X8 c- [8 [- Q0 q8 s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level; Y U, o* K7 s
- HIGH_level_read_time = micros();4 u. j& e8 ?9 d& a3 r. I8 u& c b
- LOW_level_read_time = micros() - LOW_level_read_time;2 f' r& H* r3 i3 P) Q3 [
- 5 Z& I% q6 k$ m7 [
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
~8 U i" q4 @% u, e$ i. f8 D0 p/ h - {
9 j$ P6 C4 m. q( [ r+ r; p* f! O5 f - return 0xff;
$ U3 P. n3 B1 {2 ] - }
# g9 x5 z& r% } R$ B
{; U1 w9 b1 I- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' I$ L/ P% m. d ^0 G+ d/ {
- LOW_level_read_time = micros();
3 G2 [7 e3 a* ]! z, s. Z( k8 C - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level/ s5 @ r: q" {: ` Y0 u a+ Z' Y
- 0 E( R+ @! b; g7 Q/ a# e8 A! v
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1: E" A2 m T0 i2 S P
- {
* O" Y4 @" q& E& y7 M - Sensor_Data[k] |= (0x80 >> i);: I- w6 ]3 I# \2 D
- }3 Y( [' \) E9 z
- else if(HIGH_level_read_time >= 100)0 O) Y* V$ _2 p/ Q0 n0 b9 C5 v. y5 N
- {
7 n8 ^( @8 Y8 [+ k7 o/ [ - return 0xff;: ^1 s/ W- I- P! r& Y: O
- }
; R9 c j/ B9 N8 c7 F N+ s; U D
4 Q# [) v+ u' w& ~1 B( G& [- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
+ D2 G* W' a# ~- ^; q$ d9 M) C6 F - {5 d2 H" M9 \7 }/ b
- return 0xff; K3 z9 d* u" k4 h+ y
- }% t: n, {4 `9 Z% t* g. k: f* n
- }7 [4 N! K3 i9 q2 h8 B3 n, N
- }; a. P; N* o2 h* ^- n) p$ J S/ S2 V
- % _3 i, }9 G3 L2 L. V
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 g3 L) {8 y. [5 C - HIGH_level_read_time = micros();7 N6 b \% M; T) _& i- z
- LOW_level_read_time = micros() - LOW_level_read_time;4 X! _" w6 S& T# t) K8 M
; C+ H/ B. k6 P$ [; P/ I0 H9 Y# x- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 ]: u& M1 ]9 i5 M( ], T# H - {
, I; Z1 S* o% U. w0 l - return 0xff;) I2 U1 i! R! h) F& m. Q
- }8 {0 F7 i2 f" [! a: j) U
- " \8 ~6 ?: ]( F4 f6 n
- pinMode(_DataPin, OUTPUT);
- B. u n% |6 D1 p2 A' T - digitalWrite(_DataPin, HIGH);/ H2 g1 Z! ?: `: B, H
- U- m: v% t5 T* g- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
4 V6 |( [3 P9 l# v - {
5 R6 J2 O$ y4 r5 T& `- O$ B6 H5 d - old_data = Sensor_Data[0];
9 B! Y( m# H( C3 } - return Sensor_Data[0];: Q h2 f5 h) e0 ?: M
- }) |2 E2 ]; N( ]) @. C
- else
+ b! k) b# s+ j) \ [9 o3 \ |/ t$ n - {8 k I/ j8 V, e2 H
- return old_data;
0 w e# a' q; r0 W H; P9 [" r/ y - }
# n9 b8 x! Q! @! u7 H0 l u3 h - }: y$ w) a; X0 n" }
複製代碼
+ Q$ J9 t. r" ~0 g- C. N. pMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
8 a% U" b$ ~8 r$ [" [7 X; p' Y) k+ D4 d6 z8 ~. i
|
|