|
5 \; J1 W j7 y& D0 X9 g
- #include <Arduino.h>' Z8 g) b8 ^5 i
- #include <MeAuriga.h>7 l* O; Z _3 d2 N' @& ^
- #include "MeLineFollowerArray.h"
$ {3 g8 M- C0 r2 P9 M* R - $ s. G' x. v; d
- MeEncoderOnBoard Encoder_1(SLOT1);1 M" I. c3 }7 ?* W. b; }1 u
- MeEncoderOnBoard Encoder_2(SLOT2);
( d1 s# o4 K1 r) ]! F2 p - MeLightSensor lightsensor_1(12);7 Y$ |1 q m6 W8 y. c
- MeLightSensor lightsensor_2(11);
/ p: Q" ], w% n& w: s/ D - MeBuzzer buzzer;
- K0 Q$ b6 e2 J5 h' X% `2 j- A - MeLineFollowerArray linefollower(PORT_6);
$ T8 D( h2 r# w' {7 J
. m) d* B1 P, A9 V- #define Error1 1) M. j- Y! \ S
- #define Error2 2
& n, ?( q( Q, x; A+ R* q6 v* V) {7 Q - #define Error3 3
' U) w4 Z$ D5 D# a4 [
: P+ R/ y, l; Q( `- #define Kp 15( n1 w- G/ U% `" `, d3 _
- #define Ki 0.15$ j1 i2 ^1 g, _6 Q2 D9 W/ ~: c3 e$ Q
- #define Kd 0.03% H+ |: m* O0 K" u: ~
- . f" N9 v8 k1 S! q' Q0 f3 W7 O- q
- uint8_t sData;
. @) T, i% b: n) @) f - uint8_t D1;5 h6 c% o# j# Q O! j
- uint8_t D2;. l# L, l2 z, V% E. d0 _
- uint8_t D3;
' M {7 S4 U) D6 N! l - uint8_t D4;+ m6 l5 U' A$ t% ?) n" u2 F- E
- uint8_t D5;% j/ a7 X5 i! m) t
- uint8_t D6;
$ p/ J6 F: {/ i: W* x6 d8 _4 o
$ s9 i' s, o3 s* Y- float previous_error = 0;( M* C% H# W( k, V- t( h" l3 l
- float integral = 0;7 D- k4 R r$ V/ y3 @( }' ?8 ~% g
- float derivative = 0;+ i! m" @8 w; {0 D
- int Speed = 160;; M9 Q/ `( ~, g
- float output;
4 u% F/ q+ u" c2 C( r
) L* t6 D% }; m2 Z- byte Left;
3 u9 {4 C* m9 C" Q! k. m" y1 ~ - % J0 l' Y+ K' @
- void setup()
3 D7 b8 ~1 g. m+ |6 E8 W% t: z - {
4 _: E% {+ O; @: e& [9 l - //Set PWM 8KHz
, s$ z/ c$ f5 s; k) E5 } - TCCR1A = _BV(WGM10);; I" D1 I+ @5 V3 E* F, ^
- TCCR1B = _BV(CS11) | _BV(WGM12);7 ^& S' `: u% M, F$ N/ C# N
- TCCR2A = _BV(WGM21) | _BV(WGM20);
: Y7 q% k$ q7 h. V6 O - TCCR2B = _BV(CS21);; j/ y. ^, \+ Z2 w9 {9 {
- Serial.begin(9600);
7 L' n; h, e0 O( L- D - buzzer.setpin(45);
5 s$ s: I9 |3 |1 M# \. f1 f - }
6 t/ y3 V" ~* l% R w - . T+ b1 b3 R' ]; B
- void loop()
+ t5 v5 Q, x- L: B% ~" a, l B - {; T& r1 \5 v4 w+ U5 C, F
- while(!((lightsensor_1.read()) < (10)));
3 {- P- S) ~8 W+ O& Y o; X - buzzer.tone(1047, 500);
$ W( o( f1 W, P$ V" C9 _ - delay(500);
; ^" |2 B% Y5 d' g5 R; N - do
2 s a+ G1 {' G* `2 x - {2 ?" X2 { D2 l, A$ G8 [
- int Err = getErr();
; D, j( m& H! `. x, H/ M7 u7 \ - if(D1 == 1)+ [; r! u5 ~( ?; D
- {
a5 v& a6 r0 B; q - Left = 1;7 X8 t$ Z/ Q; v. s9 \+ b" n
- }
: Y( u& C! ?$ \1 v F - if(D6 == 1)( m3 H" P, {, k* n( x
- {' K! V( Q9 R( |
- Left = 0;
4 q# @. r1 h6 b [+ \7 _ - }
, p1 |' p/ e: A/ r! [6 e - if(Err == 99)
. E( M$ k' W% K - {
" n9 M+ G0 D: q8 |8 `, w - if(Left == 1)6 \8 G: U( |' {+ D+ Z
- {6 v& s6 e6 L, m9 F
- Speed -= 5;8 k+ Z" d* v- c
- moto(0,Speed);
1 M$ w& e8 r9 L6 ], ?# Y# b - do& J5 m/ ?! \- g! d
- {
4 m1 c* `# c8 Z7 v6 @$ r - Err = getErr();% y) @8 v' C) H$ x8 M
- }while((D1+D6) == 0);5 W* r. |+ m2 B
- }
7 X8 F( m' j+ h l8 Y: g - else
8 |% U: K2 X! i) }7 `8 R - {5 \6 u$ O" ^- c4 x3 ^% [5 l$ N6 A
- Speed -= 5;& d9 K' W% r# Y X4 Z w, P1 g
- moto(Speed,0); , S, D" ]+ O8 u- k" z
- do
1 G ^) ^% @, ^ - {/ d4 Q; r% \: z% q0 u
- Err = getErr();' j/ l7 O9 C Q' Q7 u
- }while((D1+D6) == 0);
+ v( Q/ a2 ~% ?. U9 l1 f - }* Y Y" y# W5 I0 |' k k% M7 Q
- }! A! c2 M4 j1 L' s. L) c8 r
- else2 i5 [4 m% g$ {
- {
{4 E: Y0 l% m" r& W - if((Speed < 160) & (Err < 2)) Speed+=1; 6 w$ M8 ~+ z# c% V/ Z: ?# K
- if((Speed > 100) & (Err > 2)) Speed-=2; 8 x3 h8 p0 O! u* r
- integral = integral + Err;5 \+ W+ s. Y+ I, \3 Y& V
- derivative = Err - previous_error;6 W y' \: n, ]0 A9 z: U2 \
- output = Kp*Err + Ki*integral + Kd*derivative;
1 c$ e7 S( L$ U3 r! z) B - moto(int(Speed-output),int(Speed+output));
, t/ V4 k: h8 E - previous_error = Err;
* g- k1 F C8 e - }
# }$ b8 S+ l3 J$ t - }while(!((lightsensor_2.read()) < (10)));8 h) d2 F; I9 {+ v2 q
- moto(0,0); L$ z/ [; \4 Q6 ?0 A5 T2 y# J
- delay(500);
9 ]" p6 I* A5 k& s - buzzer.tone(262, 500);* R, ^9 ~% s% \% Q$ J& F
- }& _1 y* g0 A+ m9 E# ^% a7 s0 ]
, \6 c. A) [( `1 x- int getErr()
. Z' [0 p8 I6 X: r' x: ~ - { # k& y1 E8 \% h- B7 H/ U4 y
- sData = linefollower.getValue();* A3 E8 T$ @7 @8 t$ Q" P
- D1 = ~(sData>>0)&1;
; ~$ z8 ]3 a- s) ] - D2 = ~(sData>>1)&1;8 C* C& b7 e$ y9 O% Z, p: e$ P3 U0 Z
- D3 = ~(sData>>2)&1;
& k, i3 @* i! n( X - D4 = ~(sData>>3)&1;
: w: o) W( w% t& c/ c - D5 = ~(sData>>4)&1;
; q5 m1 Z0 }, \* ]* y/ {0 m - D6 = ~(sData>>5)&1; V9 ~# r! z# C2 L# l6 x
- int downD = D1+D2+D3+D4+D5+D6;+ [: o' h7 I* T1 K
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);0 }- M' w2 ?1 b" W' W
- if(downD == 0)
6 ?: q9 r2 Q! Z# m - {
. K; v6 \" J, Y/ g4 V( i6 l8 N - return 99;
/ l$ x6 s' L# l" ]+ d/ J - }
6 A! p2 [% X8 H7 O# ` - else
. r+ t" B& z! U) u7 A1 e# S - {
8 A/ v) y; `8 N7 X- m; t$ T - return upD/downD;
: g/ u9 j6 D# T/ f. Z+ F1 V - }( i7 {: B/ ?' t/ d$ {! u
- }
8 [+ ~+ V, q0 k& |2 `* j, V; |& c! E
- a8 |8 u0 \0 `- void moto(int Speed_L,int Speed_R)! ]. G# k5 @+ E7 B
- {
Y% J ~: r& f5 ]% F9 O" S - Encoder_2.setMotorPwm(Speed_L);
! `* v# U( z: R# R w9 f% | - Encoder_1.setMotorPwm(-Speed_R);
( j5 z; _7 o! i L - }
複製代碼
7 _4 K+ l' f/ H1 l; bMeLineFollowerArray.cpp
& F! _( J, J: ?% F+ k4 s' @( c- #include "MeLineFollowerArray.h"
- s* J! M. ~2 ]; A2 @2 i, W - ! \9 a5 [' A; X- F6 B6 H3 F6 {
- #ifdef ME_PORT_DEFINED
( M" h" X; L3 v( I- v - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)8 F/ [/ F8 v% {0 ~2 b0 S: t7 ~, {
- {
. \5 _4 ]2 u$ Q1 p+ p. c - ) ~; c/ v; V+ y2 }7 q
- }& F! Z3 `, \2 c
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
( @2 K* t9 V2 G( c; s" t, y - {1 \, U3 W" s3 K
- _DataPin = mePort[port].s2;3 T8 o" X( [3 i5 A# G
- pinMode(_DataPin, OUTPUT);
4 z! ~4 R/ j. B# h2 o1 r5 ?2 x4 B% e - digitalWrite(_DataPin, HIGH);
) m' k: |( @3 p; ^! ?7 n - }
/ v0 I+ c) h- S - #else // ME_PORT_DEFINED* o7 `# k$ e; n' H
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)% }, _. \. N( ^; |2 [/ \
- {
" M% S; o& n* c& p9 r) ^ - _DataPin = pin;3 d, m0 ^% }( f0 _* n
- pinMode(_DataPin, OUTPUT);
! w0 r' ~0 S" d) c( b% R0 b" W - digitalWrite(_DataPin, HIGH); h3 ]3 Q7 ]0 K% Q, A
- }3 i! N/ R! T/ c \3 O0 Z
- #endif // ME_PORT_DEFINED! L; r/ O# g& r' c. {
- ) ~$ w% h7 ]' T
, q8 s2 B# Y; g- d( F* R- void MeLineFollowerArray::setpin(uint8_t pin)4 u4 ~0 D" l7 ^5 v5 G+ g3 z
- {7 g5 a3 L! R5 n+ s4 l
- _DataPin = pin;
* \3 p- a' A% U7 I) a( @' S - pinMode(_DataPin, OUTPUT);
/ Z" [( S K3 r4 @2 s5 j" D' q - digitalWrite(_DataPin, HIGH);, m. Y% k I$ X h( _! r
1 T- K% M( x; a- |7 E- #ifdef ME_PORT_DEFINED
$ l3 K% u' A, [* v7 ^3 A, N - s2 = pin;
8 u" O" a! e0 ?, E' A4 a6 C, J2 n - #endif
/ M8 t) g$ P/ n7 Z5 t" ]& u( r - }& Q3 Z" Z/ C& _
- # u; ^0 W' | \% H! x
- uint8_t MeLineFollowerArray::getValue()) c3 m7 _% ^" w2 b0 H1 C+ n" H7 X
- {4 |' _ w$ O+ _" i1 S Z
- uint32_t LOW_level_read_time;
: I7 _- S$ R. w# H2 M- O+ E/ n/ { - uint32_t HIGH_level_read_time;* e/ O8 t5 P4 O# @
- uint32_t time_out_flag;
# L( p6 _; f- H1 K - uint8_t Sensor_Data[3];
9 i. L9 m& w% y9 N* b0 c m - static uint8_t old_data = 0xff;: G9 p! a# I3 f) D. e# J
- ' O' P# ~6 P M" Z5 k' ?
- pinMode(_DataPin, OUTPUT);, W. ~' o: `" N1 n0 P/ F0 d
- digitalWrite(_DataPin, LOW);
8 L8 [/ ~: I" q - delayMicroseconds(980);" P. @1 e& `; t' b' l
- digitalWrite(_DataPin, HIGH);3 `7 F6 Y) y/ r6 ~" N: i, i s' P% n, ~
- 8 ]: v% u5 l; A9 }
- pinMode(_DataPin, INPUT_PULLUP);( S' q& y, T1 W
- delayMicroseconds(10);
) V0 a8 M+ e( C. J. X4 o - d: x1 i6 J8 O+ E3 b) n
- time_out_flag = millis();( O2 `% J: c3 g( l, j- \% Q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 O$ v' x% D" Q! F6 p
: j) ]! s I4 Q( }( H- LOW_level_read_time = micros();) C0 Y$ ^" n0 D) k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out! d9 N, J. t: `! J0 T1 |
- {+ G, [" f4 C8 a+ V" D. B! r ?; \2 h
- return 0xff;
; n4 _5 A8 H! [4 C; E" k# y - }& P! {- k0 P7 P$ u$ H" M; Z+ F
- 8 A+ z+ R- H/ }
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 c1 k4 K/ ~) X. Q* M - % _# k% i5 E* G
- HIGH_level_read_time = micros();
/ k+ k: O7 }9 ]+ t$ o - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
9 h' _ O1 v8 z' p/ |- c
5 f- C& W' v# @- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% o# e) O! L4 N
- {, J R3 D% \3 U; A+ x+ q" k
- return 0xff;
, d2 q [( [ S- a - }# s: D$ m6 S! O: |* A* R( U3 H4 { |
. R2 A4 {3 G" `5 s3 z- q$ x5 g- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
2 j4 W" q# Q# s3 ~2 G - {& N3 {3 Q7 w0 ^) o2 t
- return 0xff;
8 j" F5 M& I- r( [% c - }
: [9 H$ w8 |& A' a$ @8 g
; j( K- g+ u& D! z- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ F8 S4 M; Q2 G+ ? - LOW_level_read_time = micros();6 h/ W3 w5 @& {/ l+ {2 e
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level1 B/ W7 c9 [! Q/ |9 U
- 0 c+ @4 y. ^% l: ~0 ?" N
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out$ p& `% B J. t' V- \: {, Z- { v9 r
- {# s6 z S. i( b: @8 v- g2 t
- return 0xff;
& x( g1 a/ c9 j+ ?; b2 M1 ?7 W4 Z* p - }9 L8 l; c9 M0 n3 I# X f
- 5 Z# p7 O' W6 T% ^
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)). ]5 [% r) j" t. T, `9 v6 c E
- {
) t! L6 k6 w1 g/ t$ r - return 0xff;
, g9 t3 q3 v/ a b9 S2 T - }" m! G8 c. ?7 G: D3 d- M) m
- . k& S7 E, w$ v8 S# D6 x; x+ B
- for(uint8_t k=0; k<3; k++): ^' Y) ^ l8 @/ I. V2 g* F, ~7 [
- {
, J L9 m& S ~& Z, q! I* H - Sensor_Data[k] = 0x00;- V' D% H) n9 S8 x
, h$ F) F1 y5 L9 S- for(uint8_t i=0;i<8;i++)6 |9 _) ~3 Y+ G+ `5 v* a
- {2 q1 j% M9 @9 h! Y6 V' A& ]; Z' U
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level0 p% \# w# ?- n, }* k
- HIGH_level_read_time = micros();( P- p( U: M5 w$ s9 I
- LOW_level_read_time = micros() - LOW_level_read_time;
+ a( w5 [0 y' g- R; a. e$ J9 n
6 ? M" B# a! Z D( W- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
, v5 s( U$ i- ?* O% G" E6 t" X - {+ a' Y Y7 h/ }9 d* [, |% \ w
- return 0xff;
9 m; P) _) l6 X! W) h0 {! b+ u - }
% P5 {: `7 b) f: H0 ?* Z
( R+ z" w3 V5 U! A" G8 B& P- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" ^# B) ~ G( K1 ~( O' d
- LOW_level_read_time = micros();
1 S2 N; r& m# _) x+ w1 R8 G0 i - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level. i6 h- P# m" a Q
- & b }( F6 W, V! y4 v
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1* T p) O: i+ B! H. h1 a7 U
- {
- j6 w2 H/ H) h5 s - Sensor_Data[k] |= (0x80 >> i);
l/ J9 T9 |' \* i$ }7 k! c - }
; D2 ?8 Y- D: k: q9 W! T - else if(HIGH_level_read_time >= 100)
) K2 o- [7 h2 ]# P! v* X+ ` - {
2 [$ v; l3 x- t, C# Q2 t - return 0xff;$ ]/ g" B7 U \$ a9 `
- }
" Z5 M" t4 z: R' v - . D4 Q; W& ]: _0 i; }. H
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
! _6 G9 j% Z& H. K; Q/ s - {
% _- p' J. u0 v; g! U1 |* n - return 0xff;
; }) y! _( o9 h6 w- Y$ I& Q4 t - }
2 V1 f8 e/ F. R+ r+ H, P3 }% A - }
% y1 e. ~$ Q# ~/ h - }
( Y3 g* p# c- j: D! F0 K - - M: h- m* e7 v& L/ A5 g* S2 }3 T
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level% |1 |1 i$ f# [% S6 ^" D+ H# y
- HIGH_level_read_time = micros();
, t6 m: W+ n/ ^: P v - LOW_level_read_time = micros() - LOW_level_read_time;) |& j o5 b* t# k1 }. H7 O6 Y
- ' }. Y7 a' k2 h( ]- W
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
' e; F- @9 n# q7 D - {
: x! B! o7 V7 R2 ]; W D - return 0xff;
1 \" S( \6 H8 H - }
3 z' Y7 _$ ~* k2 L - 2 v0 g V* B3 j p% u
- pinMode(_DataPin, OUTPUT);8 L# o9 J! u, ^: A' z
- digitalWrite(_DataPin, HIGH);
# ?- ^1 @& E( F C' G
5 q% L U) ^! O5 c- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
! I3 J% ` n% m( ~, \4 n+ v# g - {4 l- f7 ?$ n6 |$ d/ z: b
- old_data = Sensor_Data[0];1 E6 k4 J' d! q
- return Sensor_Data[0];
" ], i! N' z' c5 { - }
/ O# n; e4 `& b9 k9 f) z - else/ r5 H* L. p, p0 s
- {
9 C) V& h: I1 B8 r8 \, g; u" N - return old_data;
) W5 Q$ i9 q& L8 y3 ~/ {2 g - }
% ]/ U3 ~! O' O0 d3 e' B - }
; S8 k/ a# N4 @1 ~1 G# l. @
複製代碼
+ L! u" U* U4 h0 s, \! j0 dMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 G+ g0 g" F5 C" T1 p
1 s( I1 R# D7 A8 I' _6 ^
|
|