|
|
* x7 x6 C4 a& A; _7 _: Z2 C
- #include <Arduino.h>) f# I) T4 T% @& s
- #include <MeAuriga.h>! |& A2 c) R' h5 f6 _
- #include "MeLineFollowerArray.h"
7 b2 D* }: F5 N+ y9 b- _) h; { - ) ~$ }/ l# x- {' X k
- MeEncoderOnBoard Encoder_1(SLOT1);7 P$ B0 y9 o! U- G$ F0 K
- MeEncoderOnBoard Encoder_2(SLOT2);
, Y, p. J# D( T$ V% z0 i7 A - MeLightSensor lightsensor_1(12);
0 g0 R3 B. s \ - MeLightSensor lightsensor_2(11);
# `6 \1 z1 e+ m+ ^2 p& h6 R$ I - MeBuzzer buzzer;
2 u) s( \: N" T9 ]# B - MeLineFollowerArray linefollower(PORT_6);- `+ w: V1 O/ N- `+ k5 X7 p
1 T6 r6 ~/ g% [, |- #define Error1 1
/ z0 D& K( y" I/ p/ I8 A - #define Error2 27 \# `0 Q, @9 W+ D G* X
- #define Error3 30 V8 g! L0 G1 y( g
' R3 {* e" `, f, z) }4 ?; P- #define Kp 156 d& g( M9 i& K4 s) j- ~1 {" ^
- #define Ki 0.15
% p. B: J& A2 s2 K4 H) m" B- M - #define Kd 0.03
% d) C K# S0 B5 x
, T- {8 R' q( C6 R- uint8_t sData;
! Q+ e+ l* e0 @) g% {/ m% g - uint8_t D1;8 H$ [! D! x& z" ^. r0 G- E+ e
- uint8_t D2;
. [( \* i8 ^) x' ?+ a - uint8_t D3;9 W0 w1 o: w/ f
- uint8_t D4;5 K0 F. U8 \! |* N# D
- uint8_t D5; }+ R3 s8 N' O! s1 [- C- i& D
- uint8_t D6;: J/ m- B( |! A
3 U8 ^4 N! a( C. R7 g# d- float previous_error = 0;$ g% T1 k7 M: X E
- float integral = 0;# D" \6 |4 e0 e- }
- float derivative = 0;7 q% c4 Q5 N' [# F! }3 h( \
- int Speed = 160;
* o- l, s6 u" i% t- ~ - float output;9 C4 f% C- C! p1 z
- ' ], W, g8 K; |( c" E/ a
- byte Left;
9 Q& W8 X# e- {+ f& e6 W& u - ! Q# W6 i/ n+ L; [
- void setup() 6 ^/ n- B8 [$ ^" o; V0 e
- {
0 G- f( f+ r+ r - //Set PWM 8KHz
5 G9 d; i4 K& X" @ - TCCR1A = _BV(WGM10);4 `* v3 [ l- y
- TCCR1B = _BV(CS11) | _BV(WGM12);
+ d9 K6 Q2 I( c# u0 v0 F9 h - TCCR2A = _BV(WGM21) | _BV(WGM20);
4 g1 z* T; b+ c3 m$ h7 a7 V - TCCR2B = _BV(CS21);# j7 t6 x X8 m% q; N& j/ ?+ R
- Serial.begin(9600);$ u4 V, L4 ~" [( m
- buzzer.setpin(45);
$ e0 A8 t( z8 f: I - }( s5 s' S, S, k' k' b7 o7 l
- : `1 r, u$ m3 u6 N2 K8 j) A3 t
- void loop()
" |% m1 p L Y4 O; F/ u S - {: Y1 j: L/ _) A1 Z* J: O5 f' a
- while(!((lightsensor_1.read()) < (10)));
, i+ ^, Z/ x2 _; r$ | - buzzer.tone(1047, 500);5 j& S# F* j% [+ N# o6 Y
- delay(500);( I- y; ]( t- W8 w* x. w4 m4 t( I
- do/ J6 V4 U1 C' z5 b! Y, T* k9 h
- {" M0 f% P0 t& S& z3 O. w, e9 H! ]
- int Err = getErr();
1 v0 H; c6 w9 v9 \. t - if(D1 == 1)! H- L! |$ |6 J# F+ y7 t$ x
- {
, u4 V7 T3 J/ ?0 f - Left = 1;
5 Q, f6 z4 f( \6 r$ \! S - }
; t3 b* M s8 l7 C7 ^ - if(D6 == 1)
0 ~: C! a, G7 R# y6 u - {
- L7 m% V/ V- M* k - Left = 0;( U7 P" s) w8 b) {
- }
! n9 i3 w# z" e - if(Err == 99)
* Z! ^' E- z3 Q, F5 M9 f; E& Z$ K( u - {7 w# L- l! b# b* `1 l3 V, {# B
- if(Left == 1)
7 U/ ~; ~; t; p1 e - {
, I% |/ u! y" J: r6 K - Speed -= 5;
7 K8 m5 q7 L- y3 \( [7 e - moto(0,Speed);" ^ p- J* Y. l5 O2 l! h, ~3 j/ {
- do' z7 u' s- C4 r6 Z
- {
( \$ Z1 d% _1 @' @! L- T7 a; r - Err = getErr();& Z7 [7 g/ u5 K& J6 n
- }while((D1+D6) == 0);
# {, I3 Y% W! c$ @7 Y# U6 m; M& a - }9 o6 g- h; r ]$ a
- else Y- g3 `' F) P( l. g
- {
5 I1 |$ k! Z# C6 C( ` - Speed -= 5;
3 Z3 Z ]& G& Y- c/ G: t) S - moto(Speed,0);
. d6 l# C" }4 F4 x% t2 j+ x( J8 a - do
& d: }1 R# w# N- w - {
0 p7 p/ p [9 G( F$ B { - Err = getErr();0 W: j2 i3 R0 _' W
- }while((D1+D6) == 0);
; B1 o: y4 B: a7 T - }. d' T, T& g* @! I9 D3 l- C
- }
( Z- |6 h6 d3 i# p& Y - else
. m2 A: A: a( N6 r - {8 s& d$ F/ `7 L5 F
- if((Speed < 160) & (Err < 2)) Speed+=1; 3 e: Q T, H& q3 m. f; D
- if((Speed > 100) & (Err > 2)) Speed-=2;
. M9 K" t- J2 G' |# c6 `9 K0 [ - integral = integral + Err;
* Z' Z# B1 `. ?# m" X; Z/ N - derivative = Err - previous_error;
, u* c2 q4 U/ ~# f* B3 R - output = Kp*Err + Ki*integral + Kd*derivative;! }4 j0 z" q4 B
- moto(int(Speed-output),int(Speed+output));" W j, r4 D7 l d7 X1 K
- previous_error = Err;
/ t9 i8 [+ B" Y6 s7 R/ \8 D7 |: f: N: | - }
6 {2 a. J4 D: Z - }while(!((lightsensor_2.read()) < (10)));) o5 D# U8 [7 w
- moto(0,0);+ s3 J' l& Y4 G/ P# z# t/ R
- delay(500);
& U& v/ A5 s* w% m - buzzer.tone(262, 500);
9 N$ }! I) w# D6 u+ ?& S - }
3 h9 X/ W( P0 N# a9 M1 B - + w6 q* w" r- e
- int getErr()
2 d6 n" t$ G( @ - {
9 T( P! M! R5 D6 }; T - sData = linefollower.getValue();+ h1 Z, ~/ M" b% ^. H/ D$ A
- D1 = ~(sData>>0)&1;& ~" P9 u; s* Y5 Q: ~; G' ]
- D2 = ~(sData>>1)&1;
$ W) W" C& W2 m- R# ^( A+ A - D3 = ~(sData>>2)&1;7 o6 @0 w$ Z1 T0 R
- D4 = ~(sData>>3)&1;9 `" f) H" t4 R, \
- D5 = ~(sData>>4)&1;
4 ^" a+ N5 l+ H4 c }& } - D6 = ~(sData>>5)&1;
3 q ]& H- q0 p: b9 i - int downD = D1+D2+D3+D4+D5+D6;, O$ ^: S: W/ y) h) {' C, G) `( ?
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);5 S$ e6 X Q- |9 |
- if(downD == 0)1 m. d9 c. V. M8 j+ \# J
- {
8 R# g9 j# M9 U' n" A - return 99;
3 ]2 s4 O4 E+ J: Q - }
# q% K1 b, }/ G( F2 k - else
+ ?/ d) O- l( \2 k% ?# F - {/ m4 l9 X+ Y+ c
- return upD/downD;- V5 G& a: t, o. }6 K
- }
6 H- g& Q5 B( n" \4 ?1 P: ~- Z - }
( x+ z3 }' n$ D O" l8 b( r# R
H/ p9 C7 y, {9 @0 H' O- void moto(int Speed_L,int Speed_R)$ f$ p) b, a6 M' K
- {
1 q# H+ |% _2 @6 f+ Y0 z$ u - Encoder_2.setMotorPwm(Speed_L);
" U% |7 {# x' T$ O! M1 z - Encoder_1.setMotorPwm(-Speed_R);. t- N6 `' }" ?. m9 |; u$ ^
- }
複製代碼
3 f5 n9 C' V, c9 S" w- yMeLineFollowerArray.cpp
0 Q0 O' H, c7 F- #include "MeLineFollowerArray.h"
5 ?5 q; B" `9 h9 h) k - 8 i8 k3 p! B: ^! s
- #ifdef ME_PORT_DEFINED: m4 P" \1 u( a' ^9 g
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
2 f" t& t$ W5 J( M. M - {
" @' ?+ z2 f" G, A! h8 ^( P9 k
! Q1 {7 Q, I) \7 [. d( T- }
% T* m/ z; u6 n9 f - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
) M( C4 n+ Y1 X9 p! e# S - {5 T( |8 S) D2 `% G
- _DataPin = mePort[port].s2;
2 L7 _& ?; Q7 w - pinMode(_DataPin, OUTPUT); 8 U- h* ?# b! X6 i! C! l9 m3 g: M5 j
- digitalWrite(_DataPin, HIGH);$ i' Y; ^$ S* O/ b! {' t
- }+ Y6 O+ r4 c" [2 W) [
- #else // ME_PORT_DEFINED6 @0 s' Q7 J* f/ {
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)% W9 I/ A8 E, T0 V( ^
- {
) w; |7 Q6 h# \ - _DataPin = pin;
* J7 \2 [& {, P& A: F - pinMode(_DataPin, OUTPUT); + N1 I# ^0 ?+ `" |' [
- digitalWrite(_DataPin, HIGH);) \" R4 \9 W# K9 \
- }4 [+ Q: F/ V& {5 n6 ~
- #endif // ME_PORT_DEFINED
V" m$ y) U# y' X4 p; P - b# ~# g$ N- D, p D
* Q, J" Y4 m1 i0 e- void MeLineFollowerArray::setpin(uint8_t pin)
! \/ E$ W) I$ W! U5 w# k2 ` [" x. D - {- Y/ m/ Z+ F- b/ ~9 T: W- T
- _DataPin = pin;
5 J! [% z$ T! E9 _. K! z- q - pinMode(_DataPin, OUTPUT);
/ a5 y; \8 i) d& p - digitalWrite(_DataPin, HIGH);5 c1 P/ z8 t# O* b2 `; j
5 ~4 v5 z* ]* E* q; w4 _0 ?) c- #ifdef ME_PORT_DEFINED; k0 c6 H! [( k( l( Z
- s2 = pin;
& w/ `* y+ ?+ p! `3 w - #endif
" ^6 k% a2 ?! |' ^. f - }
% _4 [1 B, e6 q& a3 ~$ c5 s
. B* {$ q' L9 B! {9 j3 L2 A- uint8_t MeLineFollowerArray::getValue()
0 N7 j6 e r; L2 i/ W - {
. g# f8 A, V+ K - uint32_t LOW_level_read_time;
; ^. {7 t+ U3 G/ Y) P7 M1 ?' _ - uint32_t HIGH_level_read_time;$ F6 o- B, P. y1 W# z9 u
- uint32_t time_out_flag;: Q: o$ y1 Q$ @- W& j$ T
- uint8_t Sensor_Data[3];6 [( Z- `( j& \$ D
- static uint8_t old_data = 0xff;6 v6 i0 D+ D: \& x$ Q' e$ n
- , m4 ?0 l. p/ @: @. q
- pinMode(_DataPin, OUTPUT);
" X; a1 A1 j6 s& D - digitalWrite(_DataPin, LOW);8 A P( L) H0 ]0 o9 {5 `% w
- delayMicroseconds(980);- |0 ~* f$ p( x
- digitalWrite(_DataPin, HIGH);
4 a. U! T' Q: A* C5 K - 9 ]4 b- L3 X- e1 y# h/ O- m
- pinMode(_DataPin, INPUT_PULLUP);
9 N! [2 J8 N+ Q. |4 v; r0 O" x1 N0 H - delayMicroseconds(10);- t1 M j8 C' O
! `! E1 \1 j: g5 S- time_out_flag = millis();3 u2 W+ g. E: Y; K
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% A3 D: m" o* r" o - % Z% ~8 X' `# V# q, `
- LOW_level_read_time = micros();
% I% H1 U3 \3 U/ {9 b2 S, C - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* Z* P0 N' B6 P8 M6 n E - {# q: F- g) p& \( f( @0 t6 h
- return 0xff;! m3 p, \% C( b' p* s
- }
+ b/ w5 u" W' j2 ^$ k - 1 `7 G( m4 L+ r. ?, z
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 Y! {' p* i) V
- 0 z7 M! Q- G9 Y- Q& r
- HIGH_level_read_time = micros();' A( T' u8 _; q5 G/ ]
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level+ c! E- G% |5 S
- F( d/ ^, _, W& v* v& a
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 y0 U; p8 U- _$ s9 B* h2 ?
- {
" o( ^- U/ V) r4 B - return 0xff;
c" M* m Y. }# l - } t9 Q" b# [$ d3 D, J Z' }& R
- I. p1 E7 W# u% { `! T
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))( [* A- b# Y. K& u- @9 E
- {
! o2 Q3 z. C+ N$ h u7 W - return 0xff;
! f2 f, o9 b" [0 E - }
) ~6 l: }9 ]( c/ ?) i, K - % I3 g. \" o4 r6 L/ ?9 _
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' j: x( H$ y, E# C3 ~) S" v% y
- LOW_level_read_time = micros();
( R6 k. U" l' T! k: @+ R - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level w) ?+ W, w6 h
) S9 m7 C- [* y' p; z9 t- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
, ?% O' w4 _( c, ~, t - {+ N) O, u+ N/ ]& |+ G; j, K
- return 0xff;; e R: l3 S9 _% v9 I
- }
9 h$ U; o- Z, v* l+ a - 2 }: t4 D" N/ v! i0 p
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
: a1 U7 z, Q. N* _& g - {
3 a( u8 O3 \8 e1 s3 z! {5 t - return 0xff;5 M8 x, n+ P% N
- }
4 w% @8 e& h4 h: U - ( Z, B( f @ s
- for(uint8_t k=0; k<3; k++)6 S$ W; {- H# ^- T
- {
8 M9 H; Y# \* z - Sensor_Data[k] = 0x00;
2 {, P8 @1 I: r% _" F - " i7 `2 ~- t7 G+ f' M; A0 L" W
- for(uint8_t i=0;i<8;i++)
2 E% l9 @: O* B4 j/ z - {
% C+ X: ]" [- A* h3 B* r - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
, |, K: P/ l/ ^0 f - HIGH_level_read_time = micros();
/ Z7 k2 B6 y: R, T' Z# D* g - LOW_level_read_time = micros() - LOW_level_read_time;
7 k+ R4 M& w& g6 g O* ]' s$ }6 I - $ X4 C$ _& _+ F
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )( t" Y1 h0 i9 [) t2 h3 l5 g0 w8 Q9 a
- {. d, J* ^( R5 B/ P1 k, W
- return 0xff;
7 b) b. n# O7 \ - }
7 Q' h! L, n4 Z' L$ E+ c" U/ E
. c1 G$ x i! i9 U) C9 a- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ N% c ?' T1 h5 y# q# P
- LOW_level_read_time = micros();
7 A B+ T6 }, u, t - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level6 R+ J" f4 x) n$ N% [; D
- + |4 r, O# p8 ^7 [ |' m. ^
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
0 ]+ f1 l1 |/ r) m8 O - {8 e! N0 i# O- ~. q4 U
- Sensor_Data[k] |= (0x80 >> i);& w* d" D9 Y; U8 \& ^$ ~
- }
5 X- h$ e; @) S$ n$ x7 N { - else if(HIGH_level_read_time >= 100)
* ^+ ~4 D z; n$ o- D6 v2 ^: H) \ - {" b! p; B3 Y' V9 c
- return 0xff;
. q% u' t4 [6 q* q2 g - }) K8 j, B3 E' v* I
3 V; ~4 b: N R u- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
# B1 T2 H+ F0 ^9 ~8 @ - {1 X3 G' e1 I' w6 b/ v
- return 0xff;, u! }$ v- ~ I% u3 Z. K' s. _
- }. X8 x3 ]1 u, D6 v2 A) @7 L
- }# H0 @& R( j; ~! r# i
- }
/ N4 a4 \0 D2 x' a6 i* D2 B
+ G4 j/ {0 W: a* g: o0 m7 h$ E5 N- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
8 z1 p% g( j1 Q& o - HIGH_level_read_time = micros();
: J9 W& ^4 Y. d; r$ m - LOW_level_read_time = micros() - LOW_level_read_time;, d3 L, d5 Z7 x! K* Y5 X; K
/ ~$ |8 f. e4 k% Z- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) I0 [, ^8 Y4 g6 M" F - {; s2 d1 F/ e7 _4 v/ y. `* Q0 t
- return 0xff;
, Z8 ]$ \# C! _; t |# H3 f' } - }
. W0 F3 l' B6 S9 p/ B" j
e7 ~# _) U8 ]8 o# L- pinMode(_DataPin, OUTPUT);
) Z: }) l) O& {8 X - digitalWrite(_DataPin, HIGH);6 g& m( B) E9 `
- & e( C2 C5 P4 f$ t% Q; \1 q; u
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))2 [) u9 ?5 z. n
- {/ x) R; f, D4 M' A6 p: }
- old_data = Sensor_Data[0];# Z/ B$ V7 P* o4 D0 s
- return Sensor_Data[0];
9 J9 v9 V: v1 V, {. q/ @/ {2 b7 Z - }2 e J- w6 \, q4 ~' R* O" O# e; V
- else
: Q7 {9 N S8 o8 g6 N& d) K - {
1 S0 f) U( i" Y2 t7 h - return old_data;+ R0 y( G% F! ?0 j
- }; j9 u, s/ v6 h' Z" a8 e5 u# Y
- }' F0 K' P7 e, ^+ ^" y
複製代碼
+ t1 _0 J" M' ]; H8 _) hMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
% U- {# `% ?9 c- [5 _$ ]9 A' R
3 @$ p9 k. w- ~4 n# h
|
|