|
|
1 }9 A8 ~1 j; c- #include <Arduino.h>9 \7 V" i' C* K4 p
- #include <MeAuriga.h>6 H0 ?9 F9 Q$ a
- #include "MeLineFollowerArray.h"+ X) y, A8 W! d2 ?! Y/ w
- ( N: d* n6 ~* _, A% `
- MeEncoderOnBoard Encoder_1(SLOT1);
# N$ q) f: ]* X, M- n, h) P - MeEncoderOnBoard Encoder_2(SLOT2);
& @: k1 R1 i$ e1 Y) n' v- N - MeLightSensor lightsensor_1(12);
2 `5 D" m( {- \6 { - MeLightSensor lightsensor_2(11);) q! |3 s4 F. \0 E
- MeBuzzer buzzer;
5 j) E2 t; T. g6 d" l( N - MeLineFollowerArray linefollower(PORT_6);; w% b, a: |" e+ ^8 f @0 s k* ?
$ a) B N% s/ F# M- #define Error1 1
/ ]6 W* {8 s! y3 l5 J - #define Error2 2* g/ _+ o' ], g+ z; _
- #define Error3 3
! O& K+ z0 ^8 J* E
0 h& J. z& f# j+ K% G- #define Kp 15
; h7 V9 O d* X- y% Q, r - #define Ki 0.15" C" T8 E( n1 i
- #define Kd 0.03
: A/ R* a$ \- n7 b& w' Q+ i. I7 T - $ D3 `" y5 p" D
- uint8_t sData;3 i' z0 p( ]% H& z) B, o% L0 }
- uint8_t D1;
; ?, i: [0 {% h; }. R. L: V' { - uint8_t D2;
" x r8 m; @0 G2 S - uint8_t D3;: c, ~& L) K n, r4 x5 K
- uint8_t D4;
3 h* E9 \! H8 U6 H* D - uint8_t D5;! W( f' C1 l4 H+ E- O
- uint8_t D6;! i, U5 m3 X) J: B# }$ Y
3 W P, l3 I5 g# h2 b9 G- float previous_error = 0;3 L1 ^) x- G. J1 _4 R
- float integral = 0;3 n+ n9 s8 W1 e9 T0 G% d9 N
- float derivative = 0;0 t' w6 ~# s+ U
- int Speed = 160;
1 V" M. t3 M) t - float output;/ d# `' g" y1 K8 f, K/ z
; l5 Q2 G# `4 b- byte Left;0 r4 z& z0 p3 C b# f' ^7 N5 x: _# X
- ; p( p, ]; T7 A9 s
- void setup() ' i/ v E) b# b* @
- {
+ B& f9 y0 `0 G5 A' P( V - //Set PWM 8KHz
- S8 \! |' D K - TCCR1A = _BV(WGM10);
" K1 n9 R$ O) O% l( g - TCCR1B = _BV(CS11) | _BV(WGM12);+ q6 \9 C9 j3 t9 H4 O4 r1 x3 w( c
- TCCR2A = _BV(WGM21) | _BV(WGM20);2 ]2 G# {7 {) u$ Z) F0 s
- TCCR2B = _BV(CS21);1 G: o/ h) g) J) g0 }
- Serial.begin(9600);
1 I8 j2 Q0 x0 E3 j - buzzer.setpin(45);3 |% b8 J1 N8 [% r4 A# b
- }
) n ~- I. f) }+ u& ` - ! a3 Q( E [2 ^. q1 [) t) q
- void loop()/ n) T8 c$ Z2 a6 Z; F
- {2 w9 C/ g2 ^1 z. W( a& k4 j6 X8 C; y5 }
- while(!((lightsensor_1.read()) < (10)));
9 q* \+ Y, p% P2 r5 A$ d, Q - buzzer.tone(1047, 500);
8 e: ?! } S, `+ w - delay(500);# `! P7 m. E) K5 P9 D
- do, G& v! E. b+ q- P# h2 q
- {
0 f" u" ^2 _% Y( I - int Err = getErr();
0 o( _; v. k: ~; |+ ~ - if(D1 == 1)% f/ R; D' ?- r3 b. Y2 n
- {+ H: Y' c' L/ `- J. S
- Left = 1;
3 w) f' _& _1 V3 Y, T: V. } - }- B( H2 E- l' y. b+ }4 ]. S/ x$ C- C
- if(D6 == 1)- k& c9 [# m+ E. Y
- {
* Q% z( n& o1 h1 O - Left = 0;; H! g; w8 }( B6 z4 Y
- }
- s+ `% _& H$ r5 B3 ` - if(Err == 99)( c$ X e( }) U W
- {
( O5 E _0 J# a' U7 Y" ^$ L* E - if(Left == 1)
+ @, o) S$ n3 _4 N - {
0 e& ?& `9 M$ |4 @& Y - Speed -= 5;
- U% v2 l: l" s- f3 q - moto(0,Speed);1 L' r8 V, _5 n1 ?% ?( X
- do% u+ T! q8 K( G7 |- l' X
- {# i; u! F* [( }* X* Z* Q5 S
- Err = getErr();. H3 E! ^" J/ @3 |' \" o# Q+ N' s
- }while((D1+D6) == 0);- N+ X& B0 Z- |3 j! z2 G
- }
: J! B4 Z' M3 d: V3 ^ - else
1 K) C5 u5 ~, ]5 X - {! w0 e. ^9 ?( [4 h8 p: C
- Speed -= 5;3 l3 e* {7 T! G1 F/ _
- moto(Speed,0);
) h/ Z; o" B& v! H# X1 e5 H2 \ - do
1 G4 }) }8 ^- A/ j - {
, P# |! [2 R6 j' z K m - Err = getErr();2 E6 F. Q i5 c+ [: V
- }while((D1+D6) == 0);: g& ? H" S' j( e+ d( Y0 l
- }
; n N. S: x7 ^3 M8 f7 a5 e - }
) h( M- K9 Q; S# [ - else
7 Y* D- z) w4 j - {
- P+ T% r( g- o% i; a - if((Speed < 160) & (Err < 2)) Speed+=1; ) J3 f0 c' }1 g- T: M$ T ~% z# v
- if((Speed > 100) & (Err > 2)) Speed-=2; {7 Q( X$ Y& z. O8 w) H8 g' O/ A
- integral = integral + Err;
( t/ J) I1 H5 e/ W7 n! p4 b - derivative = Err - previous_error;
1 E8 `1 e% i2 e, k% d - output = Kp*Err + Ki*integral + Kd*derivative;5 E, y4 O, X' u% L. j9 i) w8 e
- moto(int(Speed-output),int(Speed+output));% E2 ?9 w% _ k
- previous_error = Err;
& |4 Y1 G6 h- @1 F9 R/ P w2 O1 e7 w0 Y - }* W# \( s. @* V) n
- }while(!((lightsensor_2.read()) < (10)));+ p) Z" a0 C$ ^
- moto(0,0);# ?6 @/ ~: }* Z D2 t# C
- delay(500); _5 l A- _& |5 a
- buzzer.tone(262, 500);) m3 {( L" U' M7 O( ]3 d
- }. a+ }) ^9 D+ Y" G5 Q& y3 A3 l
- " J \; @. O& g. ?' Q
- int getErr()
' X" ]) x- i6 r. ~) Q: u& _ - { 0 i0 }8 x: v% c" K) E% ^3 o& d: M
- sData = linefollower.getValue();
: V! _& ?% Z2 ]7 u, Y - D1 = ~(sData>>0)&1;
! A8 l) \4 O0 Z# K3 H - D2 = ~(sData>>1)&1;# _0 g z% q' L3 ~9 o8 v4 K- }2 t
- D3 = ~(sData>>2)&1;
, [3 c) T! p | - D4 = ~(sData>>3)&1;( A$ ]8 b7 K. R$ [& p1 b
- D5 = ~(sData>>4)&1;
7 y( J2 r# U& \$ y, K9 Q8 x - D6 = ~(sData>>5)&1;
; t3 E Q& m; n - int downD = D1+D2+D3+D4+D5+D6;
# |2 \% u% b- @, D4 C# x- x - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);! l( x, u- v- Y
- if(downD == 0)
9 e; P0 s' L! L- G - {
& J" Q7 d7 U+ h4 I7 a6 a( A7 q. g5 x - return 99;
X4 I1 F: N" [ - }
) {$ V* k4 j+ C - else9 `+ _! Q4 @; p( \
- {
2 w* A- w$ h& e4 k7 n - return upD/downD;
8 Z! j1 H1 ^& s6 W1 x7 ` - }
+ Q( B' |: u0 Q- t R - }
4 O% l; |' O$ T+ S
. J' ^3 X4 v: \( l% Z2 r- void moto(int Speed_L,int Speed_R)
4 u+ |/ n0 w- [: z+ a# V - {
7 u% `. y- A! u% J/ X: N' b - Encoder_2.setMotorPwm(Speed_L);+ g ^% B2 h9 N
- Encoder_1.setMotorPwm(-Speed_R);
8 B! F: \2 g2 ^/ { - }
複製代碼 % ]& C F; C2 ]
MeLineFollowerArray.cpp
4 u4 Y8 v9 w+ ^( `0 A5 F- #include "MeLineFollowerArray.h"
+ M( K4 R7 a: Z: ~
# Y- e- A3 J! Y2 H- #ifdef ME_PORT_DEFINED
M7 Q. p& |: O1 R' C! J$ q - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
& s* {" Q5 b' J+ T8 S/ C - {; W3 l' }2 r+ F. Z: \
- $ @- [$ E4 j0 [. k O
- }, `5 {' u7 }) b
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
+ L+ A3 }8 M8 \: n8 E$ Z2 A1 @ - {
" m& e' h, h* ?& s+ I$ K X - _DataPin = mePort[port].s2;
8 ?$ P1 u0 [5 r; K* A8 z6 n4 S - pinMode(_DataPin, OUTPUT);
: N' p& M- q7 G/ e - digitalWrite(_DataPin, HIGH);
/ ~% r8 M! a; c% Z/ f7 m% z1 w - }6 L- L2 y% Z J6 m) W
- #else // ME_PORT_DEFINED
+ z; Z( r$ Z. W - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)! i! r6 ^* w8 I) S# X" \9 J/ H
- {1 B2 I* H, P* {' O& A( `' K! C
- _DataPin = pin;
" O, L* p: I$ b1 p+ N - pinMode(_DataPin, OUTPUT);
9 {; [ x2 D2 U6 w% ^ - digitalWrite(_DataPin, HIGH);
( n, A) _# U4 u - }' }( M8 v. T, B4 V) D
- #endif // ME_PORT_DEFINED. Y& Q. e. f# T9 I1 l
5 o" H z; }8 Z; v2 b
3 Z3 z& Z1 o# _/ ]- d* n2 x( G( B- void MeLineFollowerArray::setpin(uint8_t pin)
* K7 @+ p1 m( L: ? - {8 }. T+ ^+ R; K6 }; n$ k& c9 l
- _DataPin = pin;- N6 ~0 ^' I2 |+ J" L& G6 H
- pinMode(_DataPin, OUTPUT);
$ B, i; A+ } c* M( @; S - digitalWrite(_DataPin, HIGH);
! ^% V% u- _3 M( ^ - ) ?' U/ y3 T+ l& X9 f" s% S
- #ifdef ME_PORT_DEFINED6 ]# W5 i* d+ O4 R
- s2 = pin;$ A+ g" ^/ b. P6 v: A" p! f: l; V
- #endif
4 p/ o' D& L# c* b - }
; Z* ^1 O/ u/ P& ]" J% ^1 \* j - ( }4 K, W. R- O! u# Z( G
- uint8_t MeLineFollowerArray::getValue()
' t/ H# G8 C0 h7 u$ G# Q - {
# c$ n% g( U+ _ - uint32_t LOW_level_read_time;
; p9 J9 o+ P# t: D% T# n. k - uint32_t HIGH_level_read_time;
# \* l; [* D9 K& J* T( g& \+ w6 c - uint32_t time_out_flag;
$ c# J% ^5 l# y/ N2 v$ l: m - uint8_t Sensor_Data[3];$ ?% r8 E+ L5 \- H1 H g( H: M
- static uint8_t old_data = 0xff;% e5 M; w% e' a. z" y- v
- 5 {, w0 y$ y& n, x
- pinMode(_DataPin, OUTPUT);/ r7 K: E8 S( H+ b- V
- digitalWrite(_DataPin, LOW);) u% U9 M7 i" w0 n3 C- o4 `1 M4 W* B
- delayMicroseconds(980);& `5 z9 o& ?& ]# x" j- X$ M. R) z7 K
- digitalWrite(_DataPin, HIGH);
. d/ W3 W3 h: g( `7 ~+ b
, r, N) k$ m' j7 R( u- pinMode(_DataPin, INPUT_PULLUP);
" x4 u0 Y0 M* T, L$ Y m5 r1 } - delayMicroseconds(10);
+ j- _2 ` L% U/ [( H( c - ' P: L; G! S7 z+ a/ Q* b5 {: ~
- time_out_flag = millis();/ s( J/ v9 d" x5 i1 j0 W
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- _5 ]: q! a7 N$ l) K: s" n9 n - : i3 R( e" {! `. A1 ]$ h8 o
- LOW_level_read_time = micros();& l. C9 G( r- ^9 w- k$ T
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 K' ?9 ]$ T* Y" g
- {+ S6 T6 e2 F9 P( e. a! s0 H* G
- return 0xff;
7 {9 R: j: w' n - }
) g8 G3 ]8 ~4 l. U+ t# ]) d4 M
3 X: Z! a3 k" H o9 t1 j- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 M$ Q0 C6 M; ?. I0 u9 i
4 o# t8 S2 D, z6 s- HIGH_level_read_time = micros();2 T+ L4 ~; z) M3 d( @
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
$ O# j: x; k( V( |3 m3 p
# Y# k( h" z1 I- ]+ ~- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 _% z3 R- R- t - {" k" w0 Q, M) N7 s% c$ d) J
- return 0xff;
h/ _, G# t* C8 Q$ ^ - }% r- g7 h" d# o4 H
- 4 @7 u0 _ A) Y
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))9 g- z( f$ \ f. u% Q: K3 a5 R% n! P
- {
. m; \) `" f. @( m8 L. F - return 0xff;
8 M) [; G) M: A( H# X" H - }
* z1 C, `: m$ \, K3 m# g1 m
/ M; c' R4 ?0 L* Y+ C' H2 ^! A/ { `- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* K! }" a! p6 X8 e ] E
- LOW_level_read_time = micros();
/ u$ [- Y5 D Q' q8 P - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
" t' ^$ t9 d* x3 N- Q5 e) M$ B! T+ t - " _2 S$ [; T1 J' Z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 Y* @% p% ] y) j. _* S - {
9 j3 V. X8 o, X+ U* \ - return 0xff;, G" X7 U) x: t; z) u' k
- }+ K/ U9 N# k# E+ ~% _7 {4 m
- , |, _9 c! X+ y% h4 l
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
" |5 _* A5 h2 B9 \8 w9 Z/ E' Q - {
9 A& w n% K _# K9 }! j6 | - return 0xff;
: N3 H. J* c. X) |- P5 z0 R - }
+ m4 H, a- {- s# d - ! h) g6 `2 C i* o
- for(uint8_t k=0; k<3; k++)" m4 H+ N2 `5 O3 y( I
- {9 B S: O" m3 `7 j: a6 V
- Sensor_Data[k] = 0x00;; @& b. i4 k( V6 @
- - d: `3 I7 F A, b& c3 w5 G/ e% q8 t
- for(uint8_t i=0;i<8;i++)9 F! W9 j" g! e, _7 h
- {
4 H( A1 i" X4 |" v( o8 b+ D - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level0 ?* j# S C( O& u6 m7 i) }9 K
- HIGH_level_read_time = micros();
v2 h! N O- x1 S - LOW_level_read_time = micros() - LOW_level_read_time;0 O$ R: V' q& h& C. v; h( h
- ! b z/ _, ~. D$ V: V" `7 `2 Q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )! ^* n1 A$ ?. F" t2 L% ^! ~
- {: Z: N) H; I7 U
- return 0xff;" {* Y/ \1 W; h% n
- }
" t) N" }- Z4 ~( @7 \ - : w3 r. R$ L0 Q( ~8 X: @8 _
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ v' ^% V/ q' D4 T( c, ^8 {
- LOW_level_read_time = micros();
! W) X9 Q. j& n3 A - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
% Z% ~! s6 ]2 w5 j% o
- [! B& B* t; b- v- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1: V/ |; n4 S' ^* Z2 X0 o t6 L! M
- {0 j5 H" E; T6 a, p0 b
- Sensor_Data[k] |= (0x80 >> i);% P1 |) p# h. e* d" T4 g
- }
Y+ N0 R. _3 @4 Q - else if(HIGH_level_read_time >= 100)
, ` i/ i8 O5 o4 m, T - {
# T$ K5 t, d5 A# l3 ^' p4 i - return 0xff;
3 a& e% I7 U' J$ P: D - }6 O, d; m: n/ m- C
- " _9 V O" k& |2 f, E4 p) Y$ h
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)5 {# D# i' X3 A" g1 J+ @, K. J' x
- {2 H7 l" n2 d3 c s" ~& q0 B
- return 0xff;
$ c, r {2 i4 } - }: n$ f( t9 V. B& n2 O
- }, w9 W, B0 ]4 L$ K' s- s
- }; T* T2 Q/ m& k6 }. E+ G- T0 |* K
- : S. F' {3 I. O! E m% P+ B) x4 U6 g- C
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level2 }7 k( [$ p' O. }$ a
- HIGH_level_read_time = micros();' t% X0 ]- D! J' |, r( h
- LOW_level_read_time = micros() - LOW_level_read_time;
& s, n# l2 R6 A5 i+ R# ^4 ^
5 }$ R8 ~; R: ?% @+ P; p: L3 f- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )- G- k0 Z5 w" B, E% [' P
- {5 D# J5 r9 |- H- {
- return 0xff;
. T% f# l- _, g. ~4 _. h/ ` - }) J% i$ L r; Q
- * H5 w( ?* B) o; j) r8 u
- pinMode(_DataPin, OUTPUT);( u) v4 R. u) C) k" M+ y. @
- digitalWrite(_DataPin, HIGH); ?, n9 m9 K2 x3 |9 B+ m
" F0 e: p" A- s1 c- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))1 v2 j/ c8 }" m+ E
- {+ ^! b) U6 c) k/ e: N6 g
- old_data = Sensor_Data[0];0 M3 `+ c# R: @; ]
- return Sensor_Data[0];
1 u$ J4 H, U9 `7 L$ a+ r. q# v - }
! g5 I# F) E1 K$ V, ^& E& m; T7 Z - else
9 S! N" Z1 q9 n+ T - {6 a2 x; j: I5 R& F: V+ w- d% d# x0 F
- return old_data;- }/ u% w |; u' }
- }
: S1 J u, c' f, A; { - }
' n. E% p8 |+ Q' d( D9 H! L! K t& H
複製代碼
# L. Y9 S+ I5 G$ R9 x+ Q; F' g* Q% vMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' {0 e L5 o9 L, {1 W
: [: Q; c. x* r; y |
|