|
|
; C! M1 m3 m( [3 h) U Z. ?7 c- #include <Arduino.h>) U- w6 G9 w9 ?4 f. M
- #include <MeAuriga.h> k. t- C$ t5 V5 e$ t
- #include "MeLineFollowerArray.h": N u# _4 h. T( ^4 `
4 _" K4 B* \: [8 F8 Q# d- MeEncoderOnBoard Encoder_1(SLOT1);5 i; C: X- `5 D2 c7 o$ S6 @; |
- MeEncoderOnBoard Encoder_2(SLOT2);3 y9 |- }+ T. \/ B4 k$ w% [
- MeLightSensor lightsensor_1(12);' l8 L- p3 K- Y& N7 D
- MeLightSensor lightsensor_2(11);, Z/ o( J0 U# c0 z$ J; ?
- MeBuzzer buzzer;
4 g" N6 Y1 @+ X- ] - MeLineFollowerArray linefollower(PORT_6);
0 H: H( E: w7 A
. I! Y9 x& v M8 |/ v+ c- #define Error1 1! k* `* |, V( c! s4 H! K' H
- #define Error2 29 G+ B+ Y& v: Z. i2 I9 y
- #define Error3 3- E, L' }+ P9 [5 ?
0 y4 z" G" F4 A3 d& f3 [. V- #define Kp 15
$ q: i+ |" i; F: B1 R- M7 F - #define Ki 0.15/ e# X/ G- E% @# N* T
- #define Kd 0.03
3 s* P- O& |* p7 |$ j7 A5 d
0 O! Y$ O1 e7 u: i0 q5 }- uint8_t sData;
7 w+ ~* d+ i3 i1 w& T9 I$ h - uint8_t D1;
+ ]# c& V4 O4 f+ a% o* } - uint8_t D2;: Q( Z; {/ b7 S+ l; K: k& a {! k
- uint8_t D3;2 q6 c) P5 K, E& r
- uint8_t D4;
- E6 P9 M/ l$ U, c5 H6 H9 y2 \ - uint8_t D5;
1 z8 {7 D4 w0 q - uint8_t D6;7 K# g4 Y( T3 i/ G6 h/ k
1 ?9 b7 @. Y1 A/ t5 R- float previous_error = 0;
2 n1 g2 e+ E* g* ]6 o6 ~ - float integral = 0;
0 b% @. }3 m7 `. [& z. a F - float derivative = 0;2 l$ T2 @$ [8 i! C( k; [
- int Speed = 160;# U% {' w, H1 x( X2 Y
- float output;
/ J8 d$ A2 [, S/ H& y1 \3 V
( J- a' a- Z/ \+ o- f7 ?- byte Left;
! d/ s$ m' r, n- C
! ^) \3 u+ S" ^1 k) P, f- void setup() 5 _# q! t, R, [* r, b
- {
* w- i, C% J' [4 V/ T" @ - //Set PWM 8KHz
% s Y$ P7 W+ L5 g: h - TCCR1A = _BV(WGM10);1 I; e& `9 S; y2 U4 e
- TCCR1B = _BV(CS11) | _BV(WGM12);
- P1 a6 ?- |8 q - TCCR2A = _BV(WGM21) | _BV(WGM20);
, u% L/ C7 k+ k - TCCR2B = _BV(CS21);0 E: D9 b# G& Y7 k6 t& n
- Serial.begin(9600);
* Z/ |4 W! y3 D! | - buzzer.setpin(45);
5 i: G6 ]7 j0 _; @2 ` - }( ]0 ]( ]8 b0 r, _0 G0 \+ \, ~
- : ~0 R4 r* v; \+ E* \
- void loop()) n; {$ ~7 n0 M" J3 k# T0 o! |/ {" ^
- {4 v, B q7 \$ p9 o( l" x+ D- b
- while(!((lightsensor_1.read()) < (10)));
5 r6 r) L) [0 A - buzzer.tone(1047, 500);) t: D& x) E1 }7 A# p+ e' m
- delay(500);3 g$ y5 a0 x+ a5 t$ \4 U0 [8 J
- do" {1 l# ~8 n5 d9 u l# O2 m3 y, D. T; f
- {
h* ^! B7 {' y! P! A - int Err = getErr();3 Q9 |0 s& @' E
- if(D1 == 1)# T& I, U+ d! U3 P! Q, Z
- {
: C, H# b4 |8 j) r5 l( y6 O/ ?6 b - Left = 1;
: U+ e* O3 o8 P ` - }: P/ d% F3 K6 M0 c) G$ F {
- if(D6 == 1)7 v; Z F' x9 _$ x6 ^
- {/ U" {7 ^3 S% F. T* d5 V7 r
- Left = 0;+ @, Z2 Q8 ?* `+ D
- }
9 B+ o) K, l* G m3 \& ]7 y7 G9 T - if(Err == 99)0 w: L( ~7 B5 E0 T0 `3 P2 c5 w
- {
: \6 L( p, l# J. u3 O - if(Left == 1): _) L! R# F. H: X8 i
- {
1 G5 [7 Y8 p6 W5 | - Speed -= 5;/ E. d! B3 U6 ~4 r3 @) j
- moto(0,Speed);
/ k( Q3 v$ V0 X* ^6 u$ ]- ]& w$ n5 F0 b - do( x; g. d# a- M+ n
- {# F( L1 n- Z# {+ x- Q) u
- Err = getErr();; Z) T# R, c1 j
- }while((D1+D6) == 0);
* F+ D* V5 Q; h! K - }2 l7 \; |: t8 } P3 Q2 `
- else q1 L2 e) l/ }
- {9 Q6 n n! T5 W3 I7 v
- Speed -= 5;( N" t- W8 ~# E
- moto(Speed,0); " p& B! T: I$ |5 T
- do. F7 W4 J: h4 ~5 n& T4 C
- {! U$ u- S% L( B0 d$ B
- Err = getErr();* ?0 }# m5 B4 E l$ z
- }while((D1+D6) == 0);
) ]5 R9 ~2 Q" o. m t - }4 Y% K- I3 I% [
- }
# F& P5 u9 w1 S5 A, N% J - else' x6 i* [2 T& v3 r+ x
- {
4 ]5 c* h% j8 E - if((Speed < 160) & (Err < 2)) Speed+=1;
1 j1 P5 J) ]$ e+ u/ _ - if((Speed > 100) & (Err > 2)) Speed-=2; : I) m3 N: ?4 B7 o+ b, T+ a! S
- integral = integral + Err;8 p [8 ] X9 S1 E4 ]% j* G
- derivative = Err - previous_error;
4 s, B4 I4 M+ c& J6 ` - output = Kp*Err + Ki*integral + Kd*derivative;2 H2 T6 v4 }! a+ S) t
- moto(int(Speed-output),int(Speed+output));6 T' N% O k2 w
- previous_error = Err;; d- n9 d$ w7 e/ n7 @: Q. P e8 I
- }$ @! \5 i$ R3 ~$ o3 \8 U) n
- }while(!((lightsensor_2.read()) < (10)));0 @, P5 s- K% l1 t. {4 n
- moto(0,0);( E0 G* Z' ~- j. t! k @ A( H7 U
- delay(500);
2 o' q! Z- D2 w4 a4 u/ U# A - buzzer.tone(262, 500);
9 w t6 Q! R4 z7 ^9 _ - }2 w- N/ U% ?# s, j2 K: E% I7 e) U
# v4 \5 {: l0 C$ }: p( j/ ^! _- int getErr()
+ z( R9 S/ c1 ~! _7 p - { t" I. Q7 a6 c2 |
- sData = linefollower.getValue();% f) i) q3 l; A# F! \
- D1 = ~(sData>>0)&1; K& U& m2 K0 U3 M4 Q3 c6 Q5 v3 z* N
- D2 = ~(sData>>1)&1;
: z4 u/ Y! p c3 W$ L - D3 = ~(sData>>2)&1;. m- v" E3 y5 a+ K6 ]
- D4 = ~(sData>>3)&1;5 `7 z- g' i' c2 r/ s
- D5 = ~(sData>>4)&1;
6 r/ {, j% u8 }" F - D6 = ~(sData>>5)&1;
5 L8 p1 W# m6 Q+ D7 P( {" h" R+ O - int downD = D1+D2+D3+D4+D5+D6;
- E& y y2 H: y ^ - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);. c4 e" S$ g1 f" B3 c
- if(downD == 0)
& c# |( h0 y/ g# h* Y1 Z - {% _( u% x" E- L6 O" {4 B
- return 99;
6 v# L# u( g2 b ` - }& \. V% k2 C+ _3 l( C3 E& U, A
- else0 e S" x! @9 V/ u9 ]
- {
& y8 X; j! G0 y' v - return upD/downD;
9 n D* W) L- E6 i" u n J - }6 T9 T% D2 W, y0 h$ v( Y
- }+ f4 Q$ B# x; V9 ]) {
- + y8 k7 S, j6 t, r2 Q1 T
- void moto(int Speed_L,int Speed_R)+ S5 }1 j: a; j) n0 h: T
- {
7 e8 S- y( `9 Q# q; T8 T( K" @ - Encoder_2.setMotorPwm(Speed_L);, P p: C" H# V& d: R0 P
- Encoder_1.setMotorPwm(-Speed_R);) u8 P# S. a8 m. d3 O
- }
複製代碼
( }% f5 z* u6 F5 Z7 U! vMeLineFollowerArray.cpp
. a' E: Y1 f7 _& R% X$ L! c: k- #include "MeLineFollowerArray.h"
3 ^) I0 i/ X* X5 U1 h( b2 H
% ^) v# P+ a$ L L- #ifdef ME_PORT_DEFINED$ p2 r- | }" B8 ~( E7 Q+ `
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)5 Z8 S+ D) _* V. w' u) J/ C
- {5 |5 E8 q C1 U# S" x, ~: c
- ) t" {2 i8 }, g0 `% Z( y% k
- }
. d0 i/ O! I: D1 @ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
* m& P4 F+ u' o% X* y: W - {
5 f$ \ v' g( f6 t - _DataPin = mePort[port].s2;+ f$ X0 K: V" r) b% v
- pinMode(_DataPin, OUTPUT);
: M. P5 S7 O& W- U9 [ a( Q# I - digitalWrite(_DataPin, HIGH);
0 s/ v# q: f5 y6 \1 x/ K# t - }
: ]* p9 O, O( m3 B7 S1 r% b - #else // ME_PORT_DEFINED
4 ?& F, N* e0 u G w1 q- ~$ ~) ` - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)* {9 s9 @+ Z" y9 I7 H( k9 s
- {" {! V: _5 ], N) j, \# a
- _DataPin = pin;6 `: x; u7 c* v; Y; }# m3 H
- pinMode(_DataPin, OUTPUT); ; I# s- ?* |% D
- digitalWrite(_DataPin, HIGH);
5 b8 w0 n+ Q2 V/ r) s, Y - }
, }5 S6 e' u; R7 D: m g/ ^ - #endif // ME_PORT_DEFINED
- p2 f/ p& x; G3 o& b9 w; l - 4 k! n* b% v7 Y0 b* H+ b
( T, y1 {- N' k8 S0 O- void MeLineFollowerArray::setpin(uint8_t pin)
- j$ ~6 ? f7 a8 F5 M+ G - {
* c8 R: f2 }2 E1 ?+ R, U5 E5 u - _DataPin = pin;
- G& { t3 @2 W3 W; \ - pinMode(_DataPin, OUTPUT); 4 a3 I" t+ ?, i9 A, o
- digitalWrite(_DataPin, HIGH);4 G0 E, F% ~/ Y; b6 {) | L
{; W) `& a7 p+ X; {/ Y" `- #ifdef ME_PORT_DEFINED
' J7 q% L: \4 t; \ e) X2 m8 o$ { - s2 = pin;. T: j, J' o0 O: n) ~ K2 ]. j
- #endif
5 Z3 g( {6 K5 O. a% I - }
" ~1 T% \& T0 l- l. W' h! d - 4 | t7 ~4 l( Z m
- uint8_t MeLineFollowerArray::getValue()% @1 w* G) F5 D3 I* {3 D* ?1 [
- {' c% P+ F% T0 U# s
- uint32_t LOW_level_read_time;# V" w( x" [6 B
- uint32_t HIGH_level_read_time;
3 H9 v. ~1 q, D1 c' K/ u( U - uint32_t time_out_flag;: G: E% Y2 _# e1 r' y7 y
- uint8_t Sensor_Data[3];
% |* ~$ F, x5 J" F - static uint8_t old_data = 0xff;
2 `( e' G5 _, R' J* I1 i" s! ]6 r - " F& T# _ i! v0 S
- pinMode(_DataPin, OUTPUT);
. d( y: w: R: \, Y - digitalWrite(_DataPin, LOW);( ^6 M+ u( D; j- |
- delayMicroseconds(980);3 N6 q2 U0 V/ \: B
- digitalWrite(_DataPin, HIGH);2 b& P: l. a" I, Q
- - }3 i; U; c: R/ e. R( r2 l) a
- pinMode(_DataPin, INPUT_PULLUP);
9 B+ D* T3 t- I. V+ e - delayMicroseconds(10);% j4 }4 i( t U: P% p
- 6 y% n8 Y: T: Q, J9 a
- time_out_flag = millis();$ y _* t4 E6 y0 e3 Q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, n/ @( a7 k0 e! R: G
+ J# s+ `2 w3 ^' T; F$ d; T- LOW_level_read_time = micros();
6 O k) y; P2 L( p F. S( B - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
4 M3 B* p3 r( q, i1 B, p - {7 ~0 c4 ]6 g' t* v
- return 0xff;
9 H: ~: |+ @1 n - }
& t* C. Z& x" R3 s E+ @4 y
* V/ Z8 [0 Z6 P& K/ z1 F A. K( x1 I) H- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
Y7 X7 |. P0 {
& {, c: E2 Q; Z7 k9 j- HIGH_level_read_time = micros(); n" ~$ C3 Q; n( O8 e
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level h. h$ B- N0 L! w
- 8 A# L5 d# l! T) J, W. U1 \& `; T' J
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& E+ o, D+ [7 N! Z6 g$ X - {0 j; [0 z' h1 H- O" Z7 H
- return 0xff;
, S, D/ z( v, D; x! q - }' K7 Y) [7 d7 g/ @
: [3 z" e9 P! M- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
' s3 `0 D3 W. ~' p! O k6 K- t8 r - {' g2 Y. I' p7 m5 \
- return 0xff;
% V% O" k' H4 m - }
: O6 k5 O, P9 m( K0 N - / z* z& f# H0 h# }; f: X$ p
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ L2 E* ~1 Z) C- x4 x - LOW_level_read_time = micros();( W7 K. w5 c9 c( U1 w- r
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level' N6 E: T9 Q5 e# Z V5 j
- 4 V) g: w& n+ w' r
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
- F W8 y7 a) I) ^! S - {
. v% o; ~; q' w3 O- [ - return 0xff;1 L$ ], t& P3 x! a0 a
- }
5 J- G$ w" C5 g" s% L. y; y6 W
6 T. d: S8 W% V1 y- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
3 u) [, t& b( j O3 j' }9 ~& F - {
# v; j# D- M! _" M% l7 W$ I0 H - return 0xff;
4 K* U8 J4 u6 R: S/ A9 K - }
$ }0 l2 U m! i) A
+ t6 U. V/ z: R9 w9 a7 p, M- for(uint8_t k=0; k<3; k++) d6 {& c3 P8 S! M m: _
- {
4 F. }' s& K; I" L - Sensor_Data[k] = 0x00;
0 ~0 X6 c' G9 Z" G1 R2 f - 6 `8 w% J8 S7 V2 q
- for(uint8_t i=0;i<8;i++)
7 p6 i9 c8 y, q - {3 j9 `1 }: M8 \+ E, C4 e
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level! e# z# |9 [0 S, j! n
- HIGH_level_read_time = micros();
- f, V" O+ J( G - LOW_level_read_time = micros() - LOW_level_read_time;5 Q& }7 B2 H$ Z/ W; K% l( j) m
- 8 F- M, V8 I9 v `2 n
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ), F) ^ ?$ Y+ \' p% u( @
- {
+ g3 B0 ]9 B$ F# t - return 0xff;8 V- |* X7 w! \- p6 q' _% |- P" a
- }
6 {) G7 b& U9 w2 v - 3 k2 a) j4 c. v& `$ @
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ N* ~$ Z# ~* w8 i I/ x1 |! M
- LOW_level_read_time = micros();; u) v# E# R0 G# \" I3 `, d2 J3 A" n
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
# u2 Q4 L: P2 J1 G - ) ~. y- ]5 S# W" k+ V
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1. k; `- M3 O" y
- {
) [4 J4 U' _1 l' ?8 W9 I - Sensor_Data[k] |= (0x80 >> i);: g+ X6 H6 p d9 h
- }
/ Q! f u+ q4 W) u& V - else if(HIGH_level_read_time >= 100)
. T$ M5 Y* ]% x: B - {
0 U( `$ z) H$ I$ F: d - return 0xff;
* m; p0 b; a- ]3 v6 x6 ~: I - }
8 n7 z+ _# z, c& C! W - & N7 L8 J0 z5 n# n6 K) h
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
0 |' c* p8 h; c$ T5 o8 X3 { - {- U+ H7 P4 H# r% t
- return 0xff;
$ ~2 r b" t+ U& N( c. K - }' E# W/ J5 g( q' n* ]! j
- }+ c4 Y( j1 X5 ]5 t. o
- }' b! H. M4 l1 v9 s$ s, V
6 ^; n& L5 M& e( r$ k3 g- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% ?% F& G+ d- S) E- o9 J
- HIGH_level_read_time = micros();% x t1 i3 A' Z, R2 B/ a8 ~
- LOW_level_read_time = micros() - LOW_level_read_time;
' m5 t1 U3 n X- @/ ^/ d, @, o - % \* q2 |* d! ~* i* w4 s. ?
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# |$ C& _/ f/ E. _& E
- {/ L- [* i3 e- z* W
- return 0xff;' b- L3 d6 \( X1 [( }* ]4 Y
- }
! u4 b, Q5 e6 Q; G1 M3 s- f
3 s# P+ s' v+ q; h9 K- pinMode(_DataPin, OUTPUT);7 S* p/ Q5 s# ^+ a
- digitalWrite(_DataPin, HIGH);
$ M5 r& c$ ^0 p3 U2 O, i# w - , i! B" B0 n4 [, `" X6 C9 D( m
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))), }0 G" X! _8 H# ~8 L
- {
1 s8 i5 r2 W/ E7 g R5 ^ - old_data = Sensor_Data[0];
( t4 ]/ T- P" ~: @/ u+ [2 \ - return Sensor_Data[0];
5 y7 n) P5 g$ X$ G$ V - }
: `0 c& k7 q: G& I - else! W. _3 v3 u9 J
- {0 `: l3 n) N+ F1 [
- return old_data; O$ u$ x8 i3 n& g
- }1 d0 l* i6 ]; j& N; E
- }5 v" f4 c, z/ k
複製代碼 : p3 O& q) E" T" Y5 v
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
! e2 P* c8 Q' B. {: F6 L/ K3 }
0 @6 }& e* o+ r9 N8 D! W! X |
|