|
|
9 [7 n3 E) D; V
- #include <Arduino.h>
- C1 `: V1 t1 U f! x0 M - #include <MeAuriga.h>
; K* A( [& K/ H {# Y( J - #include "MeLineFollowerArray.h"
7 D3 g6 R: H$ H" o4 ?) J2 W
; S3 N( V/ j" G7 b- MeEncoderOnBoard Encoder_1(SLOT1);
5 U @9 j5 T, q- m. t% [ - MeEncoderOnBoard Encoder_2(SLOT2);' ^2 [2 y3 p" D1 j- j/ B: H
- MeLightSensor lightsensor_1(12);
7 B# \+ i4 p w; `) ^0 o. ~2 k - MeLightSensor lightsensor_2(11);; D, |) R1 |4 K% V/ m
- MeBuzzer buzzer;/ h9 d! ~1 G. K5 u
- MeLineFollowerArray linefollower(PORT_6);5 l+ G' e8 @, Y$ e
- * K, [: P+ J8 q: [! j) N% F8 j$ ?) ^
- #define Error1 1
5 [* Y" I: g8 Z/ [ - #define Error2 2 M) {4 o& E" {9 f& ^
- #define Error3 34 n8 `$ O. t3 O4 F6 K' W( u) {
6 N {" W7 @. c- #define Kp 15
* R' d! }6 I- q6 l - #define Ki 0.15( N$ C; U" d( M" K& o
- #define Kd 0.03
5 y+ x* g7 Y" {: U1 p
# p) i& [" N" o6 h+ S& S4 h- uint8_t sData;* ] }2 @, s% \+ ^6 x; n& _
- uint8_t D1;7 u2 j/ F. s) \
- uint8_t D2;" [% L! R7 z$ }# _$ P
- uint8_t D3;
# M9 F6 \8 ^- a, V/ t. K - uint8_t D4;
4 j. q( E4 N/ Y% M& i - uint8_t D5;, s; }; @" o0 Z* ^
- uint8_t D6;
- }0 E0 @* x4 p4 M) s; c - 6 |& n# W# W/ A
- float previous_error = 0;
5 b5 N* A9 T: D( |2 h. v6 h - float integral = 0;" V7 ?: p$ c, U a
- float derivative = 0;
$ }! O' L; m0 ]9 S9 c1 i/ [+ J - int Speed = 160;
6 P, r6 f5 _, r - float output;
( j0 U9 K# _4 z; O2 F' M2 S: s/ m - 3 C) l8 y6 s/ I8 A: b
- byte Left;2 {6 O2 _& H6 ~) E
- ; s/ ~0 p1 }4 M: I+ [# L2 i/ J) d
- void setup() 6 @; ~9 i' z$ p- ?! g' R
- {
$ @! t0 `! y7 ? F5 d - //Set PWM 8KHz
0 J5 A8 J2 U5 }6 z4 j, x' S! V - TCCR1A = _BV(WGM10);
' |7 q; ?- h+ Y1 j) ` - TCCR1B = _BV(CS11) | _BV(WGM12);7 |4 A2 \0 R" u8 J
- TCCR2A = _BV(WGM21) | _BV(WGM20);% w9 |$ n' m& ~8 ~0 b
- TCCR2B = _BV(CS21);; @; H" D# d( F' F( a: L3 X
- Serial.begin(9600);
$ w; G/ J/ E( y% Y5 j) G - buzzer.setpin(45);
. k$ `6 f ^' Z4 f" A2 h2 t' S8 u* g - }+ G! ?% p0 x! S5 U" \
- 2 T5 w: V; \. k3 _
- void loop()
$ f4 K1 e" x/ h& @ - {
# v5 q" l C& O2 [4 f, G - while(!((lightsensor_1.read()) < (10)));3 o4 r0 x- f1 J* p
- buzzer.tone(1047, 500);+ X1 O# `3 z2 a: L; u
- delay(500);8 S" N3 ?6 [9 G3 ^* H5 z
- do9 Q3 ?* W" J* R4 R
- {8 O' C }3 M# u3 _4 Q1 X. u6 c# ^' h
- int Err = getErr();+ z# n' r% m' i; s$ _5 g
- if(D1 == 1)
& S' f0 ]5 d6 {' c - {
! n2 l2 H& B( y4 c. @3 r - Left = 1;
3 i$ `5 N* a' h. s - }
/ [ }+ S5 h) r0 j% y, R" V. X - if(D6 == 1) v# ^/ X/ o! G
- {
' Y( ~% ~) X/ \$ m* L - Left = 0;/ b* |% f6 C/ J$ }
- }
& n0 ?% h7 ?+ \. v, Z0 E1 R - if(Err == 99)7 D' [" W! _% j) Q$ N- A
- {( [+ C* m& l4 G1 X: M
- if(Left == 1)1 n- Z/ M; U+ G& ?0 `1 f
- {- y, O3 }2 b3 E' Y3 r+ H3 |# H4 l
- Speed -= 5;0 Z8 A, k" v5 d% W" E5 i
- moto(0,Speed);( v" H5 V3 G8 r6 \8 q$ ?4 D' V
- do
& s* |9 O' a& Y' v* g$ s - {
$ W/ D& z' v) ~! v) d$ Y - Err = getErr();
7 [) f7 G0 O5 F - }while((D1+D6) == 0);" \9 v( }8 J+ V% O6 c# A7 _
- }
: v* `/ Y7 c: q. B0 G - else
& ^, k+ v: h- E A - {& I/ U7 M+ d+ X) G" T* I J
- Speed -= 5;( b2 x; W/ N; R, b; `4 a9 Z! V2 {
- moto(Speed,0);
3 R3 J& U6 V* L/ m( r) I - do0 K9 S- v s) u! j
- {6 V. j* i t1 b1 j
- Err = getErr();
& J$ ^( F* U# _$ _ - }while((D1+D6) == 0);0 ?9 ]4 A: c% j
- }
' `6 t8 S; L- O( i. d6 Y- p - }# ~- w* F: t# @. W. O
- else
% J4 J& s9 y3 g0 T1 C8 t - {' U% ?7 ]4 ~- R3 m
- if((Speed < 160) & (Err < 2)) Speed+=1; & r7 {8 r- g; l/ Z! G' ~% d
- if((Speed > 100) & (Err > 2)) Speed-=2;
2 m5 s# K r. R. U" G - integral = integral + Err;
, U3 @+ [, @3 j, V3 y* @+ [1 _ - derivative = Err - previous_error;
m6 k- `! O' X0 o2 \3 N' Z - output = Kp*Err + Ki*integral + Kd*derivative;1 W; g/ J; e6 [0 V/ t& M/ l
- moto(int(Speed-output),int(Speed+output));& ~) k) }# R( h9 n- F
- previous_error = Err;+ O# a! O5 K# w1 e) k8 a
- }* T) @/ R( u, h- H& ?' R
- }while(!((lightsensor_2.read()) < (10)));4 Y! s/ n4 ?3 h* u# M9 P- z
- moto(0,0);# v b, V, j% q/ y
- delay(500);- Q% k& Y" ^0 a! e2 q# u) c3 Z5 R$ ~
- buzzer.tone(262, 500);2 g3 l& I% N6 j- ]
- }
8 A) F5 Y5 W, ]; Z
% k* b l' C: L" a- int getErr()
! f4 I# ~1 Y+ _+ o9 L, \ - {
5 F! a: }& s3 A* r) J9 q - sData = linefollower.getValue();
f _8 p+ k6 b2 P1 h2 V B) M - D1 = ~(sData>>0)&1;2 z' U1 [ u0 l4 Z* ~6 d5 S
- D2 = ~(sData>>1)&1;
" ]9 B7 k+ j+ M - D3 = ~(sData>>2)&1;
* r2 Y C" t l9 J. L - D4 = ~(sData>>3)&1;% Q6 i! E) x. ?& A$ x& A
- D5 = ~(sData>>4)&1;
) W o( [ D& S9 w" v& }0 }3 o - D6 = ~(sData>>5)&1;
/ i" [6 ^* g; Q+ K0 C& k - int downD = D1+D2+D3+D4+D5+D6;
0 D9 U1 T2 B G8 J9 B9 y/ e - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);" Q' k# S8 l6 e- X: w/ D+ l
- if(downD == 0)7 A) ?5 ?& ^& D& [0 e2 j
- {
% G" u2 x+ x& Z - return 99;
2 h" t, J4 N0 t; D2 j - }
0 z) G+ k$ h! d8 a3 w* E4 O3 K+ M - else
0 D T3 ]* ]8 z5 t - {
. v& P$ C0 u6 U6 q - return upD/downD;: O6 h' x- J& t- W
- }4 `7 s/ `. K& d& A& t, g8 k
- }
& y) q+ q+ Q9 L; [+ j - & `% u# ?8 @; n- _; y, l, D
- void moto(int Speed_L,int Speed_R)
( V( ~. |) W( b& [0 R7 f - { m- F% {! R: ~3 o2 G
- Encoder_2.setMotorPwm(Speed_L);
# p, U4 d$ z2 `2 a3 u - Encoder_1.setMotorPwm(-Speed_R);/ X4 J& p2 N6 W9 _! I
- }
複製代碼 , \! M0 n! k# Q; w% o( y0 c
MeLineFollowerArray.cpp
/ b) b+ ?% E- x& D1 \/ q( B- #include "MeLineFollowerArray.h"7 V. m4 |; ]9 x, K( k
- 9 t6 r1 j/ u3 F! B- ^. ~
- #ifdef ME_PORT_DEFINED
& x& R& P, O e. { - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
# c ^! E9 _6 J; A( @# r - {
( z* y4 x$ M; y - $ r# b6 Y; |9 t4 A# }2 k
- }
/ |# k u6 M9 k; G2 V# O* R - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
0 {: f k* L) q( @3 `7 g7 } - {
* l( H' ?" c8 X" X }; | - _DataPin = mePort[port].s2;$ v3 f. ~3 C3 l5 y
- pinMode(_DataPin, OUTPUT);
4 `& b5 E; c4 I* L8 Y J - digitalWrite(_DataPin, HIGH);- {9 _9 [( [4 y+ p0 V
- }
* n1 a1 |2 m" h d# E6 x - #else // ME_PORT_DEFINED
5 i) u; W* c# q7 g. w2 R# ^8 n* s P* m - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)4 ]% g9 T. L1 Q# q" x* l
- {
9 S; @3 }& [0 |+ T3 ]- i' h2 g - _DataPin = pin;3 k) a1 G! \; r* I z% Z; s
- pinMode(_DataPin, OUTPUT); / l; @2 U7 \1 Z$ P
- digitalWrite(_DataPin, HIGH);' x" x9 _: x; t: d1 U, ~
- }
; C( [9 r5 y( u6 Y - #endif // ME_PORT_DEFINED$ G4 _5 R# Y& R, f; j7 U( x% Z
; `" t2 a: B6 F, Z2 P2 J6 m- - f& S/ G& U( I) l0 o+ h( K
- void MeLineFollowerArray::setpin(uint8_t pin)! g* W( R2 [5 v. x3 Z
- {
* V( N( u0 C% ^7 z: q9 H - _DataPin = pin;+ U& m6 `7 K% d3 x, d2 |& d9 v
- pinMode(_DataPin, OUTPUT); 3 S H: q( o" w5 F3 Y9 Q0 J
- digitalWrite(_DataPin, HIGH);
" u7 z, M1 U* Y& a% @5 ~% |% i* j - % g' j% V5 {; W9 y, z M7 X- n
- #ifdef ME_PORT_DEFINED7 b4 A6 _. B T& Y. r" v- a( b
- s2 = pin;; U6 @! c" z; N- a e; y; C* u- ?
- #endif
% o1 f4 y: z- K - }3 Y* R+ c+ K# R, q* r- V! Z
4 h& `& P: F& _/ I- uint8_t MeLineFollowerArray::getValue()& b' R% J0 ]" v c% ^# Z" q
- {/ M- F7 U8 A% t, x/ b/ ]
- uint32_t LOW_level_read_time;8 _! [, k8 G+ k$ H
- uint32_t HIGH_level_read_time;
# Q. g2 ^! K$ j) R6 W/ s& L - uint32_t time_out_flag;' ~+ B: W1 h2 _. J* S9 Y: c& s% x
- uint8_t Sensor_Data[3];
. h$ @! D- W5 r# y8 V7 C/ k) { - static uint8_t old_data = 0xff;
6 B; D. V' E+ g" b1 j! R( a
, Z# @$ v5 ?7 I3 C( V- D2 s3 ^- ~- pinMode(_DataPin, OUTPUT);
9 I" G# s4 E% {3 u& z r - digitalWrite(_DataPin, LOW);- U' }' F, ]" g ~" _& h4 Z1 |& }$ ]
- delayMicroseconds(980);
; g4 J) s4 ^& D9 p5 B4 z - digitalWrite(_DataPin, HIGH);
5 {# j! Q, `5 f2 e3 Q - 8 c3 P- {% R% v3 i2 p1 {
- pinMode(_DataPin, INPUT_PULLUP);
# O& F8 O7 O% j( K% T - delayMicroseconds(10);
# M. a5 {- ] [. [( H( ]
) `9 c* s% c; u1 z3 O+ @2 N2 l- time_out_flag = millis();
( Y/ A5 _8 N( W% P" I9 `1 } - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ U# w+ ^1 H' l) f6 g
' d" m* w1 g' H4 b2 x" }$ M- LOW_level_read_time = micros();. ^/ B, m0 a, y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
6 F$ `) K9 O6 K7 ` - {$ J# M9 g" [9 h7 k1 u: H3 o( S0 m
- return 0xff;
* J5 p, `0 O5 B) _ - }
9 f: ^0 H. Q6 ]+ _5 n5 c
' H( K4 ]# S( l: ?0 y; Z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ C4 }& `" G) j3 [& f
2 {; S8 B; f) p7 T3 q: ^- HIGH_level_read_time = micros();4 \# J5 F, j( K2 i
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
" L( c% |7 @, l8 p7 E* K! H - * u4 j. S! V* A$ r4 V+ X" a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" m1 v, w8 h/ Q, ^( Y
- {& E+ P$ {4 l: d: }% `+ o. v. W
- return 0xff;
. n) s9 q3 d: h9 c - }
+ t# O1 v; k6 g" F
4 E7 f& v6 Q6 S4 O$ \- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
$ w0 e$ b1 L* Q6 { - {( F7 C7 Y; Y9 `
- return 0xff;( z6 e8 q3 Y/ X! c7 d
- }' r- a2 e4 C8 D5 t/ t Y
- 6 {' m& l6 g- q2 Y+ X3 Y, b1 {4 u) g
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );% h* U- Y# u; W/ B
- LOW_level_read_time = micros();
3 u& w/ b% @( U1 [2 I - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level) X' b" b1 S$ x& c6 h E
- ! c$ \2 U! c1 V. s/ K
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* r5 r. a# z. a" X3 X - {
- [; n; P' u: ?7 e) V - return 0xff;# P; S+ P) p; q% V+ X* p2 K% E
- }: o& r, V. h2 ~+ x7 j
- L% `7 @: W# d
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
" [6 |( a) B$ G0 M0 B - {4 m+ X# w/ F2 V% t+ k
- return 0xff;
3 M: {, V2 y- q - }" z$ }. n+ Z V
% `+ N7 c) ?: R6 C- for(uint8_t k=0; k<3; k++)1 P( R0 m t, {0 p$ K
- {9 p# P- k. S1 a; r/ i
- Sensor_Data[k] = 0x00;
; ]. ?/ `5 c' |7 o - $ w6 p3 V; R- f5 f
- for(uint8_t i=0;i<8;i++)# c% W/ D$ o6 R# p: @
- { b' d+ F3 C# o- p3 b) F* m& ^ C
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, f. C7 K; h9 D7 V8 u - HIGH_level_read_time = micros();
g O/ v* n. F) F. g- H0 X - LOW_level_read_time = micros() - LOW_level_read_time;! _! } Y4 r) ^: Y0 e+ \/ x5 Z
- 2 p) n: k/ G/ M+ Z/ T
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
. h4 g! l Q! _0 i( Q - {" Z' M# K5 w+ l$ ^4 A3 n
- return 0xff;
1 q) r2 F2 ]* d+ t% [ - }: Q' p8 x! ]) @3 }( i
- & [6 I4 x. z4 z2 C$ X! L3 _( [
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* I% C& d( N) `. f# D8 l9 B7 f0 ~
- LOW_level_read_time = micros();
6 G0 w( C/ i4 o% |' \/ A - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level: r ]6 o# D2 T/ M1 N
- . q4 x. R8 T# @- _2 l# ?. m. {
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
4 ?5 E- }" D% z( f/ B8 c - {3 g) g5 C3 |; G% I
- Sensor_Data[k] |= (0x80 >> i);
. K. r8 e( Q3 } - }
6 K; m* i! m/ D( K/ F - else if(HIGH_level_read_time >= 100)
6 S2 a" ?" _1 ^ - {
1 B: _: u$ L0 ?5 E/ k0 K - return 0xff;
6 _8 s# k8 K: }$ a: a* o% y0 v# J - }: w0 Q" a. D! x! U" ?8 i
% l- n! b* ~0 j2 _- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
2 ?. ` |* |* ^/ b- c, e1 }, M - {
5 j/ b }% V9 X+ i - return 0xff;( h1 q8 V5 c/ X
- }
( G, _' l+ k4 E8 x* ? - }& F8 @: m/ \- P! R' x- N( [
- }
6 x, b5 k: v) b, s( u - $ g/ P: D- ~# z) Q; I8 g
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level& j2 N! L7 o9 {; U$ u) u) G' O
- HIGH_level_read_time = micros();
" d9 T% ` P$ D9 G - LOW_level_read_time = micros() - LOW_level_read_time;
, U& {' m: ?2 p6 o8 Q/ C* L - " K) x( A3 k. L) A
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
, N5 |+ _: r7 m0 Y2 W - {
8 r3 J) g" _+ s5 C - return 0xff;
9 d8 K n+ |* n/ r5 ~- i) f - }, u5 B) n& r& a6 Q% T; T2 D
/ o- e6 C& |, \% D- pinMode(_DataPin, OUTPUT);
: _& [4 H2 J! \/ x c - digitalWrite(_DataPin, HIGH);
! A9 L$ Z% j, X# q r2 I5 s
4 F$ K6 U4 L7 z' D1 m( } ]+ c- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
+ `3 c$ s# ?- E# E1 e - {9 H) J! O9 b3 x% T
- old_data = Sensor_Data[0];$ D! K: \- t7 B' b' X, R
- return Sensor_Data[0];. Z! k a: p. m' R0 {# `9 s8 o- C
- }; ~5 g S6 _. o; V' P
- else
$ s. k/ N( A8 s* @ - {' o2 B m. ?; m% ]! g5 H
- return old_data;
6 L/ D* o9 L7 l' R' ~! X" u1 f7 X - }
" @5 }1 [$ N% f# n - }' r( V6 J/ N. o( @9 U
複製代碼
~+ X+ J# Y$ |" I) B. p- VMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
& i" m: ^2 v9 X% b: C9 h) s
7 n- X- b, V- @ P3 |/ J+ \ |
|