|
|
1 n% Q7 i! p- C: o" H
- #include <Arduino.h>0 O" ~+ B/ p- m: w
- #include <MeAuriga.h>9 n% U# t. ]) R0 f" L- J0 e3 ~
- #include "MeLineFollowerArray.h"' N5 d2 x+ }. S- I2 ]3 s8 `
4 M. T0 N: @0 p7 n% Q( a7 T- MeEncoderOnBoard Encoder_1(SLOT1);
# U* R" M. ?1 u4 ` - MeEncoderOnBoard Encoder_2(SLOT2);
; o# B- D3 h! `& k* E/ f* k9 b/ A - MeLightSensor lightsensor_1(12);0 Q1 N/ @6 ~( t; Y7 s
- MeLightSensor lightsensor_2(11);) X& T! O5 L) ^7 d# w& @
- MeBuzzer buzzer;
( y" w4 x5 c) L, y - MeLineFollowerArray linefollower(PORT_6);
: }. \: x( w: L3 x$ U8 D$ {
: Y' L. S2 I) q/ E- #define Error1 1
, n$ w% O3 ~ Y9 w( ` - #define Error2 2/ }! s5 g% K3 ]$ O* y* J; k
- #define Error3 3# f; Z( |9 ^) n7 f6 x% `9 _
' X$ g. x3 ?1 Y2 } h7 P- #define Kp 15
/ s) ^& ?$ e8 V2 T - #define Ki 0.15$ Z: n3 Z) z) S
- #define Kd 0.034 l6 A4 l! M# ~) Z0 c. i( W' A
- 0 e4 ~5 b5 g1 d% a& y# _
- uint8_t sData;6 D, @$ n- `6 y/ d
- uint8_t D1;
8 e- D, b' P3 [$ h6 S - uint8_t D2;
. w! h1 E- U) h! H B B! O - uint8_t D3;
) e- Z+ [5 j( Q' t5 @7 x9 g, R - uint8_t D4;
, ~0 n, e' D5 Q) ?3 `4 w- I - uint8_t D5;
6 U8 O' P% j# i' Z - uint8_t D6; T+ D6 C: x3 c4 m8 _
2 B+ Q9 S* a, {. N+ z- float previous_error = 0;
0 \! Y% s2 y4 R! }6 M# {8 d - float integral = 0;1 |4 I* m& L( ~5 V& n1 M) _3 V
- float derivative = 0;( q# \* e4 u! ~2 a" p! e- T/ y
- int Speed = 160;: x* c0 K# G; P* k
- float output;
( z5 i. k+ r" s; P. Z
- S7 B o0 D0 {8 Z( q8 T. n- byte Left;8 O; y) C' [ B: D& \; ^5 d
2 m5 t6 `/ p9 h7 e- t- void setup()
% ]. X0 G2 H4 m4 Z- j+ t0 Q& ` - {
2 V7 R& V: ^6 D" I/ t9 v8 A - //Set PWM 8KHz, K: ? {/ E" v V
- TCCR1A = _BV(WGM10);' n0 Z2 i, d3 u& D4 t2 o
- TCCR1B = _BV(CS11) | _BV(WGM12);
. z/ J# a2 A9 ^ - TCCR2A = _BV(WGM21) | _BV(WGM20);
& y* e, R% k0 d6 a - TCCR2B = _BV(CS21);
! F3 c, Q: T% z: d - Serial.begin(9600);
4 [- O! {' N& |$ y# ?" R - buzzer.setpin(45);
3 l- r; i* L9 [0 c7 r, y - }; [4 z6 f: `& ~) d% L/ e+ ?
- 4 S: I: P# x. [" X; N+ I$ T. i
- void loop()
5 K" R# K7 F7 d - {2 V% f9 t$ x: D/ g- P q
- while(!((lightsensor_1.read()) < (10)));. C) c) P( \5 a! m8 f/ [ N
- buzzer.tone(1047, 500);
5 {2 `2 \0 N; V" K+ U - delay(500);/ T2 g$ L8 M1 A& x% y2 v) [2 p
- do
3 I6 p8 R4 N: [ m* {+ l - {4 _0 Y. U, [" X' q4 @; A
- int Err = getErr();3 y: |+ J9 X' q6 W% |
- if(D1 == 1)
/ [7 B/ H2 u( a8 }- W0 n - {, }9 z$ [& G' `3 d6 [$ |9 J
- Left = 1;
0 O0 R! Y$ Y% s8 Z+ q - }& ?2 U8 _: D/ A" N: c- b
- if(D6 == 1)
4 g9 }- E3 t! |7 o0 Y - {! d; x% x6 L+ _) b8 A1 K2 k6 F* Y
- Left = 0;) g: _. w5 V5 z
- }
; X: U5 {5 U" W( m - if(Err == 99)
6 X8 v Q! @ M) I) V8 k1 I - {2 _# C8 t0 G" h$ e! F" S o; M: R. C
- if(Left == 1). X J% [ `2 E' O5 c& r7 |1 V& U
- {4 ^% |1 S6 s6 T8 p8 p& c: E0 }
- Speed -= 5;1 ^- w0 ]! V8 k5 P2 t; d
- moto(0,Speed);* Z% @; Z8 Z1 N5 J7 I
- do3 \7 @4 y* J# y! o% v* S
- {
k+ w9 Z4 m# h, e" W/ ~$ Y$ ^8 L - Err = getErr();
* T) f6 R. ]& P& C0 k2 _8 c% t1 V1 Y - }while((D1+D6) == 0);
" n- m3 s4 o u1 d; i5 s - }( r. Z0 y/ W2 ]7 r8 k) c% y+ P
- else* D2 ~4 \0 @! \4 V( W
- {2 [* B8 m* [; g+ L9 n0 {
- Speed -= 5;
5 k" n; d% C l+ |$ Q( z) J1 A - moto(Speed,0); * M# j. Y1 I! y0 ~- u9 { L
- do
/ g8 A! N+ M3 ~0 g" w - {6 R# B- H( ~: f* c, {
- Err = getErr();* q9 s/ N% ^! X# |4 [' V
- }while((D1+D6) == 0);+ s6 u/ @4 o' F/ |. ]
- }6 H0 z8 u+ }9 t9 M: p% s& Z
- }) W: t% ?* Z- E& p- F
- else# g! Z, }$ j1 w& \
- {
8 S% A" s5 w( m# ~$ n9 N - if((Speed < 160) & (Err < 2)) Speed+=1; 9 _( y7 _+ q8 S1 k9 a
- if((Speed > 100) & (Err > 2)) Speed-=2; . P0 o% i u7 J6 o+ R5 h9 H- b
- integral = integral + Err;
8 _" O" q) a/ ?3 L' Q5 Y - derivative = Err - previous_error;
2 J3 x0 R B9 H9 J - output = Kp*Err + Ki*integral + Kd*derivative;8 h2 O. l% g S, E9 I
- moto(int(Speed-output),int(Speed+output));5 i0 h) M; u' V
- previous_error = Err;) c5 B. v- @& j0 y
- }* L& |0 |" _% n3 O/ J' l q
- }while(!((lightsensor_2.read()) < (10)));
$ y/ N: ?/ T- V$ N# R5 P5 D- H0 s - moto(0,0);' E0 U5 d0 V+ H" S4 S7 p7 R+ H
- delay(500);$ I+ Z j% q( p, H
- buzzer.tone(262, 500);. n Y5 D) l: B+ t
- }- B$ p! e: h" R# B& L# l
- 6 E+ c1 ^) C8 t5 u6 d' v
- int getErr()
) D$ O4 e1 o) | - {
1 }% E2 e7 ?. q5 ^4 B - sData = linefollower.getValue();3 c F$ a5 c. ^9 Y8 ^
- D1 = ~(sData>>0)&1;# S2 [: \( R! X" \3 K
- D2 = ~(sData>>1)&1;$ |% K9 l8 }8 O. d0 G
- D3 = ~(sData>>2)&1;
# E5 s1 w, M' Z; B7 `- h - D4 = ~(sData>>3)&1;% U- X/ N; w6 N0 L+ f# V6 j
- D5 = ~(sData>>4)&1;3 g8 K* o% S: D& E- ]- A
- D6 = ~(sData>>5)&1;
- @; O; J1 ?7 d9 D+ o2 H/ _' Z - int downD = D1+D2+D3+D4+D5+D6;
* Y5 z3 y( q+ T( U- y4 i# q - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
; G# ?" N! u; n/ X7 U$ t5 O - if(downD == 0)
9 A" Q) m: X# |5 L0 A - {
, n# P* N+ q- h6 l% B+ j/ G - return 99;
, I+ |( M; z! b& ~: ^: k- O - }
* E k' W8 Q) f1 ~1 |% b: ]: b3 ? - else- N; ?. W/ @6 @* w W o: c
- {2 _ e: ~6 B; _1 u% |- T4 N% C' M
- return upD/downD;
% \. d5 M8 ^& R1 x! s - }
5 E1 ?9 i% _! x- g' U - }
* p; i: |/ e. ^, [6 g - / [- b, k, _; P2 ~) k: t. A z- t
- void moto(int Speed_L,int Speed_R)
9 T# |1 B! f% q9 O L - {
: w# O& v5 a2 ]0 N& @ - Encoder_2.setMotorPwm(Speed_L);
' Q7 U+ |& M7 \5 a8 e' l - Encoder_1.setMotorPwm(-Speed_R); ~' ^* Q" B9 {, L# V2 A
- }
複製代碼
; P$ ]# d) L9 N$ T; j2 ?- Y6 pMeLineFollowerArray.cpp' H! H2 w/ c1 G, G* D7 _3 L
- #include "MeLineFollowerArray.h"
3 _8 S4 g! a4 ?# v4 ` - ) Z4 i9 h( D' D1 d( d" ]
- #ifdef ME_PORT_DEFINED
* N% H& _* M3 n' E9 O7 J7 p- p - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)% H/ Y( n2 `8 D0 S# m K, \" c
- {
- x. R& P( H5 l( s0 x - 8 j! a# }1 ^4 S; G
- }) ]9 a0 T" y* ^. q0 M6 J
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port) [+ w, {. z$ L# K, y# G- Y
- {
+ A" c: ^( e+ n4 W% p( p- {1 K+ h - _DataPin = mePort[port].s2;
! T9 X) E; N( A* O$ O" o) e - pinMode(_DataPin, OUTPUT); 6 H7 i7 n! t5 n$ G' x
- digitalWrite(_DataPin, HIGH);0 V" _& H1 g( D2 a* A
- }
+ P* c) h: z( w! ^ - #else // ME_PORT_DEFINED
+ }: h* r7 G' r- I5 U - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)6 p2 H( m( e5 K( a" z, |
- {) z' c$ i+ j" W# D5 G
- _DataPin = pin;. t w0 e" K* @. b8 R8 U/ c- C
- pinMode(_DataPin, OUTPUT);
, Y, x! U g# O/ n9 I - digitalWrite(_DataPin, HIGH);8 z1 ~& P/ K" q ? m
- }
# K; m; V+ A0 L" v$ Q - #endif // ME_PORT_DEFINED
; l* T' B2 f- A; _5 ] - 9 x2 B3 t0 X1 b
- 8 _ e+ {7 L& `7 Q7 P" _) d: V
- void MeLineFollowerArray::setpin(uint8_t pin)4 c, I: ?) d! w+ k* a5 s$ q' G
- {" G' X2 I: ~: T! ^5 o
- _DataPin = pin;
9 S( A7 x( x; j" S/ | - pinMode(_DataPin, OUTPUT); 4 r' J" K0 H3 O; N
- digitalWrite(_DataPin, HIGH);$ H) s) D, s( F: x0 g
- v, U. l% a+ w. [/ D- #ifdef ME_PORT_DEFINED
# E: H+ b: z2 e; w/ d& Q( C) U7 e - s2 = pin;6 M9 | {8 t z! z- E! G. L
- #endif
: N+ x! }3 u Y; {% ? - }. z1 a+ X! v& e
- , v P: z( r: z$ y% |/ H# a) p% `
- uint8_t MeLineFollowerArray::getValue()" @$ R# T0 L' H, V. w) F5 K
- {
3 H% h1 X; }" }- z - uint32_t LOW_level_read_time;" U9 @ z- ~, M" o; K, @' ^
- uint32_t HIGH_level_read_time;
: [9 {9 y, ] H# w - uint32_t time_out_flag;9 r# I' m+ k: B: Q, M6 c6 k. e( U
- uint8_t Sensor_Data[3];& u3 ^2 q# u5 P# ?' ~- r4 B7 D
- static uint8_t old_data = 0xff;4 f1 h: O# o' c. x- q/ h0 H$ W) `7 c
7 W* ~( T' T5 }! d2 U- pinMode(_DataPin, OUTPUT);! D* @, A$ I/ D7 i' A5 \
- digitalWrite(_DataPin, LOW);
: b; b+ |% @1 E! {1 _" Y - delayMicroseconds(980);
) |: O4 u6 t( y4 j& z% U - digitalWrite(_DataPin, HIGH);2 v1 [& D& |, b( K2 y
; Q5 _1 M% ?0 \, J- pinMode(_DataPin, INPUT_PULLUP);& r) d1 F- I" O. X( D2 ~
- delayMicroseconds(10);, ~& O* |6 N% h. Z. R: m
- `3 a) Q' ^) ~8 |- time_out_flag = millis();
% ]! f# e) B( n1 K0 ^5 l - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 F n3 J7 q- M/ ^- S
* u/ m |1 _, L, H- F" ~8 p- LOW_level_read_time = micros();
! m" E. W- s" T9 V4 ?3 a& j - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& I, q' N2 l% ~& ]. d
- {
- Y- y2 b) L3 _& a- |. P+ W - return 0xff;$ ]% r- s4 U0 }5 u% |( T
- }
2 \0 m" N& f6 G/ ~
) I- `. y/ Y3 \, g- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ i7 e) T5 O1 e! Y
) C% z9 m8 I- F; W" i) V. B- HIGH_level_read_time = micros();
6 {4 i4 }4 K, M; m8 j K - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level" |" `, _- [0 y% E. ^" ~
! P+ x9 k# g% p- G, E+ E- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" G d% k7 J- D5 x1 Q
- {7 s: u( X* }( Q5 }$ S8 T6 f
- return 0xff;
3 m( o0 m0 j6 z8 r- \ - }6 X4 B9 M9 G8 o% o; N h
6 x- p7 t1 X3 T: m! B' H9 D- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110)), A' d9 y3 Z8 ^9 S7 A
- {1 t: F' W( j' Z0 g$ W s
- return 0xff;' m# S$ x; j; n$ X9 j
- }8 M) N: d* y4 |+ \1 H; g+ [
, }, @8 y/ a- @; @8 }9 G) d- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
2 E% r6 v7 r1 q0 t - LOW_level_read_time = micros();' Y( e) I: c3 H0 | g
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
+ S1 ~0 S! }3 n - 5 {0 N& A7 d) c8 m* @0 h* E% i
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& S8 r1 V- e0 b$ n" K
- {( G' J) z6 \! i0 v, l# @
- return 0xff;
6 `6 X) K6 t7 K1 E8 r( W - }
7 l" }% K, X' E3 p - & s; ~' A& u! }
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))& r) I+ X1 Y- q1 l7 p7 l
- {/ w( ~1 _1 k4 S3 i/ ~
- return 0xff;) [' n6 x( x3 i# y2 j9 @
- }
7 q1 S8 @9 j" S. q3 _9 Z+ v
/ z# P& L3 S% E. p- for(uint8_t k=0; k<3; k++)
8 C3 z1 K& M1 x n# Z1 c# x5 V - {
+ o8 m% A/ A8 t+ @ R - Sensor_Data[k] = 0x00;
9 G1 _! h3 ?9 ?8 s
$ T, u% p3 N. l8 P8 D, B5 z7 P3 o- for(uint8_t i=0;i<8;i++)
1 ?% W+ } W2 g& V$ E* I - {9 N8 H. u1 d9 D) ]4 H
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
2 e9 v) y7 E1 F& y; G - HIGH_level_read_time = micros();/ w# {/ x5 O: `# f: l+ @/ O
- LOW_level_read_time = micros() - LOW_level_read_time;4 B, j3 M% _! v7 p' |
- ! `" ]( x. h5 {+ J0 D
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
- e4 u2 F' X! R/ F - {
. Z& b6 e4 ~1 ?9 `6 M7 } ~ - return 0xff;
$ R# t: w6 N" h: M7 G1 Y Z - }
6 H- v0 Z6 [' f) O& y) F
& l' k7 M8 S7 Q- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
( l" t& N4 m l! Z s; ]. F - LOW_level_read_time = micros();$ h2 S+ E: i5 x
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
% n0 S4 w- X6 |; a: D% E - ( j7 M5 r1 |8 j" a& [ U7 c
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1: P! E0 L; }5 c) M8 a4 w7 [
- {' b: R4 {5 U: U+ O
- Sensor_Data[k] |= (0x80 >> i);/ ]9 D* w$ {; r B4 Y
- }
9 w% R# ^* d0 o - else if(HIGH_level_read_time >= 100)
: ~1 \; f* Y. _, _: o - {$ k+ L/ y9 Z" ^0 U" V! G% `9 ~* I
- return 0xff;; L3 T; b0 L9 \
- }
+ f% \/ j* m% u! @# n$ f' z
! L; n* X4 ?- r4 d9 I) u+ T7 y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
0 e1 O( l y8 p - {; |7 t5 J' `' F* |- l
- return 0xff;- W" M) o- l5 p( ~
- }
7 F( u+ c2 Q5 g - }7 [# t; c# e# H, ?
- }
/ a/ t* z; {; ^
( @/ B7 n; L. }, i2 f# h- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level# L! L& V5 }1 C8 a
- HIGH_level_read_time = micros();
) ~4 h/ D* P5 R - LOW_level_read_time = micros() - LOW_level_read_time;
5 o/ n" x: S0 ^ - * |! `- m+ f& J9 R3 H5 K3 _- k2 [' M
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
* G- ?. O- ~. K2 q1 C8 g - {
2 b! Q( k* X6 Y/ I. r( A# S - return 0xff;' @' j4 d/ T1 C# I ?4 ]% r F1 m* [
- }
' F: I+ K9 O5 K1 T - , \! w: t4 W: ]9 e7 X4 H& \
- pinMode(_DataPin, OUTPUT);
1 s; z# I& i- ^% k% @+ K1 v - digitalWrite(_DataPin, HIGH);
% T9 @; C/ M/ r3 r - 9 U! ]) Z0 L5 Q/ V9 M3 U5 u1 U
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
' J; T8 c5 U; r - {
' S2 o5 W9 t5 x - old_data = Sensor_Data[0];
/ D0 b @: G' W$ o% y8 _5 W - return Sensor_Data[0];0 A/ i0 h0 J+ w* F( P
- }
. @3 a, q/ H! V. M5 V - else- Y" d' K G/ `
- {6 B3 [2 T" f- {% O O8 z, Y
- return old_data;, v8 o4 x8 m0 W9 v: R8 [8 y0 r% J
- }
3 ~) [9 Q. j; x' ~ - }
% T5 [7 p& R: x: q
複製代碼 % k9 V% H3 ~1 C9 i
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) m( u% x7 t6 D! }/ D0 y* M
) [: f; o5 }3 q O5 _8 \( h
|
|