|
|
. _' H; e! y4 N% |
- #include <Arduino.h>3 N# b3 a0 _4 }% D2 `
- #include <MeAuriga.h>
) A: o2 q4 {- t9 t( b5 {; v5 Q - #include "MeLineFollowerArray.h"
0 Z$ A! D7 H: t! S: `
# t3 C" h9 K' P+ m3 n! A5 i6 L- MeEncoderOnBoard Encoder_1(SLOT1); C p: o/ }1 i# J3 r
- MeEncoderOnBoard Encoder_2(SLOT2);
, x4 p' ^' @% t3 q - MeLightSensor lightsensor_1(12);# h, x; X5 U8 F$ U& v9 Z" C
- MeLightSensor lightsensor_2(11);
3 O/ Y& x3 [- u - MeBuzzer buzzer;9 J8 a6 j, M. q
- MeLineFollowerArray linefollower(PORT_6);- j" i) l& l4 D! ?5 u, C
1 f8 N& ^5 E# M- #define Error1 1
8 x2 r! U: @ m! G4 {' ]* g: s - #define Error2 2
) N3 ~$ }7 ?; q4 r+ n" p& Y - #define Error3 3
8 P# D' m: S9 A0 W" z: M' K - c/ n, F; `; q" g- Y& T- k
- #define Kp 15
" N6 G) _7 U4 E4 z - #define Ki 0.15 B4 J+ w6 \# Q7 n' l+ D
- #define Kd 0.03
# X: e# T3 M# j7 O6 ]1 G4 L - 5 W2 k" ?! [7 o4 N) w$ e5 |
- uint8_t sData;
; x6 C, h( }' S, J% h - uint8_t D1;
- b. B4 }: _3 A - uint8_t D2;) K1 _, _& E5 q" j
- uint8_t D3;1 i$ r4 |# o! V, k# m. Z6 ?
- uint8_t D4;
) r8 g, ~* q; L$ a" _ - uint8_t D5;
8 z: N& X0 _4 d [. e/ f" ^ - uint8_t D6;
0 V6 r4 N) b: i4 D, H
4 G x9 ?) w% Y. ?. O0 ~- float previous_error = 0;
5 A; D/ o% n* l8 k0 m6 n/ n - float integral = 0;$ j3 f# V( N, I* I8 T& H# k; @! A
- float derivative = 0;1 R+ q, n% m1 H K
- int Speed = 160;
9 x& W- [7 H3 m X9 _ - float output; e- O: d# f4 c: t0 H; A
- % D; L: k7 Z( Y8 E. V
- byte Left;- u7 H8 p" d1 O$ }
4 {5 v: p2 @8 J) o- q" c- void setup() 2 N2 \5 D* {. Y7 ]
- {
: D( Z% O+ ?; |1 f9 a$ [6 i. K - //Set PWM 8KHz2 p, ?. ~ w* t7 Q0 K
- TCCR1A = _BV(WGM10);. O U$ X# e; z. i
- TCCR1B = _BV(CS11) | _BV(WGM12);. \/ m; e4 w u- ]; J
- TCCR2A = _BV(WGM21) | _BV(WGM20);
9 ^) R$ v8 [( ^7 g: X - TCCR2B = _BV(CS21);- u h" L: M% d; y- ^4 b- H
- Serial.begin(9600);
: R/ L9 H0 G) e! H - buzzer.setpin(45);- a' u& V3 e* C: ?5 N( b
- }7 f; v9 ? `9 \9 G; d% N% m9 K
- 2 G4 E/ K2 z. o. R
- void loop()
$ g5 x; l$ V& `3 M - {
4 Q1 f) M9 P0 S/ Q2 n - while(!((lightsensor_1.read()) < (10)));6 m- l9 z$ j( o1 u7 W# p$ Q
- buzzer.tone(1047, 500);; B9 Q: V( O$ w. ?
- delay(500);9 A0 U0 s( k: O6 I) P y
- do7 E" z* E$ L4 c7 d6 l/ D
- { Y# F1 G& ?; `- |! U7 [, z
- int Err = getErr();) W; n+ v+ v3 T. D4 a
- if(D1 == 1)
5 g( ]6 m) q6 t, ?) W; L& o - {8 _+ @7 E2 O- a8 g3 B
- Left = 1;& K, m! ]) M$ b
- }* r' B+ e8 I' V5 f7 _) d/ `
- if(D6 == 1)
' i2 a. l8 A# f/ y# v+ Y - {
/ p. _6 L0 g# [# ~ - Left = 0;4 p% L0 [% A- h
- }
6 Q; V& c7 k. b* q3 G, s% V - if(Err == 99)
' Y' j1 y$ ]! L - {1 A; q. l& u2 b4 J0 g; Q9 D/ h
- if(Left == 1) a- L) ]& }* q
- {. ]) b4 W* {- R' j
- Speed -= 5;
& E6 v# {2 i+ I" {& f3 w+ L - moto(0,Speed);1 O/ i% r; B! y% R4 g
- do
8 A3 o0 j* x4 d/ Y - {0 [ ^/ }" t& e7 A" ?
- Err = getErr();3 X: ~: ~/ Y' |
- }while((D1+D6) == 0);
_# X8 { T2 i( M - }
/ P9 o+ x. |: B# g Q- N' n - else
7 x9 O' Y ~) |0 S9 o" L) z - {9 D. F, v/ D0 ?; I! v; q3 p1 P
- Speed -= 5;( ~. m* n. x8 S
- moto(Speed,0); V3 ^% h* ?. |1 o: \
- do
, A8 \1 ~" h" Y5 M! }: ?' |# A" c - {0 q s/ B0 U0 O+ U
- Err = getErr();
) e. a$ }2 t6 w1 v5 x& t) ` - }while((D1+D6) == 0);# m7 R% G6 \( `- Z" B
- }: }5 V, k, b0 e7 K' E! v
- }
1 N4 \7 s2 F/ l7 p - else# ?$ z( }$ i$ R
- {
4 X( T& |" |/ U - if((Speed < 160) & (Err < 2)) Speed+=1;
/ I7 c7 t _5 ]: M' i8 r: L0 m - if((Speed > 100) & (Err > 2)) Speed-=2;
8 X) J0 R6 a! }. ^# A5 O - integral = integral + Err;
" T8 t; F: W1 S - derivative = Err - previous_error;( v+ A7 e) b* _9 b+ |
- output = Kp*Err + Ki*integral + Kd*derivative;' d9 M6 v7 r0 u- c7 f! J& @9 s
- moto(int(Speed-output),int(Speed+output));
. `6 J: Z; D K/ A% e& ] - previous_error = Err;
5 G4 W0 G1 Z, B5 C- N& a+ a - }
3 [' Q, U" L4 b - }while(!((lightsensor_2.read()) < (10)));
( ~* V' t5 ~. u$ U - moto(0,0);$ e: L/ _* g( [8 z, e2 o6 C4 Y
- delay(500);
6 v2 n/ A' B0 c2 Y - buzzer.tone(262, 500);
- y. t5 c( r# N7 @# p - }
+ _, p1 e, t6 f3 h% Q4 k
$ ]# I; E. S9 ]. I a- int getErr()2 x l9 f/ ~( V5 h6 Z8 B& W2 ]
- { * O0 |' s4 L6 w1 N% ?1 \+ a
- sData = linefollower.getValue();: G: H* C9 \& Y ^8 {8 X
- D1 = ~(sData>>0)&1;; u9 I9 Y( H: G
- D2 = ~(sData>>1)&1;
% n1 r d& A! X- M: O - D3 = ~(sData>>2)&1;
" O) ?! |& K# C$ b7 \ - D4 = ~(sData>>3)&1;; [2 C, x" \- L2 {* n% p( K
- D5 = ~(sData>>4)&1;, i) H7 R5 n# b5 k. e3 n
- D6 = ~(sData>>5)&1;
- Y9 z% S; ^9 X+ U6 @ - int downD = D1+D2+D3+D4+D5+D6;
" Y% \% N5 D2 p+ H - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
: m' q- Q3 v w& ?3 y$ o - if(downD == 0)
4 t/ G4 y: T5 }# X - {
* L' ^+ F7 ~( {% L6 Q - return 99;
/ T* s7 I0 d5 [0 A, T3 V" E - }
8 X' v+ Q: p- c' h% m - else
, V- |7 L- u; \4 H3 i - {
: A' l' j- K7 L' H# t8 B$ U6 ^. Y - return upD/downD;/ J; p4 M$ w$ f2 s* g- c: v. l" c/ V
- }8 F# w& _7 w# A) |( m. D
- }
; u; B: [0 S9 {" @0 s - 0 |7 L7 z/ g0 t1 Y5 N2 k
- void moto(int Speed_L,int Speed_R)
2 U, ]% J" ^. e4 z - {" f' ?/ z2 {- a
- Encoder_2.setMotorPwm(Speed_L);; h2 R, C$ ?2 e g
- Encoder_1.setMotorPwm(-Speed_R);. q; ?6 Q- I* t
- }
複製代碼
" [; m' O6 T! Q9 v4 ?1 \9 bMeLineFollowerArray.cpp5 G/ M. Z, e/ e
- #include "MeLineFollowerArray.h"# g3 ~# b) t; z0 W* i8 }
- % ~1 w# b$ ?) E, L/ J6 w7 t* [1 G
- #ifdef ME_PORT_DEFINED
5 E* P3 E/ n4 |2 X# ~ - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)* B$ \1 o" I% ]: N+ G( Q
- {
6 K2 c5 }* S1 _ - $ B j1 u# [9 X: P0 \/ c
- }
. a- U5 Q9 j; g) r - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)1 _- i0 ^1 i8 B! f. Y- _
- {: s* C3 D J( U8 U5 c# A+ B) p
- _DataPin = mePort[port].s2;
; _4 |' ^. V# q" d$ o* x6 @: c - pinMode(_DataPin, OUTPUT);
$ ~6 W4 y- g; x- e% P, G - digitalWrite(_DataPin, HIGH);$ T4 W! e: t, u/ n' j
- }
: a) H ?* W* F4 T6 g( B% u/ S - #else // ME_PORT_DEFINED
* f8 [* w0 H" p. I4 ]8 T% x - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
/ l0 x& Y( g b: @1 [- o* I) y- J - {- C" _2 ~) f2 U, {- j3 |
- _DataPin = pin;
+ T) w, f( G; ]1 X8 C - pinMode(_DataPin, OUTPUT);
3 A% I8 u. T9 T4 Y - digitalWrite(_DataPin, HIGH);2 _. m% v9 s" d, I& G& l
- }
% }' C ~" [" w9 W0 C5 Z% q& \ - #endif // ME_PORT_DEFINED
: X0 s: \7 Z# O+ w, U+ [/ V1 { - 5 v9 C0 f% }2 G; U* q
- $ U5 r* f( y0 n; B2 v/ D, d y3 F
- void MeLineFollowerArray::setpin(uint8_t pin)4 |. t, [9 o! e) N: U7 S% [
- {! j! @; T, M* B; f& X
- _DataPin = pin;; P! b, ^4 v: W1 m6 \2 B
- pinMode(_DataPin, OUTPUT); 9 b# U, r% ]& J$ l: k! W* D4 ^
- digitalWrite(_DataPin, HIGH);
% K$ w2 Q! ~5 z, ?4 l I2 ` - , {* m( `$ N4 Z, S, r8 \' J
- #ifdef ME_PORT_DEFINED+ C3 E' X. I& e; V5 D2 a1 t7 f) ^5 h
- s2 = pin;
& ~, ?1 h+ ~/ d' P8 }0 R5 O - #endif
) @" }5 A) F. a; U7 u! f3 O" @; o - }9 c" } T: h* ~% m7 w. t( X
, v# @; U8 q0 l" ?- uint8_t MeLineFollowerArray::getValue()! N5 Z2 a# h7 b4 S! d7 `
- {
/ C* ~/ Z6 ^) T/ p1 |) Y$ B - uint32_t LOW_level_read_time;
2 ~" K( B% w0 y& r1 |$ [) m' M - uint32_t HIGH_level_read_time;
) I: @2 u9 J# ` - uint32_t time_out_flag;
9 x0 \6 J/ v9 p2 A8 }' m9 { - uint8_t Sensor_Data[3];3 ?, z9 ?5 I4 k# U1 B
- static uint8_t old_data = 0xff;
9 e. y- P* Z) H8 ~! p - / c; c! P8 W3 L; a2 X
- pinMode(_DataPin, OUTPUT);
0 d/ m% m5 [$ d9 V2 D2 w6 U - digitalWrite(_DataPin, LOW);" I4 A* Y ]! [6 i7 Z
- delayMicroseconds(980); ^0 ~, V6 q. |- e# o! G- K) x6 f
- digitalWrite(_DataPin, HIGH);* B/ F+ p- x) p( Y$ o6 v: C
1 f- R" y# b2 @1 `2 ^, U- A- pinMode(_DataPin, INPUT_PULLUP);
( _; T# b* h0 G* l3 T$ p+ k3 g - delayMicroseconds(10);
, y+ L/ Z8 Y* z p6 }! g' x
4 @2 l e, X) A- S- time_out_flag = millis();
0 J8 [0 E9 K' W% V8 }: e" Z0 y - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );! b. ]: b! v* H$ C: N/ y
- # h4 F9 }- U; J- ~8 M
- LOW_level_read_time = micros();3 v+ H0 O: U* X! }* g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 }8 L3 ^9 Y: b
- {
0 [+ N9 t, j, l; T4 P - return 0xff;+ m* s3 G5 C+ B a7 ?: q
- }
2 t9 D' K$ e( g+ W5 d# J6 B+ S
9 \" D0 [0 V: A- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
1 j* s* P& ~. E8 U7 P6 ^6 `
8 ~& Y8 u" Z; }8 [! [0 n- HIGH_level_read_time = micros();
- O h; X; l% `0 J0 i - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
) j, a) l5 V! ?4 x; g' n) V; d
/ v H& ]+ a& G* n0 `- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 o3 X2 `6 A) l# C - {
) Z9 w U, `: f$ [5 x3 ^ - return 0xff;
6 g9 g" g3 Q ~0 D# `) p - }( U0 l, J. R7 F5 v# t R+ S
- / Q' ~7 D3 Z( Q
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
0 q2 D7 A" F1 K6 x - {
+ H/ T: L" A- M- n3 q7 @* d7 q - return 0xff;
3 d r2 \" P8 ^# R) r: R6 U( Q - }* x) F; T: w) q) h* u( g5 ?4 Y! i
& {- P: S) z4 s/ P- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 z# ^$ G% w7 z, ^* S9 d3 D) y
- LOW_level_read_time = micros();
3 ?! }) s H/ l7 a - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
2 z) I$ ]2 a" a& t8 u
9 U l. ]; l3 U( ?' q! z/ l( D9 v- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( x% {+ s1 a; ]1 ?* L8 \) Z - {. {: V' H1 H; ^
- return 0xff;8 V) v0 L7 R6 J/ ?
- }
& g, O3 z; u5 w2 \5 B% n - . { _0 g, M1 l8 g! |9 H
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)): G5 O& ^1 u! I4 i
- {
( }: T; z/ E) [- c9 P: X1 o8 | - return 0xff;! x7 @! R6 [- {9 l* E* n. B
- }
1 [2 T2 ^) X2 R. a& L8 b" j - ; K% H( S2 Q5 x t! Q8 a
- for(uint8_t k=0; k<3; k++)
; x+ |: l2 T8 A8 K, S: Q4 w - {
# I4 j4 P% U3 t. i - Sensor_Data[k] = 0x00;
8 \% h/ x% [+ }! _( n' \
3 e9 u. h1 Y: i2 p6 q. l% Z- for(uint8_t i=0;i<8;i++): l' F, Z8 w0 y$ S8 Y( _
- {
' f. ^$ q. Q' c, k! Y - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 K9 V H4 V. t; N" y. m8 n - HIGH_level_read_time = micros();
8 G: a/ e1 s d0 z; E5 b" L7 p - LOW_level_read_time = micros() - LOW_level_read_time;( y5 m! s+ D3 v5 G1 `7 v" h7 L
- 2 j2 w( G8 F* t9 Y2 w0 }
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
! v+ F% j4 @ [$ V - {
! j2 T0 z+ d0 e( o& b; [ - return 0xff;
. K% l, s* l# g p: ?* I" ^5 R9 u0 K - }' [1 y5 D( P, v3 t8 }. H, c
( S. X4 q$ S/ A# i) B4 s- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. U4 k' m; ~7 V* ~: A
- LOW_level_read_time = micros();
5 p0 U2 Y- G4 v( w; O( d - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
, O% k4 [' w% z) a6 `4 ~# M - ( {* x" P% m1 y: E5 P& l
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
4 D/ q* q4 z5 h. W2 b% \* P/ k - {4 Y4 x/ O& J8 F0 a2 @/ b# ?
- Sensor_Data[k] |= (0x80 >> i);+ |: k# ~- x( V
- }& m( a- n8 n z6 p: h3 T9 P7 K
- else if(HIGH_level_read_time >= 100)) O$ |9 [, }9 u. v! P
- {& w) k6 R N3 J1 d1 V9 Z$ w
- return 0xff;9 b; D/ v$ k* N! g& T$ b
- }( B! [" x {3 j9 H; E
b5 n, f _9 |4 ]7 h. ^- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)7 j, U# u) ]5 `2 k5 y
- {; x* l+ h U/ w7 n! j* `6 b
- return 0xff;
% G( j' ] c( D% \% Z - }
$ ?9 j, A/ i7 A& L7 t; Z - }
' ~7 L3 f: U. V6 p, W* K% y* d+ K - }
- c! L7 m) [# e' @
& l W+ T0 m& G( s- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
% s7 T; `2 W4 K. g0 B4 Z - HIGH_level_read_time = micros();
# c0 z! E$ i* r; ^8 T - LOW_level_read_time = micros() - LOW_level_read_time;
" J1 q4 d6 u; |6 S8 c8 T* ^ - ' J" K* l8 L0 J u+ m; h: Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
" i' p$ N, e5 i2 L2 ^* q - {
s- L2 Z+ N. \9 H7 s' @3 ~ - return 0xff;+ K# l% k% @6 D; S
- }
. [( m8 ]+ `7 `8 D8 K8 B
# R) L+ ?. f2 x' U, X8 r- F- pinMode(_DataPin, OUTPUT);. U1 J; R5 `/ u% k, m! G7 f/ q
- digitalWrite(_DataPin, HIGH);
) o$ ^" V5 A# o" V) v9 t
" L" y' f! \ l& v7 S- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))$ o9 P+ Z t; S2 t; _
- {3 z! S9 b6 |2 j% s$ T" Q4 M
- old_data = Sensor_Data[0];3 z9 _: r$ A; B S! E3 P
- return Sensor_Data[0];6 K& r# m) @ O! E: l
- }
. D$ d. m% B( a3 f7 F2 ] - else4 ^/ ]! y* L/ |
- {
3 i9 K6 I) H5 ?* K3 I: D - return old_data;# u% @) g8 _8 c* k: M7 n
- }) ~; M( N( ?# d1 N* Y2 k1 }3 r
- }
) C3 _5 o6 C6 C, V8 U
複製代碼
6 e$ |. Z0 q& I/ R6 d# @MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
7 y1 N8 `0 J" j" E& `3 {
' z* ^. }; r l+ T% d, Y |
|