|
( k. {$ s) a" V2 Z4 T
- #include <Arduino.h>7 {% j; u P8 O8 u9 ]( O
- #include <MeAuriga.h>9 z8 i2 {5 C- A: w7 S5 i
- #include "MeLineFollowerArray.h"
, q" H6 C7 ~' J# R' V, u$ X' } - & b0 A8 U2 i6 y% d3 `
- MeEncoderOnBoard Encoder_1(SLOT1);, a# R! L. _9 v2 \, r" [0 T5 O
- MeEncoderOnBoard Encoder_2(SLOT2);6 w% E8 B- b! B2 g, W5 `+ _
- MeLightSensor lightsensor_1(12);
. k8 I. }& |! d$ w - MeLightSensor lightsensor_2(11);; d f4 n. r8 `
- MeBuzzer buzzer;8 t( B8 K# A$ o+ U; v
- MeLineFollowerArray linefollower(PORT_6);
! v% @. [6 d s5 [" D - ' n; ^- O, g4 I/ D$ ~% Y1 h: @
- #define Error1 1
8 H/ _4 k5 ~, x. ?# s - #define Error2 2
! g9 ~1 ]& D/ F; m5 Z, r9 I& \ - #define Error3 3+ [. w" e3 e, }5 {& d, |5 x
% h& U2 f2 H- Q- #define Kp 155 A o( T: n& B. e
- #define Ki 0.152 p* S2 `& p+ ~% A
- #define Kd 0.033 m. j) h$ _. p2 k1 ~& G
- 4 P8 ^$ X q/ P' W2 d( {
- uint8_t sData;
: p: E( T% z$ ` [% z3 ^7 O - uint8_t D1;# C0 N! v+ R8 H" U# G! z
- uint8_t D2;+ i/ k4 p; ~8 Y! s. Y( U" f
- uint8_t D3;
2 c5 }! C5 c& C5 T- t: f - uint8_t D4;
3 U D4 }9 {0 _. o3 T+ c - uint8_t D5;
/ Y2 g6 v. g2 H3 t - uint8_t D6;: u' v( b9 z5 A* B* R6 L
3 p5 g* h1 u+ P3 R5 A# C2 c' s, ~- float previous_error = 0;
" r$ f# M8 D+ u. c - float integral = 0;
: g% D q$ n6 e) G# ?0 q: O3 h - float derivative = 0;
% R9 h& y/ h% H. S8 P - int Speed = 160;! y ?" K4 E7 G" x8 V+ d( z
- float output;( R( x( W* b' `: } V3 I) r
- ' u' f0 ?: D( i( J
- byte Left;8 l: _! h: p9 R2 }
- |. t) c5 O/ l) e
- void setup() ! X$ R' K- ]1 ~: P8 h
- {
, v! [* l2 I$ N# M2 ? - //Set PWM 8KHz
# ^7 P7 P3 J3 _: t, k - TCCR1A = _BV(WGM10);0 k2 q% t! c `/ o6 x; m7 c
- TCCR1B = _BV(CS11) | _BV(WGM12);
& g: u0 B3 i7 R/ {- B; F0 y/ k - TCCR2A = _BV(WGM21) | _BV(WGM20);, t3 B" y; I3 ~& I5 m/ o, ?0 G
- TCCR2B = _BV(CS21);
% c2 k/ s$ f C - Serial.begin(9600);$ P. Z: N$ M9 l4 q" |( P' Q3 j
- buzzer.setpin(45);
6 j5 P4 a0 G4 x: ?/ p/ O$ w$ @ - }" Z& A2 o; @' p" N1 k
- 3 X3 e4 h8 N# \& q! b, G0 w9 V
- void loop()
7 U+ d" h% y' y - {' b/ P9 {5 j8 N* ^
- while(!((lightsensor_1.read()) < (10)));
, ]! e% A2 w( G3 q! F: y - buzzer.tone(1047, 500);
6 s, z% b& I5 I b- U9 V' h - delay(500);
2 c2 g. o% N3 Q$ n - do6 p% A$ u3 D N! R
- {
' ~% I8 o3 k L& o7 J, y8 s - int Err = getErr();- ]6 L5 G3 T3 N8 }' J
- if(D1 == 1)
3 a" I1 |8 v, _- u/ r: W0 z - {: b% R- [5 S9 F# h' Z0 D
- Left = 1;
4 h4 B& x* i! a2 K9 { - }
2 F% o$ n% A, F9 G1 Y, b, V4 K0 G - if(D6 == 1)
$ B$ O5 [; q/ | - {( M" t* c2 ~ r* A6 y- d
- Left = 0;
; J1 J. t, H! k# Y - }
2 w6 _4 X/ L+ U$ m% T% } - if(Err == 99)
9 K! i. @' a, [& t - {
! A/ w" Y5 x; O6 {5 k8 z - if(Left == 1)
& t" L! A( h* f1 k - {
$ Z8 c8 k' Z$ I+ r' Q) @ - Speed -= 5;
: E& S3 K. x( W: [$ G3 z" Y - moto(0,Speed);
" X# H- C9 ?( V5 {- V. Q8 ^ - do4 f( R- p) n+ [4 _9 Y- }0 r( }
- {/ E9 N4 i, t" _
- Err = getErr();2 r0 h$ A6 M/ s7 s$ f4 @/ z9 h
- }while((D1+D6) == 0);
8 u6 c0 O8 H* j8 I5 L7 I - }
+ r D& a" s: i/ T0 I; v8 a - else
# x( ~4 ?/ b1 a/ L - {
+ ?3 H& T- k+ W; \0 i1 i - Speed -= 5;' L" K6 W+ C7 R y, f8 ]
- moto(Speed,0); 7 v6 S0 j- C- G- k$ y7 L0 u% L9 Q
- do
' n; v! b; Q) }5 W - {
& H" L# P3 c/ Y; v- p6 A" v - Err = getErr();) A6 V% H! K+ I6 e$ h; \
- }while((D1+D6) == 0);+ p" z E- d3 Z- f7 C7 z! [
- }7 y" [0 N) W# ?: s! W' b5 e
- }
* H k3 Q; \6 I4 D" o - else
- D, |" N4 n5 g - {4 _# h$ P7 b5 v, y
- if((Speed < 160) & (Err < 2)) Speed+=1; $ M/ z& @3 C3 k8 c
- if((Speed > 100) & (Err > 2)) Speed-=2;
2 }( Z3 D& t: `4 j - integral = integral + Err;
( e$ z- d* j: T4 T/ F8 H! b0 @ - derivative = Err - previous_error;* b' s% C2 m/ | ?2 d( s1 ]
- output = Kp*Err + Ki*integral + Kd*derivative;% r. q ]! S, d! Y/ Z
- moto(int(Speed-output),int(Speed+output));
+ h) V* d7 R+ W; n. q c1 B - previous_error = Err;' G, L ^! K7 A: I. ]0 F
- }
7 n6 n8 ]) n, g) [% @3 @3 X - }while(!((lightsensor_2.read()) < (10)));
! I% K" H( y7 B. Z - moto(0,0);
+ b! p3 l* S4 [6 V9 J - delay(500);
5 Y5 _2 i# d5 m. F. W# [5 L - buzzer.tone(262, 500);
1 n b3 X1 v- g% K6 D - }
% T3 x* @4 y; F7 d) Z% _ - ( L8 \3 \0 H+ t+ m1 Y
- int getErr()
0 p N+ r" _6 [2 [' l) L& b - {
3 r+ s4 ]# L( Y$ x4 T3 K p - sData = linefollower.getValue();
! V8 ~" p" O# j4 H& M( ] - D1 = ~(sData>>0)&1;
* y. A: r4 ^! \0 Z! i$ w6 U! F) ]! \ - D2 = ~(sData>>1)&1;
' w' P* v. F! w @, X - D3 = ~(sData>>2)&1;
* U' L# N' Y3 J8 H& Z - D4 = ~(sData>>3)&1;% w3 w1 m: F3 i
- D5 = ~(sData>>4)&1;2 W4 Q- b) r! ^$ P
- D6 = ~(sData>>5)&1;+ g( a& E! \; e) s
- int downD = D1+D2+D3+D4+D5+D6;2 u& ^: e7 B3 B' V! A7 R! ?
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);6 Q3 i. H! I+ L) T: s
- if(downD == 0)9 {' ? ^: Y' x$ K0 M5 X
- {8 l' V1 X# p9 k! w( @! _% A
- return 99;
6 g9 m% X6 c6 W3 b - }% N9 z+ w& _& p2 u. ?2 `4 u
- else
* Q7 {: u% {( q0 r8 c - {
; O6 ^3 N$ ?$ V' i - return upD/downD;9 d. f0 Z B# B7 |2 r
- }+ e1 c$ H. ~& ]6 g( |
- }
{1 g: b! \; h F* y8 K" A - + s1 C8 F" E, D4 T% H4 F7 P1 o
- void moto(int Speed_L,int Speed_R)
! }, u0 Q+ @$ E$ T# m% \ - {4 ]- N6 W6 K0 j+ O# C4 n7 R6 N+ @
- Encoder_2.setMotorPwm(Speed_L);0 J- A. ]* C, E% s9 @
- Encoder_1.setMotorPwm(-Speed_R);
v/ O7 K' f0 F' L - }
複製代碼 , U" D; H! _9 _* D' ^$ Y
MeLineFollowerArray.cpp9 x- _+ b0 ]% [7 s
- #include "MeLineFollowerArray.h"
8 Z& X' n9 u; d3 |& B1 }# { - . `- R) ]1 T) t9 e( A1 T: K6 P/ C
- #ifdef ME_PORT_DEFINED
) n9 M5 k8 c; N+ k2 b2 W: h - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
) ?7 p6 s% b; c6 k8 A$ e - {$ O t% n# e7 f, |; ]( U
- * E. s3 T; ?% g$ I* w) v
- }) p/ h* U! `" E) o
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port); A; [5 |' m/ K) |
- {
" `$ y- x( X! ~! s% Y- b - _DataPin = mePort[port].s2;2 n) g- o' p, U" A9 r& n; f+ F* F* K
- pinMode(_DataPin, OUTPUT); ) O5 _6 w C% r( P; s
- digitalWrite(_DataPin, HIGH);
, G. ?0 F$ A* y( b ~ - }, z: N3 u0 N* V( O6 Q. y/ J1 O8 O1 z: O
- #else // ME_PORT_DEFINED
# e% m+ `0 m5 c - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
. g! G7 L7 s- v - {7 r3 V2 K8 q, a! ]0 V4 M( l" |
- _DataPin = pin;0 m; Y2 `' h/ ^
- pinMode(_DataPin, OUTPUT); T8 \# @* L8 D9 d3 G
- digitalWrite(_DataPin, HIGH);9 u3 ]7 C: ^- F: {4 H; ]
- }
5 E- H6 W! W% y8 w; l - #endif // ME_PORT_DEFINED
9 R8 B4 _; {& b# ]" D
' V8 X2 ]; v+ h
4 B$ ~% w- S3 G$ x0 T9 t, J& x- void MeLineFollowerArray::setpin(uint8_t pin)6 P1 N" l0 f; v/ I
- {
9 ~; N; U6 T0 V% X, T2 B' o- L - _DataPin = pin;
& V: C4 M0 I7 g6 V! U - pinMode(_DataPin, OUTPUT); # b& b: y0 l# L$ V, w5 | [
- digitalWrite(_DataPin, HIGH);, F& b! S. C, V% I' z" W4 K
- % S$ \( b; `& |2 s7 @, J3 O
- #ifdef ME_PORT_DEFINED. v; x/ B) f& B# o6 ]
- s2 = pin;
! ]1 @2 P; {- h1 U. A - #endif
! n m4 J+ Q2 L. v3 P6 L( N! t! A; d - }$ I, @3 a2 B" C, J
" j2 N% y+ q" k: f* H7 g+ _- uint8_t MeLineFollowerArray::getValue()
7 r3 I0 k& C6 o$ n0 p$ J1 j - {+ v% i l3 n4 n. Z
- uint32_t LOW_level_read_time;
5 W# g5 q4 ^0 B# R, U6 f/ Q. Y1 j0 m - uint32_t HIGH_level_read_time;4 A- l/ J- b0 Z. k) L
- uint32_t time_out_flag;
* L$ `6 i% h$ Y' O, P - uint8_t Sensor_Data[3];
, K9 @% [. O7 J6 r8 c; M% P) G2 N+ `6 K - static uint8_t old_data = 0xff;7 ?) s. A, f# z/ `" d4 }" j
- 7 W4 B0 \! M, ]% k- ?8 Q. W
- pinMode(_DataPin, OUTPUT);9 ?0 c! O3 i% ^0 Q+ t
- digitalWrite(_DataPin, LOW);
; L0 O. i; W8 D0 k: P* A1 n - delayMicroseconds(980);5 j2 L' Q t* Z6 F _5 O3 K5 X
- digitalWrite(_DataPin, HIGH);
. M* g) l& e! V9 m" Z1 a& ^ - / F0 N0 U9 Q _# T0 }! m2 {+ a
- pinMode(_DataPin, INPUT_PULLUP);
' I& M9 m+ [/ k - delayMicroseconds(10);! C ~4 R! o% A9 M- {7 s7 e n
- % ?' W& W2 o& m5 m" R; M, R
- time_out_flag = millis();3 q c% @ p2 }
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 I# V5 k6 X; `+ I0 |
- 3 ^. r0 G* w4 M% y( \9 O$ c$ ~# F/ V
- LOW_level_read_time = micros();: i; ]7 C) V2 h! o4 z6 \* A
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out4 D) l7 P7 ^. A& n1 N
- {
* r& W0 y# i9 u+ _$ @9 c& B6 g - return 0xff;7 ], t/ O6 i0 S
- }8 j6 C' {) r: L7 _5 [0 ~
- ' E+ H5 ^/ ~* k- m
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* t7 e/ O) n! X, n! r1 o
- & u, |8 e% ?7 d# n
- HIGH_level_read_time = micros();
' i( h/ a, @5 y* N - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level: D$ w4 A5 q! _! y: G
- " a9 {, a/ J9 J1 B; t \+ s0 |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ x+ @: c9 B, h; b1 p
- {4 o) S* L% t- I- Q! w
- return 0xff;
: N8 t! a8 X) D N - }( c) X( s) q6 W% u" M9 A8 i4 H
- 3 J4 d+ o8 [4 Y4 e
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
6 e) z% W6 u# G' B; C9 \ - {7 B8 n& e) I A A7 i* a
- return 0xff;
, ~, H3 k- Z& x0 s) X- G - }+ l6 V$ `9 S. n8 r3 s% F
- 1 ?/ G: P7 H/ j, r; z ]& U
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 D4 D. V5 j2 ?3 U* m% V+ }2 e5 X: f - LOW_level_read_time = micros();1 n; Z# a2 d8 W) G) y8 }# ] s% ^( `
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level# l7 ~7 t9 s4 | K8 y+ ?# V5 x
- 5 [3 C+ {# j7 p% d( R: |) }
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
2 v) M( |: I4 L9 C8 f! { - {
$ _) U7 A& {: J, z5 |6 ? - return 0xff;1 ]" I, z0 G3 k/ V" h3 N
- }
( V- l$ p& |! Y1 o
( l- y; Q! K# M f4 k& u! Q* e3 p- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
+ H7 ~% f4 O) ]9 D - {6 R( G9 ?. ?$ w7 B$ y9 ?
- return 0xff;
. r& }0 o3 P3 Z8 E - }: w/ X, W6 P" @1 {% H
3 U% a8 h" _; j) M- for(uint8_t k=0; k<3; k++)
* e6 N8 X: Y0 \8 y1 ^ T( Y" I3 u/ d - {$ C. f Y5 W2 w9 d+ f
- Sensor_Data[k] = 0x00;0 s+ O% U1 x8 G3 g; S
6 h* t4 k) q; L$ J$ @0 z5 ~: L- for(uint8_t i=0;i<8;i++)* m* q/ j2 e: ?( _7 K- E% M$ C% R
- {
5 O y& P! \* Z% D) B; q! J$ E& I - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
5 j6 S4 s N. T% B$ X; t2 g - HIGH_level_read_time = micros();# r( L5 ^! Y$ _0 n. z& ~6 Q3 g5 W4 s
- LOW_level_read_time = micros() - LOW_level_read_time;
5 e4 A9 g* G0 S7 F$ }% V4 m
' c- P* G- p9 Y- k& V- v- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 J' n' I A3 |' b - {
% m8 u% w( F; |0 o5 A - return 0xff;3 z: G( H9 a8 r# ~& p& F' | e7 u
- }0 m% I9 l+ Z7 d5 [/ v, f ?
6 b# Y* ~( R* |/ c5 g8 E- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
0 r. \$ T2 F* r8 b; U! m& z; @ - LOW_level_read_time = micros();
9 g. x5 d3 k( K" M7 g2 p; _6 i - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
- q; Z, r+ t) k) R - ' C# d% T0 F* a9 y
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 k% j8 o* J& }' T" B! R1 ^5 V
- {' h; P6 k/ a8 x0 }" S% U2 ?
- Sensor_Data[k] |= (0x80 >> i);
) N+ X6 F/ {3 B+ i& q, Z! \ - }
1 d: w" C B3 A/ u4 A - else if(HIGH_level_read_time >= 100)
) b1 k. {0 k. E0 |1 B8 p6 A - {
* e) X) m2 @% s f1 D - return 0xff;) |% x O7 H$ s- s
- }, G$ [$ K! H, D3 `
3 \0 A) g v* r+ Q( M! G5 D- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
! L2 |- `5 q! p+ F- f( E - {
7 |0 e0 a S7 @. @; ]' R8 i - return 0xff;
. N' M, g1 A/ W - }. t) x/ b- ]' u$ d- Q( ~2 v/ _
- }1 \* H5 w5 x8 e5 M9 j* q) Z
- }
- Z3 N# e0 E4 z* G. w
* u+ Y. E. e, U7 P5 k4 p6 ^- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
3 N1 x6 a0 U. `- M" ^5 t' r3 z - HIGH_level_read_time = micros();9 x' b0 c( b0 G$ B0 A
- LOW_level_read_time = micros() - LOW_level_read_time;* s) b5 [0 E6 n
- - Q, L9 r6 q8 \
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
+ w' }6 g9 _! I7 e0 x - {6 U+ S( a; b9 [& E+ f; H* |
- return 0xff;% j- D7 ]$ x, F
- }
9 O0 { `' q2 j - " Z+ w Z" `) V4 ^9 N( n
- pinMode(_DataPin, OUTPUT);
2 v& ]+ B8 [6 e9 [7 f - digitalWrite(_DataPin, HIGH);
% ^3 E( M* s1 l$ T M2 F+ E) N( U# I
! R9 b0 k. N, L3 C3 ~- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
1 G1 q5 Y- u( r; G( I! R - {; y8 \2 G! W0 E; K4 t
- old_data = Sensor_Data[0];
8 T" s- X: n3 A) d5 E& Y - return Sensor_Data[0];
: m$ n" c' J; s' l& G1 s - }5 ^' f" w5 \$ n. ^. ]" `+ \1 v
- else) h D* I% H, ~! a
- {
9 }+ d( b; D! ^. w9 A/ Y6 d - return old_data;
- y- ^+ h3 s2 [3 ~0 _1 O# | - }/ N% L* y% _- n8 o
- }+ m1 c* s/ ~5 r
複製代碼
2 z# ^+ l; E0 @# J9 m& [MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
7 a& P8 V# g8 {) E3 Y: I, s; N+ d
/ n% Z z$ x- H6 U! m) w |
|