|
|
/ M: y' i% d! ]; r) ?* A
- #include <Arduino.h>
3 U; H0 |+ p) M8 m - #include <MeAuriga.h>( |0 [4 m! X3 c, w. @) m
- #include "MeLineFollowerArray.h"
' Q9 n0 V9 j+ n. a' @
* A" \9 W, o+ q" |- MeEncoderOnBoard Encoder_1(SLOT1);
& T9 C9 {. \ v7 i# M - MeEncoderOnBoard Encoder_2(SLOT2);
T6 B! c5 r. D, _2 t - MeLightSensor lightsensor_1(12);" L/ M5 @2 k8 q( c4 a2 \
- MeLightSensor lightsensor_2(11);: ?- A+ Y9 D1 Y! E, x+ s4 b
- MeBuzzer buzzer;
4 y$ M4 w& g) X4 s7 ?& j6 D& O, V - MeLineFollowerArray linefollower(PORT_6);
: T) Q7 ?4 t. s# f
" |1 s3 y. k7 Y `- #define Error1 1; E! i1 S6 L4 w. P; ?9 A6 g
- #define Error2 24 `( {( n: k7 D. l" V
- #define Error3 3" ?+ F; q0 S7 Q6 Q# F6 Y
- # Y9 g: \; f- k! H4 q/ F# M6 W/ ? A
- #define Kp 158 {6 a6 n2 O+ I2 K3 a8 |: H9 Y' @4 n
- #define Ki 0.15; Z7 |" R( V% Z( d; |
- #define Kd 0.038 u7 Z& a/ S' y8 q, z) `
! x; v$ V }2 g( O! B5 L9 _- uint8_t sData;
5 h; D1 \5 X7 m# J* t - uint8_t D1;& [' Z. n8 \1 |- d$ J+ f5 G# T
- uint8_t D2;5 e+ G$ f# k0 E2 @0 i- O
- uint8_t D3;
( [5 y S. V4 t8 a0 q* O, n) N - uint8_t D4;
) J3 f; v6 d" D4 b/ z. f - uint8_t D5;; [" j J+ p4 E! ~
- uint8_t D6;# f$ w/ P P5 u
- & h: Q8 @) O( c. C9 D
- float previous_error = 0;4 |2 O" g5 E" [' j0 _
- float integral = 0;
6 t4 l" l# v" @& c% J; x - float derivative = 0;
& n1 N& [. e' F5 U+ b - int Speed = 160;
8 g& z0 A* ^, g& j5 B - float output;# y7 a; m8 A+ g
- / W, g2 z8 t" \% m, o+ c# S
- byte Left;8 m& B4 ~1 a' T
- 0 W& S9 T3 }* Z Y8 ?$ o
- void setup()
) o# S7 a* D* ^+ k - {
: p A3 p f. b - //Set PWM 8KHz* ?, O/ f+ X5 L$ T j6 c# R
- TCCR1A = _BV(WGM10);, r) A" b" q3 S% `3 \0 L) F
- TCCR1B = _BV(CS11) | _BV(WGM12);6 `$ p4 d: S/ w) K- b
- TCCR2A = _BV(WGM21) | _BV(WGM20);8 ^% `$ U z0 u) O; r) s
- TCCR2B = _BV(CS21);# L1 f$ j6 L f8 ~$ M
- Serial.begin(9600);
/ v- Q2 d; x, I" \9 ]7 k5 ` - buzzer.setpin(45);
4 @' T* W! Y# K5 c( x - }; Y/ u# s+ ^5 S! C8 F; _( {; g3 s4 D
- 7 G2 E5 |3 n2 f' n+ q& e
- void loop()
9 A. ^9 i! [* n# ]% B3 K# r - {5 N9 o9 l: {1 _* `/ O* S9 w3 g
- while(!((lightsensor_1.read()) < (10)));
4 A. E8 G( ~) D" U& u4 G$ ^ j - buzzer.tone(1047, 500);, w" S. o1 S; G! m% D7 ^) L- F! ]
- delay(500);
) D. E+ O5 K2 V7 }5 R- [* e( Q' E! A - do
0 U) J% ^9 K. D2 b M - {
0 H& L* V. V& {- b - int Err = getErr();* q2 } q) Z( X E! {$ \: X
- if(D1 == 1)
! }( _ D, T$ q) C9 w - {; J$ A+ e, e6 X4 Y& h* X; E
- Left = 1;* r G0 ~( d t
- }/ @0 ~7 J1 {% N' ?
- if(D6 == 1)
- v0 o; G& U8 n w# V( } - {# `6 Q4 Q" }* Q
- Left = 0;
- Y% P. x( K5 q& _ A - }
( b! z- F2 W( Y - if(Err == 99)
% g) l8 v! e6 X: h7 f9 ] - {1 M v+ H- f8 K: P) g( N! v
- if(Left == 1)/ L/ @* X2 S1 w- s
- {
* A3 U: {. Y% E- G' ^' d - Speed -= 5;! y6 k6 @7 z- L: U `2 W
- moto(0,Speed);4 _7 ]" }0 c+ b0 _ |, o# b
- do1 n# Q5 m% m9 \- y4 q6 V
- {
E( e$ Y: c" t" N/ n - Err = getErr();
& n# F8 f3 f/ p. U - }while((D1+D6) == 0);
' b' N9 k& Y$ C- o/ r C& v - }5 {1 N. t! ?- Y1 ^- R5 A5 N
- else6 ?$ C# j9 z% O* T; F# n# ^! B
- {
8 q' B% E. J2 F) X7 Z7 \+ D9 x - Speed -= 5;
4 F! E; x0 f" J( O: p( A3 H! G - moto(Speed,0); ) K; c" U: B1 J. e$ S
- do
+ i3 O4 X6 p% Z3 ]) Y: b - {
5 S; C6 T- K' }7 y, m1 _9 ] - Err = getErr(); A' F0 \2 D' U; _7 O
- }while((D1+D6) == 0);
" G3 Y" `2 S m( v2 S: I - }
3 ]4 B5 F* V& x: r9 x! D8 G; I - }4 e: K# i: D: c# `2 N, R: f
- else
3 Q1 k5 ^1 k# R# J! w( | - {
5 H: [4 m' }: [3 W0 P - if((Speed < 160) & (Err < 2)) Speed+=1; " x/ [0 ?. B* }
- if((Speed > 100) & (Err > 2)) Speed-=2; : V" I# A) ~( ~+ s
- integral = integral + Err;
- o! k3 u$ N8 n3 S" M- K4 n$ b - derivative = Err - previous_error;
: y( k. J9 y2 y - output = Kp*Err + Ki*integral + Kd*derivative;/ H/ H# O/ p4 ^; i) o3 x
- moto(int(Speed-output),int(Speed+output));
1 Z4 z( I. k' H; N( f - previous_error = Err;
3 o, F$ s& a% P [3 N - }
0 N9 I5 L0 {/ k% g j - }while(!((lightsensor_2.read()) < (10)));
7 L$ o( T. O) m - moto(0,0);
: j+ R8 g/ I! I# i U& d& l - delay(500);
3 y- G2 Q3 ^% E - buzzer.tone(262, 500);
. W" I" c+ T, q! I$ n& x - }8 ? I, e) f, m0 w
7 b: M5 Z$ ?! P* z- int getErr()
' B3 o0 O8 j4 r2 L - { 5 M6 D* `# B5 }+ A
- sData = linefollower.getValue();- t7 ^; V# I7 w3 }
- D1 = ~(sData>>0)&1;
: O) Y+ j# S4 `3 S$ y - D2 = ~(sData>>1)&1;. c4 ]& C; k% N5 [; k4 w
- D3 = ~(sData>>2)&1;: z! \" h- a* \7 c- Q
- D4 = ~(sData>>3)&1;
+ C( u S% @. \% M e: W* W& Q - D5 = ~(sData>>4)&1;
: q: G5 K% w2 V2 H# Q - D6 = ~(sData>>5)&1; \2 E% t( t, Z3 x# @
- int downD = D1+D2+D3+D4+D5+D6;
$ l" s' p7 f- I" i4 r - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);& j- i, A! S6 T( P, b
- if(downD == 0)
' a+ \5 \$ x8 Z/ T" c2 c - {
: R( `# k- L$ {3 I3 ]4 e7 R; }" N - return 99;2 P7 H/ _4 ~/ C
- }0 a' ?2 o; ?( g6 _. _" w% w# c, V
- else
! M9 }8 H8 u/ W4 S1 T3 `+ f - {7 Z6 } b" H% v3 x4 y0 |* @2 n6 h
- return upD/downD;+ L0 ? V1 I% Z/ _5 Z
- }% M3 C/ |8 u; ^2 R
- }
+ h S6 O9 \+ w& B
" q7 |+ q0 G5 {; i) y: a0 M9 W- void moto(int Speed_L,int Speed_R)
& X A8 I! T0 ^' B$ E S - {+ b& L' @# M) a
- Encoder_2.setMotorPwm(Speed_L);
+ x0 Z: f- x3 T( x - Encoder_1.setMotorPwm(-Speed_R);: n& k; `) p, T2 S! K/ |
- }
複製代碼
8 h7 N. H) c6 @& F# M5 EMeLineFollowerArray.cpp' X5 p; a% [3 P1 ~- }0 i% k
- #include "MeLineFollowerArray.h"1 b6 ~ z- c- p3 ^7 a/ v6 k5 H2 s
$ ?/ i0 c7 I9 u! a' v- #ifdef ME_PORT_DEFINED. K: F" ~- q6 H( n. j
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
" P) z! [0 U* G/ W - {) i! n6 v, f$ |. I+ X" A
3 y7 l; i6 f! `3 W! S! v1 J) Q- }. v( n* u+ n! e4 Y( B' m! ^- H
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)7 L4 G% G- O, i; M
- {4 B( G: {6 ~; l8 W
- _DataPin = mePort[port].s2;; T* P) q% t" @; x; H
- pinMode(_DataPin, OUTPUT);
}: r* T" l E0 L" D' e5 A. n - digitalWrite(_DataPin, HIGH);5 Y) g+ W6 v& m) G3 M7 o
- }
, f0 }, z9 n) G8 z) s8 A) m7 c - #else // ME_PORT_DEFINED
' R' B2 U6 N7 K6 k( I- D. w4 T k - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
" a& g$ Q7 ^0 o# F$ |8 e - {
8 z2 Q, @ }: Q# v - _DataPin = pin;
9 [& s! I2 n7 Z' P - pinMode(_DataPin, OUTPUT); : g6 Y% ?& n3 \. U
- digitalWrite(_DataPin, HIGH);
, K% ?2 _* I/ _8 A4 a& G - }
6 K# s! o9 L& ^& u - #endif // ME_PORT_DEFINED ~" i- u# u8 }# N( x9 e) B
6 T+ ?, a$ U/ n# c, ?& ^4 O
4 W" w7 y: a2 x* z0 |+ x K- void MeLineFollowerArray::setpin(uint8_t pin)
7 Y8 V6 U" ?* G" p9 x8 X - {
* ~9 e5 r% w- ~( N2 _% m- q - _DataPin = pin;
1 X) d4 w; F4 M h; ` ^ - pinMode(_DataPin, OUTPUT); 8 s: s7 e' n2 Y2 B, i+ f: L& Z
- digitalWrite(_DataPin, HIGH);
) E$ C) R% o, W3 Q
0 f. h1 L9 h( S- #ifdef ME_PORT_DEFINED2 I% u- k8 e7 m* b2 b
- s2 = pin;
' L0 I4 V, s$ o* D - #endif
% R: k; U: R, F6 L- v0 ^9 F - }
3 }9 V0 j: u \+ e - ; |: f2 A% `, I& q
- uint8_t MeLineFollowerArray::getValue()0 I6 o0 G# k9 p5 W, x
- {
- ^ Y! e) u6 f D/ q - uint32_t LOW_level_read_time;
+ @, ?! _. f: j8 m5 [+ P( K( n - uint32_t HIGH_level_read_time;3 U% S1 o7 |) Z! p6 h
- uint32_t time_out_flag;
2 T2 |3 [" L, D- D' a. x, K# Y - uint8_t Sensor_Data[3];
$ ?* c- u, J9 p. R. T2 m4 B - static uint8_t old_data = 0xff;
4 V0 Q1 E7 t6 A) w - 8 L# j1 H% t6 x" n p$ I+ X
- pinMode(_DataPin, OUTPUT);
' F a9 m/ C& u2 ?1 F- t; [ - digitalWrite(_DataPin, LOW);
) v; X) [5 e s# x/ \# C! v1 f# X - delayMicroseconds(980); I% s- a3 O+ ^0 }: W
- digitalWrite(_DataPin, HIGH);
b% n7 G* M/ ~, ~* F1 ? - - u; Z _- f8 `. v2 i4 h; z
- pinMode(_DataPin, INPUT_PULLUP);! o I2 {( d" O; \1 E2 E& a# { K* W
- delayMicroseconds(10);
! }% E7 l! q+ s0 N3 ^8 O
( Z1 h4 p4 r; X; Z0 ~- time_out_flag = millis();
: ]" m: z) }* y2 K( x - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );/ o. |0 N8 @2 v& Q2 d0 ?
, P r: y, d/ ?" v; @* U- LOW_level_read_time = micros();
( ~0 |/ l9 N6 M% y D4 b2 C- _ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
. v8 u& U- a& P. q6 a) v1 u - {
9 \' m7 U! s( B9 k2 Y {, e; R - return 0xff;/ A% r$ k4 F& u S: F
- }( Z; o& U) C2 a1 p' f% |
2 S! u, @) b" o- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 }( F6 _$ y' V1 t
" ]- c- ?5 G. Z8 d7 B+ e- HIGH_level_read_time = micros();# S. f, M" H# ? [7 x
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
/ Y2 o7 M" I" ]+ y+ B; \$ h
! i& n9 w* ]6 ^ K, q- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ v! P0 S6 J3 T n - {
, G4 ]" ]- P" `* M+ J - return 0xff;; r! c/ h, {5 f& I
- }
- a3 A, s: B9 j6 ^! m" i; [
- P5 y% {4 S& m' t& h. x- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
" |6 E0 W, g7 g& ~" T" J" I - {
; _+ I& D7 B, E8 z) c; r! K4 h - return 0xff;" ?0 F3 m1 B# M$ s
- }" a& ~ M5 A* M: _4 `8 N t& T
- 3 t# `+ L j) g+ F1 I
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
7 ?4 } |8 A9 m: d& k - LOW_level_read_time = micros();
+ l" S% B, g) M! U% z - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level: w/ B3 c7 B! V% B
7 M: i" y# }3 c w! S) r. ]- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out2 n5 k! {/ T( a$ J! t
- {+ S" y8 L! [. e2 P& a+ {
- return 0xff;$ p: p$ n* |3 o
- }
# f) u8 B' Y7 q; T+ b/ U/ T - 5 G# H! j' {3 Y W0 w2 n: p
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))$ b) F' ^3 M S& B7 ?1 [
- {
( z3 W# q* R; d4 W* c1 }$ F- G - return 0xff;/ E7 Y' Y/ a/ E( }* ]0 W8 b
- }
. ^- e& `" {' p; x! v
8 z `9 v- o9 m9 Q% U- for(uint8_t k=0; k<3; k++)0 l' l1 ?; F. r# V0 E' p# `) |
- {' x" Q' d4 i" f1 c0 |
- Sensor_Data[k] = 0x00;
- ]+ h$ c7 |8 \) H& R - - E2 l/ H# z, |- i/ W4 R0 o
- for(uint8_t i=0;i<8;i++)7 B c j/ V2 Z; b+ V6 D3 @
- {; `8 y3 O0 Z3 ?7 o5 }
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* E# e3 ?/ B3 F, m2 ^ - HIGH_level_read_time = micros();3 r1 P$ m1 N8 j3 G& Z) W
- LOW_level_read_time = micros() - LOW_level_read_time;! q- w0 b4 t. l) S( M* T( @0 }- }
8 ?4 [/ P1 Q. s% n- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )1 w$ L! I+ g2 ]+ S9 U! U; T
- {1 M( {6 H& h8 [4 R/ U' \
- return 0xff;* K6 z" a; k# l; E* w6 R
- }% k k" l9 n* E% h) K3 H+ v" d! h
- ' m0 e2 G% q2 t4 d
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 L1 n5 D) _8 A7 @
- LOW_level_read_time = micros();
F- y5 E+ x4 n9 O - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
7 r+ B+ w4 k; D" w - " W2 C( l1 O, W3 m2 z
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
" |$ r) d; g2 K/ u' _$ I4 | - {
; D6 x |* o1 U9 S - Sensor_Data[k] |= (0x80 >> i);
% ~" c$ |% z* @0 A8 B - }
) A: O- S$ b" l* }* g' J! u# o* |' ~ - else if(HIGH_level_read_time >= 100)) b7 [- T+ \9 [% N4 ~& v# v
- {
* x7 M1 J+ g# K8 l- G. I$ F - return 0xff;
2 ~7 L# C$ k: e& o - }
& R4 K! Y8 h1 l) c3 V# |( k% \* v - 6 b4 K" L+ C0 e2 |9 P1 |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)! {+ |/ n+ h9 ^, E7 ~* G3 B
- {
) _, I( r) f$ k- h/ E: I7 c - return 0xff;
" j' Y1 Y! x3 ]7 R" n4 } - }
% ] Z. m% W9 ?3 l2 u* v# q9 t, Z - }
- S: a$ @: }& x6 `. W - }; P8 J7 j; u& ^
- ' p5 D8 C0 S# N( K# x
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 d8 m4 Z& f% x% X% o - HIGH_level_read_time = micros();
! B; v' ]( F( f# K1 C, }1 I - LOW_level_read_time = micros() - LOW_level_read_time;
+ h- k! s9 D4 `7 ] Q1 o3 }
8 t. [0 ?: L3 f: g5 {! I- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 o' l: f7 |. T0 A+ N$ A - {
# E! n+ b3 [+ S+ A6 R$ y - return 0xff;: r' ]0 y7 {( |- _: C* ~% c& c
- }& P$ G1 F& r Y1 s# Q# E& E
! @1 j/ r% `+ y% ?9 M- pinMode(_DataPin, OUTPUT);4 g, O; S* M" \) k4 F0 g* y" }
- digitalWrite(_DataPin, HIGH);7 x5 a" j! ^. K$ M& [& y
- 5 a8 V& O9 a: j s" F
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))# ?# k- E7 Q9 A) P, f
- {: `! z1 _& B" u+ x9 a4 G% P
- old_data = Sensor_Data[0];3 T, }/ w' N% m+ ?
- return Sensor_Data[0];
8 @, z+ V: }6 e4 \2 s9 M5 i - }& c& b q# b: S) I$ b, [
- else
: ^$ R: J5 }% ?8 I" ]* X - {3 x K9 O% H1 e3 P# K& G
- return old_data;
4 _ U/ \$ m$ H4 G" F G5 K - }# C S/ p1 g1 b+ w' E# E" W. Y
- }) \' i0 y1 ]: l: }! w" x) W* f
複製代碼
. K/ j& B' H [2 z) |$ `; A) jMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ b0 x5 M, q1 U! D" t) U, x! P; c0 C0 a
|
|