|
|
1 D. ~; {9 _1 V6 a( I$ g
- #include <Arduino.h>
+ n i ~7 h$ x" C; b - #include <MeAuriga.h>
8 C7 T. {6 R, R$ n - #include "MeLineFollowerArray.h". {! t. X5 T9 N, \4 A
- 6 S _0 ^) f) J$ f+ A
- MeEncoderOnBoard Encoder_1(SLOT1);
3 q1 j3 W# u( ? A$ L - MeEncoderOnBoard Encoder_2(SLOT2);
3 [; V, i& O4 i6 \5 h# B - MeLightSensor lightsensor_1(12);: n% p# P$ {, A/ T r9 _' }
- MeLightSensor lightsensor_2(11);# q! }/ A2 F. N$ t
- MeBuzzer buzzer;
, \& Q* D- @. H# p' @; F - MeLineFollowerArray linefollower(PORT_6);
1 Z6 [4 _: K/ {2 q; q, o0 z6 x' j - 1 e' `- A# r9 f& @& X/ M# A
- #define Error1 10 l4 g: Y" A$ o
- #define Error2 2
# z; ?9 y! B$ L; P X - #define Error3 3. E4 E8 C+ D) l. D) C" Y
/ S) {4 O2 `: F8 b- #define Kp 151 A. }. z3 s" \/ A. F' r" {! r! F
- #define Ki 0.15
/ B( ?+ d8 r1 T! l4 c - #define Kd 0.03
3 f) {8 F1 P& }3 c
5 F; p" N; Q: ^; \- uint8_t sData;
+ R) l; |, G# ^6 U9 B4 n - uint8_t D1;
7 }$ x Z7 Y, `+ B - uint8_t D2;
2 K/ {, B6 Y. h f" z - uint8_t D3;
" W' @" ~- F% _; l: ]" F& I4 E9 J - uint8_t D4;
7 A/ F l5 O6 v3 Q - uint8_t D5;1 O6 \8 c( V' Q- U( W
- uint8_t D6; h' i, o0 q7 w& X
- ! m7 K' P3 c; r7 n/ O& g
- float previous_error = 0;9 X9 G8 L4 T7 d# H5 R' `1 y% k) ?
- float integral = 0;+ Z& z4 O2 A: ^: o# ?% C6 N
- float derivative = 0;
" r' L4 l$ _' q$ [8 F q$ y - int Speed = 160;
( f1 z% K( ^- L- | - float output;
- S$ y" a! B) X/ F6 P
8 J- V4 |' H) i, `1 ~- byte Left;* C9 j0 S* ]2 e( j$ w2 W: U! I
" P4 T/ b; P) l3 R! R- void setup() : s3 w4 S/ [8 h( F8 w: x$ p
- {
F, x5 p4 N& Z& h. O. ]! ?; V4 ~ - //Set PWM 8KHz
- ]( Q$ D: `8 N$ x8 X: d$ ` - TCCR1A = _BV(WGM10);
. I/ v& L, L/ d$ k - TCCR1B = _BV(CS11) | _BV(WGM12);( S$ d2 L; N# R: S6 a
- TCCR2A = _BV(WGM21) | _BV(WGM20);" ?, ^* v2 Y& A: p+ t
- TCCR2B = _BV(CS21);
, I3 f, j$ M2 q0 z& g1 @1 H: B( k1 _ - Serial.begin(9600);' Q+ @7 l" E* ~8 ~
- buzzer.setpin(45);
3 d% x5 K8 f3 O. O3 { - }
7 s z) s6 |8 w/ `" a8 N$ m - , l- e/ G' H, n }0 H; Y
- void loop()) S0 R6 e1 j- s T' X5 K# o, ~, J
- {
* [7 U+ S# K( Z+ S, L - while(!((lightsensor_1.read()) < (10)));" D, P' t5 {2 O/ C8 B
- buzzer.tone(1047, 500);; o$ ]' t7 h7 }6 Q
- delay(500);
8 k8 t5 x" C3 p) c6 |0 `# Q! p - do6 p3 F: H1 v& B1 d0 ~4 j# n: S' g! ^
- {
( n$ v3 K& |' J3 A' S& f1 \1 n6 k - int Err = getErr();
& Q7 A( u" c+ z( [5 T - if(D1 == 1)9 L% ]' w& k8 Q9 F& K, z
- {
& c& g# [8 r7 z8 n& D2 P: E - Left = 1;
1 Y" @9 @4 C) b5 y* E - }
* }9 M* y. ^( n" L5 q2 g! i, c2 t) l' ] - if(D6 == 1)9 n( |: Y5 F5 U0 m% q( p' p' X& O
- {# ^2 |1 V4 c/ ^
- Left = 0;
' M" I' K' K$ B2 w3 m+ v6 o - }
- S$ ~( d. l; L B, p - if(Err == 99)+ D+ h+ }8 V: h+ X$ [
- {
: q6 J U* {2 r( P5 R' `! U& ^7 w - if(Left == 1)
9 F) r9 S/ \! y: P \1 r - {$ D( Q/ O6 H" \: g8 Q: P( X
- Speed -= 5;" z! D$ U! X0 w8 n
- moto(0,Speed);
6 l% L; }3 d$ i+ z* g0 S- M - do9 r% `% C' b2 z" a
- {- f- w2 E3 _' C: U+ y9 ?% Y7 g
- Err = getErr();
/ Q/ B/ A1 ` f! b; S$ _- o! I; k - }while((D1+D6) == 0);8 Y/ F; P. a2 G& Z' `" U
- }
) n- ^& d; c; M: }! o - else% ~& P! z9 u& @& h8 X9 }; r: O9 S
- {
: j6 b3 a0 I7 v* T+ i6 |; I u& Z0 Z - Speed -= 5;
$ |" _7 G7 J7 X - moto(Speed,0);
) B+ N3 C, M$ N - do
" s/ ~ f. L9 X! y9 A" w( k - {$ |- Q* u% M" [1 }6 }- W
- Err = getErr();6 U& D' {/ e: a* w3 A3 }& v: B
- }while((D1+D6) == 0);6 s2 t& E6 B2 D2 V
- }
2 P$ E* }: Z5 }0 v, I4 ` - }
( w2 W' V1 ` ~2 c) } - else
0 ^$ W7 V( }8 }+ J, e4 a# } - {
3 o3 V* \& X6 e, |5 [4 g - if((Speed < 160) & (Err < 2)) Speed+=1;
! D8 l" v/ u4 D# ~4 m8 S3 v - if((Speed > 100) & (Err > 2)) Speed-=2; 7 ]6 b \5 }4 q' |+ P
- integral = integral + Err;
$ Q) o8 C8 b( w9 n! k7 g' [2 } - derivative = Err - previous_error;
b- [% X- N. z4 R6 @ - output = Kp*Err + Ki*integral + Kd*derivative;! N. p% Z6 a; r6 q) G, \; S' p/ J b
- moto(int(Speed-output),int(Speed+output));# {" V! d& q9 z% g
- previous_error = Err;
8 p, [7 W( W$ v. o, e+ x - }
* |/ o V! d; S6 J - }while(!((lightsensor_2.read()) < (10)));8 U: z9 t: p5 F1 ]; ^, V+ M) I1 _
- moto(0,0);' S' u, c$ l# I; F
- delay(500);
. C+ N8 Q9 X$ ^. a! ?6 |! {4 O - buzzer.tone(262, 500);) Y9 A: L9 n; m3 i, q& J2 b& D
- }
0 ~! d5 Y/ F. |$ z [( v3 D3 H
& H( p# |; d% R0 C1 B. ]- int getErr()
4 v. j' W0 g" {0 U2 O - {
% [7 }$ s _) F: k8 m - sData = linefollower.getValue();# z) q9 D; g- p1 p( h) i4 p: P
- D1 = ~(sData>>0)&1;6 \3 s- }# s( O! Y9 }) g
- D2 = ~(sData>>1)&1;
- @4 D( ^4 ?+ g' C - D3 = ~(sData>>2)&1;" N. H! l8 _4 M' A
- D4 = ~(sData>>3)&1;. M8 O4 r' Z% x- P( B# v3 _4 B1 p
- D5 = ~(sData>>4)&1;
: O( h& Y9 l! k9 B8 @+ c - D6 = ~(sData>>5)&1;
# ~* u% G+ F, ^# I0 } - int downD = D1+D2+D3+D4+D5+D6;# u% Q* q5 W% k* H+ n" @4 D
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);* N: B9 b6 z o* E) d; w. G6 M
- if(downD == 0)
+ a: l6 K9 ^) V# W3 d - {" E2 r0 N* Q* \+ V j
- return 99;: {" _6 k- s+ d2 m
- }, ]$ L& u9 k0 ^7 N
- else
! l! Z1 k( v- ~, K8 D - {( q% m) R b. T0 T. t
- return upD/downD;
1 I0 `5 I9 i4 I! m- o5 `- o - }
& ]9 E. M! }- W+ h n - }
' S! o6 T( d, j1 q% Y0 [* l. d3 ~
. s2 M/ R& q; U4 `" i! u- void moto(int Speed_L,int Speed_R)
3 A( n# E) m) z. N3 `$ V, v - {
2 I6 q s9 Z$ D5 I+ V - Encoder_2.setMotorPwm(Speed_L);
& M2 n' A2 T7 c5 Y- r+ a - Encoder_1.setMotorPwm(-Speed_R);: V& Q& V( G# R, q- N& l4 l
- }
複製代碼
' m0 j( H1 m: }MeLineFollowerArray.cpp
* q+ C- \$ `/ Y- #include "MeLineFollowerArray.h"
; f) X0 q+ e: r" r6 u - * q' v: q, o& _% T+ R
- #ifdef ME_PORT_DEFINED
) ?6 {6 N; A6 f' P1 I - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)- p- Q) g8 q$ j
- {
( U% s8 z0 k2 i) ~" `( K3 K- ? O/ A - 2 _& B) p+ x3 T2 d6 M# e4 W
- }% g. D4 D8 Q; L7 a2 a3 g
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)+ y: e- u1 V4 i/ Q2 J$ |
- {. u, d9 T* K4 ?# q3 ~& ?: n9 F5 E
- _DataPin = mePort[port].s2;
5 C' |1 T6 w) ?" y4 } - pinMode(_DataPin, OUTPUT); . R f7 x& O7 h2 v( r
- digitalWrite(_DataPin, HIGH);
) n. s+ R3 `# O6 U - }
6 f9 ^# k* B; S' r, r - #else // ME_PORT_DEFINED
# a3 i" }% s, ]$ k3 Y4 W9 `* L - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)# U2 h7 h/ j% s$ s8 j
- {3 C3 o$ l I" b+ o2 r, e# m
- _DataPin = pin;. S8 D0 S7 H/ A0 Q9 [
- pinMode(_DataPin, OUTPUT);
( ?: q6 k7 d6 q9 [. [ - digitalWrite(_DataPin, HIGH);
' @$ }# H1 L$ \% Z& Q) V# q% C- ` - }
! E6 w w$ n/ s8 R( G - #endif // ME_PORT_DEFINED* |, D% x6 j6 j/ N& R+ W8 v. ?
4 g4 L I' e3 C: o7 q
) k, ]* j# }% l- void MeLineFollowerArray::setpin(uint8_t pin)
, H2 I- \; P: o9 |* j: D - {* R4 i3 M1 i- q; w9 |4 V' z
- _DataPin = pin;
! g8 g8 J) y" x; A' j ^5 n; p - pinMode(_DataPin, OUTPUT);
9 d- J, f6 Z! }- m8 W$ z - digitalWrite(_DataPin, HIGH);
( x( k# U0 U# V
0 v! O0 X/ d6 M3 I: F- #ifdef ME_PORT_DEFINED
1 D" ]/ J8 }& e( j/ C2 b! s - s2 = pin;/ U! V# ?; a$ W2 T
- #endif
; t7 X- w+ H4 ]; K/ N/ J W! ?* o3 f/ h - }
6 A2 p( m4 @8 m
$ a4 C5 g) }) [3 c- uint8_t MeLineFollowerArray::getValue()! _. L' b7 [4 h( x: n1 G) `3 u
- {
. z* Y$ _! S( F( _ - uint32_t LOW_level_read_time;
! F, U; l9 _7 i4 L& W# a - uint32_t HIGH_level_read_time;
. D* _# S. u$ V/ ~$ _; s& l( ~ - uint32_t time_out_flag;
' E$ S9 S4 w6 v4 F; \1 X4 C - uint8_t Sensor_Data[3];
" Q+ o0 x. B, k* r, R- [: Z4 s - static uint8_t old_data = 0xff;. H# i4 ^4 N# T: K3 o6 F- K' x: P
' [6 L4 m4 I, z- ?! y- pinMode(_DataPin, OUTPUT);
) u& O9 R2 O" Z% i - digitalWrite(_DataPin, LOW);( f5 e" _! h" [' B. u% h& k0 j/ l% ^
- delayMicroseconds(980);
7 f" Z( C: u% o* L - digitalWrite(_DataPin, HIGH);; ?% b6 O( L# t$ l# C6 t
- 9 G& J4 U% r9 b; W: h+ p5 p( O8 g; w
- pinMode(_DataPin, INPUT_PULLUP);
# F; g" X6 L3 I& L% W - delayMicroseconds(10);
/ |; r9 O3 p, O: n - 0 G0 j) h( Y3 e* m+ y. f! p" R8 W
- time_out_flag = millis();
; r3 L# i6 }/ C& R v# T - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- _0 I! v4 J* U% f - 1 l5 V7 Y3 m# b6 C
- LOW_level_read_time = micros();, n5 F# a7 C0 V- I
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out8 u" a2 @( r; [$ y
- {
; K' B& O3 y) {# M; p - return 0xff;
2 Y k" W* A' _ - }
8 ^) ?0 }+ j: T6 z
2 V& ?/ T3 w( c4 I- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' U; e5 T, a' l# I
- $ }# D9 p5 R4 e& Z3 G# u$ U
- HIGH_level_read_time = micros();
) O+ d$ f6 Z& [3 k - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level) T. ]" F! E. Y* @0 H5 `; F b0 ]7 M
/ i7 J6 W) U& K- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) V: c7 E- G5 v- Q4 t4 e- u - {
% Y# d* o6 v( s4 L. I - return 0xff;
* s1 a7 G4 \2 z; Z$ g% P - }
. L7 ? }) X. }; b# Z6 P% q+ R
; b& ]" p3 l1 P$ N: H; L5 e- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
/ B/ E7 ? ]' Y: J- k( I - {, H6 T8 }, T/ v* e: u% d1 w# r
- return 0xff;
% r& P0 I1 w( | - }1 \ N' l/ @* n' l5 u; k8 ^
* V U/ l/ \) F+ F- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ g; p3 [. W+ }6 Q - LOW_level_read_time = micros();( X0 y( B0 u. B G6 x6 _
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level4 _, G A4 u5 k( o1 |+ L
- + T, y3 `( ?1 @! |
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* H* M+ U% D, A, F. T9 z - {
$ }4 F) n9 @! m( R - return 0xff;
& s3 ~' w' k8 K+ r - }
5 ]; ]/ m* [- m& `6 y2 c3 \, f0 {) [
. T7 i& ^- n2 D2 |: ]' d, `- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
/ m) C( T `" i8 p+ T8 S: \ - {" ]* i0 Z* }' v! Z2 f0 R4 q
- return 0xff;; L6 x# o+ j$ ]" F1 Y' v# H9 ]
- }& o1 J" S3 Z' Q2 ^' L
- , F/ N$ L$ T E% b [! B
- for(uint8_t k=0; k<3; k++)+ b4 E* H, k. t3 w$ t- `8 @
- {
1 M4 C3 o7 Z/ g0 c' n2 y0 f0 L - Sensor_Data[k] = 0x00;
" G B$ k, b5 d! K3 d; c
- k) L0 X( `- s$ e) S+ I- for(uint8_t i=0;i<8;i++)
2 `! G; {# m6 a. }# c3 P% }6 ` - {& i5 l: C Z" i$ N
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 D* K c6 D% a* R# J: S. D - HIGH_level_read_time = micros();% a% C: C8 b1 P7 k4 X5 g4 w
- LOW_level_read_time = micros() - LOW_level_read_time;$ p! `5 n) \+ ~/ d, X ?
6 Q8 \0 U7 ^ v# F- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
2 N6 C2 e3 T1 T% B" i - {
0 N$ {, V" M9 q( ?% H' z( W2 q - return 0xff;# N& J+ f1 ~. E z" C) A
- }
: W6 z0 }1 W. @- {
& K4 ?0 K. V: W) c Y- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
, C1 A: k/ s" }7 I; b0 u' J3 u% Q - LOW_level_read_time = micros();
3 B( R8 E( p$ V( ?4 F - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
$ k2 j9 }0 R3 c& O7 f; O4 x - 0 B: p9 e1 Y/ P6 F& R( ]
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 13 G; n4 T8 r+ d( F* ~( }
- {5 f8 |+ z4 J) x& u
- Sensor_Data[k] |= (0x80 >> i);$ J# i# V0 }" D( j+ ]% U- {" r9 Z+ @
- }4 D) n1 G$ \: H/ t9 d' _
- else if(HIGH_level_read_time >= 100)2 i2 p3 H3 |- J$ Z; v
- {
. H* `" d8 ~6 V& g - return 0xff;. w5 u8 `9 T. z1 Z( S. T3 ^; @
- }
) C. X' o4 c. N& D0 M% n3 ] - D* N/ J" V! k
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
/ v+ q) l8 P- Q# Q - {
) H, b; @; x8 D! e- r6 d- _ - return 0xff;3 U9 Y- ]; ]2 T$ v$ D1 f7 T5 B
- }
. ?3 B/ t3 v7 Y' ~( x+ j' r - }$ u: O, s$ l: v0 r+ \; A, f
- }
! w! q/ h. H" r% a9 Y1 t# M% o
$ m. F5 Z3 g2 k# Z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
0 b! K6 t1 ?; |1 b% q9 Z - HIGH_level_read_time = micros();0 D% ?5 g/ J. l/ j3 H) ?1 _# v) m) J
- LOW_level_read_time = micros() - LOW_level_read_time;- d$ r5 ^, g ~) X
- c' s" }8 o/ r2 H- e
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )# \% |& O+ p* {1 [
- {. [0 y( `4 J; W; u. U I
- return 0xff;
8 |1 U( s e2 p& ~; Q - }
/ }8 ?$ w* d; g2 X: b- v - . {. ?: [$ T. q/ s6 c
- pinMode(_DataPin, OUTPUT);1 w) n: f; q7 {7 H% |1 z8 R
- digitalWrite(_DataPin, HIGH);
5 Y1 _/ N+ I# ^2 M3 d2 y& i4 K - ' P3 m# V/ I4 V) t
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
8 N% c* h) S8 ]3 o - {3 I& O' s0 ^6 @" H" I
- old_data = Sensor_Data[0];
4 ]% I: o W# i) y% \( N - return Sensor_Data[0];
: |; I" W F* P) c \/ H - }7 R5 T# c4 Q5 g( i4 t" A
- else
: x3 m' |. Q6 @ - {
- u; A' s5 \+ h% F6 E( t - return old_data;2 C* L% m( z% D
- }
8 T; ^! J2 j* B - }
, M4 t3 j9 {: P
複製代碼 4 ~0 e5 l# J+ S
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
% Z. t. q( R: U7 B9 V! a2 U. F* B q; [; b8 c! g5 [2 Y! Y
|
|