|
|
. v6 i/ x5 V1 H" K$ U
- #include <Arduino.h>7 \$ ^) k3 n* {, ]0 ~3 T
- #include <MeAuriga.h>
' g# t, h0 |) \% r( G# c X - #include "MeLineFollowerArray.h"
' w9 H$ K' }7 T& q9 b - 7 J& t5 p* O* R' g( j& t, I5 B9 u
- MeEncoderOnBoard Encoder_1(SLOT1);9 J1 p$ g$ e5 i+ x
- MeEncoderOnBoard Encoder_2(SLOT2);
& R, t5 B9 O4 U/ } - MeLightSensor lightsensor_1(12);' `4 X3 s/ V' {9 \$ M! S6 t" J
- MeLightSensor lightsensor_2(11);7 x" u) c ~3 i8 L8 J4 [; D7 [
- MeBuzzer buzzer;
# X& O( p6 T4 `6 ~' N - MeLineFollowerArray linefollower(PORT_6);2 p. r+ C" u* D: N* E5 Y4 g
- ( }, \; g( x' s4 A& c: b
- #define Error1 1" p6 e) t: N% {" t5 K
- #define Error2 20 O% m$ ]; q2 Y4 {9 K" c
- #define Error3 3
8 f6 K4 F; _1 D* R - & v- ^ `* ~1 X6 D. | D3 W
- #define Kp 150 [' _$ v2 N1 t, w, ]) B/ [% n
- #define Ki 0.152 Y5 S! R0 x1 e" ^
- #define Kd 0.03) C. j( I/ ?7 D# X( V G2 I
- 8 J( Q# M0 V6 J7 o2 n' W
- uint8_t sData;; `% `1 R0 U3 C9 X8 S' g" s5 K
- uint8_t D1;- D1 i+ y* q3 x# m
- uint8_t D2;! N* G$ C( g0 E, s7 Y
- uint8_t D3;$ v7 ~4 G, |% Y: G: o) C
- uint8_t D4;
' p$ O- ?. g9 a. H5 W- y - uint8_t D5;
' C' m2 L$ n. ~+ C' z% ?4 W# i - uint8_t D6;
1 C6 J- X* q* {5 i$ K( U# L - 9 M- C7 Q% [* S
- float previous_error = 0;& o, h8 o( ?/ D. z' Q0 M9 m4 N
- float integral = 0;
! ]5 B) M* i3 D) R3 g) O - float derivative = 0;$ j% ?+ F0 h% _% m5 v7 h* r
- int Speed = 160;+ T+ G" d' d9 h' ^- E7 {2 J. k
- float output;5 j9 m# y6 `8 M& `
& A& X8 m, ^; @8 z- byte Left;
5 F0 d v2 T+ T: R - % C s" \1 z" P) X
- void setup()
- x* z& g& t: l3 W - {
3 q G3 w! E# \; v: o - //Set PWM 8KHz: m, v7 [! \& w! [1 e
- TCCR1A = _BV(WGM10);4 s7 w: W. d4 I9 g5 Y; ?
- TCCR1B = _BV(CS11) | _BV(WGM12);; V( d7 h+ p9 g% {3 Y* {1 u, b
- TCCR2A = _BV(WGM21) | _BV(WGM20);
. a4 N9 Y& d, p1 P - TCCR2B = _BV(CS21);
' \ A% w" @! u1 r0 D5 D - Serial.begin(9600);9 H% L; U. `+ y- S8 Z2 o; s
- buzzer.setpin(45);0 \# U$ L5 m! J2 G" ~
- }/ W+ _: [1 C' G [: N# G) g
7 G7 k" @& S9 K0 n! y# b- void loop()5 I) k/ k3 t+ e W
- {
1 C' @# O/ N, [ - while(!((lightsensor_1.read()) < (10)));9 h. x9 z& X0 ~# p' V% f8 }8 \
- buzzer.tone(1047, 500);
! Q" y$ K8 C$ U! X( E+ q/ u, I - delay(500);# b5 o1 D) i; l, {2 U( H1 f8 z
- do
+ x9 R# o9 V7 U1 f5 f" Z - {" _2 |% r) B6 N
- int Err = getErr();
+ `* q# e8 b& A% d- T* U' c - if(D1 == 1)
$ `! d- y- ?1 g - {
, j6 |! T& j8 E+ g/ ? - Left = 1;
( d3 k0 W( Q1 u' L. ~2 K b - }7 N( Q( i* R3 d4 G
- if(D6 == 1)2 F4 [. h8 `( k, c0 J
- {! Q( V, u" u# \0 c; t; m/ ^) |
- Left = 0;* m* k4 f8 Y$ d! K4 S6 [4 l$ `0 a
- }! t( z2 A% E; J$ i; }6 ~. `7 p. v
- if(Err == 99)4 o ]" j/ B5 x2 A
- {# ~. k X0 U6 E: p
- if(Left == 1)
. @3 W+ s- a# i; f7 v0 I2 G% G# } - {
1 o9 r6 O5 h6 }. Q - Speed -= 5;
: d8 @8 p! ^3 o( A! s- v+ t2 B - moto(0,Speed);! D$ C; p" H. x
- do
- v1 K0 e T: l8 j& H - {" |: A6 ?/ b; T! Y
- Err = getErr();
; j6 \9 B" L9 [% |% g. O' @ - }while((D1+D6) == 0);, T+ ^: v2 R' `
- }
! T* V2 x/ g) Y8 H3 M. V6 y) j2 f2 H - else
+ W& B. o0 H2 v, A% q - {
# K# z# E! m; C6 q - Speed -= 5;8 S" L" U8 t$ Q+ X4 R( s1 o5 @
- moto(Speed,0);
, b7 ]/ _- `* X0 O - do! j) {2 n$ @" q0 b" ?6 K
- {
; q- O" n( \' S) d5 x - Err = getErr();
& D ~" r$ H0 ^0 [% r - }while((D1+D6) == 0);* a+ D% p9 x% q U& g P! w
- }& ? L _9 i7 W) y. E
- }) a7 J" J( d/ C4 r
- else" }, I- _( m" {0 W
- {
2 K* G9 S! _ v8 ^7 W8 j# p - if((Speed < 160) & (Err < 2)) Speed+=1;
9 f6 W" s/ N6 l1 X3 H1 R - if((Speed > 100) & (Err > 2)) Speed-=2;
5 v* i/ C' z9 ^! _ - integral = integral + Err;+ S7 Q0 O5 D6 d8 a1 p$ I
- derivative = Err - previous_error;% F+ r0 ~4 Y/ |; J- K
- output = Kp*Err + Ki*integral + Kd*derivative;
! u$ b/ x" q% x3 t' d - moto(int(Speed-output),int(Speed+output));+ Q" p$ n" F! y0 ]( e ]
- previous_error = Err;
+ A( F$ G4 u" A* V% ^ F$ z - }
9 R$ V3 S: G4 s- l* p - }while(!((lightsensor_2.read()) < (10)));
6 Y; `: \1 A0 [ V% c - moto(0,0);2 C7 t6 k+ V6 F- o, a
- delay(500);9 V, W; W) G" B! o1 K
- buzzer.tone(262, 500);
" S. t! }. W% b! u! P$ n1 f: G7 T - }7 |8 `6 ]; m8 b) {. p6 V2 C# B
- : _, v4 h1 S# v, _7 o: M3 F6 y' P
- int getErr()2 ^: R& I" o$ a5 v) T8 w1 [) u4 k
- { # @ P' Y! r+ f
- sData = linefollower.getValue();
. d( `: |) k* J" w+ F) P - D1 = ~(sData>>0)&1;
, i8 W2 N7 ^4 ?& |; H( J/ K! C - D2 = ~(sData>>1)&1;
4 R( }' }2 b: K1 Q# V7 i7 I: X - D3 = ~(sData>>2)&1;
6 _% h0 e* ` v* f9 Q- |4 z+ k - D4 = ~(sData>>3)&1;
% } C* q3 Z4 i- H - D5 = ~(sData>>4)&1;/ V, J Z3 d; v$ }0 E& |
- D6 = ~(sData>>5)&1;
5 _% w! g' R0 q+ o. J; m - int downD = D1+D2+D3+D4+D5+D6;4 P9 O! v: G) F8 e4 Y2 M0 @
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);/ Z x) o- C0 n' b- I* k- x
- if(downD == 0)
) o4 g# ~. p( D - {
, A- W# \9 m* t+ @% U - return 99;
+ A. u" G; _3 L% p d* w - }
! {+ h: k( c6 K1 g- B4 Q - else
! L! w3 A, n: W3 u! X5 r - {" }; G, p$ r& ~8 `: S# l$ u7 _
- return upD/downD;) A, ~* I/ q. E3 F# C4 S4 B
- }
. l7 k3 V' r, G - }( I1 v9 e q: I; T/ J: g9 u
5 t7 |0 I. M2 Q2 i2 [6 N- void moto(int Speed_L,int Speed_R)9 `. G! K! L1 p
- {
( j+ P# F% Z6 v/ m* g# s - Encoder_2.setMotorPwm(Speed_L);
6 ?" S6 q5 Y/ W7 b6 V' k - Encoder_1.setMotorPwm(-Speed_R);% _ l/ R4 h- U: _7 o$ b
- }
複製代碼 ( i) A* q) L0 U! b- ]
MeLineFollowerArray.cpp8 r$ [6 B( t4 e( R
- #include "MeLineFollowerArray.h"# }4 c( T& M% a4 q1 n- L
- 3 _) z& w' Q4 t( D; o2 M9 m# p. m
- #ifdef ME_PORT_DEFINED+ _, w% X5 q/ h
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)& e- p$ q( u2 ?5 v( ^3 m3 @' H' ]
- {: y/ z( f: W& n6 m+ E! N
- / F2 g; }7 D: t% M W, q
- }- a; }6 v% b1 A6 K3 p
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
" O( d. ^; \' F# [- X8 Y) w; l - {
k( ~: m# {: R8 E7 ?) ^# r - _DataPin = mePort[port].s2;
/ O {+ B6 {- p7 \+ O" `4 ]$ k6 { - pinMode(_DataPin, OUTPUT);
! G$ I: h1 P# f/ F6 f; N - digitalWrite(_DataPin, HIGH);- d. g9 A1 n: T, A- I
- }/ v! R- f+ q5 c' F
- #else // ME_PORT_DEFINED
) c5 h# S8 f8 b# Q1 t - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)" t6 q4 e, E/ ^* \
- {0 n! ^+ b+ W$ k& m! J; q# ^- X) p6 B
- _DataPin = pin;
3 `7 d* B; w+ ^1 p - pinMode(_DataPin, OUTPUT); * k( K0 D9 G* o# [9 x) |* T
- digitalWrite(_DataPin, HIGH);
5 P. D) } V% H: }" U' @5 a - }
- f- H# m; ~4 e X - #endif // ME_PORT_DEFINED
! B4 O/ c9 }5 e0 B& h" o3 e# n - 7 L( B5 T, p% @
- $ d/ h' T( `3 Y* T9 P; D
- void MeLineFollowerArray::setpin(uint8_t pin)
2 z7 j( W. `% F8 C* ] - {
5 P4 J1 U/ ^: I6 ` Y& U* ? - _DataPin = pin;
0 a: p" c) L7 j* E7 P - pinMode(_DataPin, OUTPUT);
: `) r9 ]( h# P' R# p - digitalWrite(_DataPin, HIGH);
4 X8 @7 X; k0 x# z# E - # Q) r; ^; `1 A/ n# `4 D
- #ifdef ME_PORT_DEFINED
5 K5 ] S, k; W% r1 B - s2 = pin;6 \+ q( W! |; F% H& N) Q8 c
- #endif
- a& _2 L7 ]( b% ]9 S - }* K0 }; z4 w- B7 S0 Q
- " D; H4 ~7 R5 p; t
- uint8_t MeLineFollowerArray::getValue()3 D2 f( b$ x" I7 y
- {
! G* a) P2 E' B# E - uint32_t LOW_level_read_time;
" p5 p: B) [- G" B - uint32_t HIGH_level_read_time;: I7 k- X- G: l& Y0 z
- uint32_t time_out_flag;
+ p& b" N9 u; q+ O; l0 [7 O. t - uint8_t Sensor_Data[3];0 _6 u- ~) J* U1 C7 d m. B
- static uint8_t old_data = 0xff;
9 n5 X7 H8 W0 x: ? - 2 P) }8 {# j$ s o
- pinMode(_DataPin, OUTPUT);
$ i+ U$ X# M3 `+ T. \5 u - digitalWrite(_DataPin, LOW);* y, @5 r0 B! v' B8 b
- delayMicroseconds(980);
1 H) d4 h9 F" l" N/ a5 d - digitalWrite(_DataPin, HIGH);
3 e9 E! {" [2 G3 ~2 C' S: K
) O, k/ M+ T" G' d2 V, I- pinMode(_DataPin, INPUT_PULLUP);
+ G' T# D' M# J* M$ Q- S - delayMicroseconds(10);
6 n9 t+ i$ Z: z0 l - , m% g# i. r3 ], u0 h) c3 W' e
- time_out_flag = millis();
' _2 [ V5 P* V - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 R2 e) s; N3 B8 L2 u- b - 0 Y" T) j' [% c1 e
- LOW_level_read_time = micros();# { C, h N( t+ C
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out, |9 {; j1 k) B+ t; G
- {7 m0 k! K8 d/ z) p3 @* `
- return 0xff;5 d$ S' J- p$ S- I/ C( {/ E) f
- }
: V) `- M& f8 U; ^ R7 n
S0 _- m' ^. H# _7 o- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, a: X6 M/ @- E/ H3 @4 Z6 _" j0 z
- 3 V( H2 D6 X3 x
- HIGH_level_read_time = micros();
+ ? C) }0 q( ?+ B; u7 C) s - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
u8 R! k* I9 i) r
; x* E6 p) B5 C( x3 q# J- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 w3 P4 Z7 X& e. g Q _) B
- {& b4 m4 }- m. Q: D; H7 |
- return 0xff;
5 y" T" |4 [$ e! m4 V) k" F3 d - }
8 d2 u% f. J! F+ O7 h! z/ ~/ q$ V
3 D0 u2 z& T9 c) O3 S/ b- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)); }5 `$ E+ ?8 b
- {; q; }( ~' p7 \4 J, z: p$ B3 o6 `# f
- return 0xff;8 v C: G4 k( D/ D
- }- I! h1 f+ x3 i9 F
4 D, W/ W- X2 H- f4 x- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ |% c) ?. x& j- ?! @' u
- LOW_level_read_time = micros();$ f5 r/ D$ Y6 V: D4 ^
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
0 \; W) i$ y9 n( J/ D5 {
+ i: Y3 ^: @: w) O8 \ D( t) l+ Z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
9 X2 o* E4 z, h) Z) K& z - {/ K2 p% T. n' E( Y5 y' j2 u
- return 0xff;
- T2 s0 s6 c/ c# z& | - }
+ e% T, L) W3 n K3 I
7 s( k# R9 G) ^4 w- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
' A& `+ ?) n+ r - {
, `7 m e! P# r" M: e - return 0xff;* Y% [: Q3 j0 L
- }
& n+ r; U4 J1 C& U c/ C# d( |$ h
0 C. A; u- h7 w, T' w- for(uint8_t k=0; k<3; k++)0 m( r- f$ z; n% ?$ ~7 B$ _. j) t
- {. {. E$ K! d( y: D* ?
- Sensor_Data[k] = 0x00;
) ^- V3 d$ _, J6 B a, o! c$ e
7 p. @* H/ x% O# F; T- for(uint8_t i=0;i<8;i++)
6 j; U5 K) Y- n - {
9 `9 [7 e: ?/ V- m( X. h - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
' o/ O# u p; _( C' r, s - HIGH_level_read_time = micros();
9 E* g1 F$ U# {" R+ g7 h3 _ - LOW_level_read_time = micros() - LOW_level_read_time;
4 D/ L5 L. d+ |' J1 m
( O; C) ]1 F; K# R, k+ t) I- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )+ I: f, S# o5 R5 m4 G
- {( M" ?6 E7 X) T1 ^) \+ a6 q
- return 0xff;5 n! o8 [. \6 f& e+ R3 t8 u9 n
- }
7 \ g9 v% V# v: W4 |) M3 s
% S% o' T- I9 \0 X2 C l$ r, o- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; v* u J+ M& f0 p4 E
- LOW_level_read_time = micros();
1 d$ |$ O& K) x' c+ d+ B9 ^ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level1 V( p' r- A( w; a
- 1 ?* f9 n, p; O) k: g
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
; v9 l8 Z7 `# b% J - {. a. b: P* y0 s
- Sensor_Data[k] |= (0x80 >> i);
' j! P3 C: Y, m( a - }; x! ^, k% ~0 b4 L6 S
- else if(HIGH_level_read_time >= 100)
d& N$ P @# f% C - {
$ ~5 E* H3 {: n# n% v* i1 z% E) ` - return 0xff;5 M# b4 ]. V$ V7 Y
- }; {; A2 _7 y# r$ `9 `% h6 _
/ r; s2 B, Q+ l( Z- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
+ ^$ q6 Q' m/ i+ v" h+ K ` - {. t5 `8 u: f2 W3 E
- return 0xff;
; M' a/ n6 ]- z d+ k1 q* i1 l8 s - }3 \- z- x+ b9 r L. ^
- }; M" O R% V. e. a7 t. E
- }
* l" @% M/ I1 _( Q' a [$ u0 R
* P$ ]3 x; I7 X' B4 P6 Y) k6 r- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level+ V2 R9 e$ K2 _. u
- HIGH_level_read_time = micros();
9 Z7 T- @- `2 d - LOW_level_read_time = micros() - LOW_level_read_time;7 Z8 |" \' ?+ {5 }5 G" @
( p5 Y- ~7 C2 c' m$ V* s- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )' J1 s/ x4 m$ p
- {
: v; Y1 Z7 }) _* W - return 0xff;& K/ K7 q# d0 T- d) D/ M8 T, F
- }/ J9 o( C! E" C' s2 @
- 2 S/ K$ w; \6 A! _
- pinMode(_DataPin, OUTPUT);& U) m+ T+ T: G+ c/ J
- digitalWrite(_DataPin, HIGH);" i8 Z9 d* U" w4 N- ^
$ c! p0 k7 j- ]6 G2 d$ G) W/ U- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))4 z* w: v8 V) R0 i: t
- {/ `1 c! y; o+ q" f
- old_data = Sensor_Data[0];
0 Z# Z" r9 v5 ^4 j - return Sensor_Data[0];
$ r, R7 F. T7 b/ A; G( R4 D ? - }
0 N' R" p" t, ]0 s - else
+ q! q$ T; F. @( R. E - {
/ N1 [ f0 C ?9 l; @ - return old_data;
" K+ w5 r! l( v+ t# \0 h - }
+ m8 z% A; y' C2 C$ c - }# {! W6 p7 o2 A) f P5 A6 J! F2 J
複製代碼
5 l I9 b% E4 d y. _ }- GMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ m4 a: ?' v/ h
1 w4 q% X+ H$ P, h* q; o, k |
|