|
|
# Y: B) v, A* i- #include <Arduino.h>
; r# R: Z! b4 p! U - #include <MeAuriga.h>
. i; t6 k) X& H* Z" O - #include "MeLineFollowerArray.h"
. T1 i; Y" V8 J+ A4 @6 @8 R7 j
6 l/ ]: j) q: \; Q) |- MeEncoderOnBoard Encoder_1(SLOT1);$ R' D. V, `8 e3 p3 A
- MeEncoderOnBoard Encoder_2(SLOT2);
. G! N, ^) r# p+ S6 m, E. Z5 L - MeLightSensor lightsensor_1(12);4 ?( r$ }# M+ g4 E9 y
- MeLightSensor lightsensor_2(11);6 D$ X; |9 z6 {& G' ^
- MeBuzzer buzzer;
! J# F( k7 M% n' r2 I% ? _9 G - MeLineFollowerArray linefollower(PORT_6);
+ J" { S0 p/ S
8 |1 j* c" V! c; n# `0 g3 M& y, E: O- #define Error1 1
]7 u" J; d+ k1 L- I1 J - #define Error2 2
6 d( ^: B6 O1 ~0 _ - #define Error3 31 S+ Z+ A8 T: w9 q" k' J3 _9 y
- - S; |* U9 u" ~* ]1 m
- #define Kp 15) F' Q/ H, q3 O- }. d
- #define Ki 0.15$ |9 j! i) X( p9 F, v
- #define Kd 0.03: P: \8 z3 h" ~+ Y, M
- 5 y. U. {) ^% l4 n5 w# S
- uint8_t sData;
5 ]1 B" z' }# f) L - uint8_t D1;
: J+ }2 N, D: d - uint8_t D2;
0 e& x6 k) ?# c - uint8_t D3;
* V" Q! I# V6 Y8 P2 @2 _ - uint8_t D4;
B5 r2 K/ \2 k# n4 O z, l - uint8_t D5;
' X. I* i8 l, k - uint8_t D6;+ |+ ^$ j7 m8 f2 w- M2 h
- 1 a; {8 |" E& l
- float previous_error = 0;
# r" G5 _3 K1 k - float integral = 0;
* \& @* I' ^2 N5 |; I - float derivative = 0;
) |$ g$ |- m7 r: x: p/ p3 _ - int Speed = 160;& Y+ v) \' X+ A( d3 ?
- float output;9 t) x6 F1 l* b
$ ~& X8 ^, [6 e/ B- byte Left;
; h' h! O6 @4 E4 u) b1 d3 | - 5 _+ _. t3 V+ M7 y# p" R! I
- void setup() . L* H8 d, U; H$ l( @5 |
- {* M' G& a4 K# R; b ^
- //Set PWM 8KHz8 h. u x, d5 X' z
- TCCR1A = _BV(WGM10);
! U4 A H. i3 Z2 S/ A# [; | - TCCR1B = _BV(CS11) | _BV(WGM12);' p. E a4 ^# w
- TCCR2A = _BV(WGM21) | _BV(WGM20);' [/ U5 b2 B# _4 c) r
- TCCR2B = _BV(CS21);
; [. _! ^( G8 ]& [! y - Serial.begin(9600);4 `0 p8 _9 r0 O. t! l* B! N
- buzzer.setpin(45);
" X7 T7 y6 N: E; y - }
( N8 K2 F/ D# s% c - 6 L. w7 x/ k" s9 c
- void loop()
1 E. C! d6 @) ]5 E - { T$ ^% R% c* L/ F% H' x. R! P( k8 V
- while(!((lightsensor_1.read()) < (10)));& ^, Q c4 |7 i! s/ v& O/ r
- buzzer.tone(1047, 500);2 s/ O: s1 \. w' K( `3 x8 y6 _
- delay(500);0 a3 G4 E# Q9 E& O: N
- do" \9 ?: T( A& K# u6 V+ ~
- {9 s1 M$ V8 h5 C) Y) t7 L) p
- int Err = getErr();
) v6 A1 {/ S! R' P# a) C3 s( ] - if(D1 == 1)
' F% Q, E6 M' x2 |$ c3 N - {
) [* u$ [5 q* _2 f5 P3 B1 f9 R - Left = 1;
- k4 ^* h T0 w3 ]* M - }2 F+ r$ {! a& ]' G
- if(D6 == 1)
! h0 J0 `* R9 S! w1 y* u% K/ z - {8 T" H% y" h# B; r) I
- Left = 0;
, X/ H0 a2 ?0 W0 W - }
# V& W! _- J" }8 ^. Q+ W+ a q - if(Err == 99)
6 B: i0 s& b; Y - {' x* |8 ]/ W/ F: O1 f
- if(Left == 1)9 w7 F/ b% L* e3 A' A
- {
0 m- |2 I/ B* U% ~7 Z - Speed -= 5;
+ o1 N% w4 @$ d, S* g - moto(0,Speed);
3 i$ |5 [. f/ | - do6 v' o" u: M- U. @, o
- {' [ c+ [7 P4 E+ m2 q* t5 G
- Err = getErr();
" |# v L4 t! b - }while((D1+D6) == 0);% Q4 j# e; K9 j! K/ s! N. ^
- }3 w$ F- ]2 j0 Z9 B1 o
- else
9 ^0 ]3 Q1 g; ?8 V! \, ?1 f! h) E- P - {6 k9 a8 t+ p. V i2 {& Z, X3 Q5 h
- Speed -= 5;" z2 E5 P6 M y9 v. \, h
- moto(Speed,0);
. W) W0 K7 N+ b4 r; j+ V - do4 @/ W ]- X0 b* O
- {' r; p5 @! J& q9 u
- Err = getErr();
6 v- f2 m) [. V# u( L* q - }while((D1+D6) == 0);
9 R1 u. i/ b& }2 }8 Y - }) A" V1 ^& E- `- d( x
- }$ E/ E x. ~. ]5 v s
- else) N3 B: w# ]+ b
- {3 t+ L) q& I* q9 j0 r& g2 T+ {2 a
- if((Speed < 160) & (Err < 2)) Speed+=1;
) V# v; C" g( M8 H8 k3 M( G - if((Speed > 100) & (Err > 2)) Speed-=2;
7 `% ~" ]' _6 F- W9 j* O2 W - integral = integral + Err;! [% n) o, `' H% Q" c
- derivative = Err - previous_error;0 A' v! R( Y2 Q- \
- output = Kp*Err + Ki*integral + Kd*derivative;
& b5 t" O1 c3 \9 y+ }2 a9 [ - moto(int(Speed-output),int(Speed+output));
; @' z8 \, d) W k( H - previous_error = Err;
% e0 J- `& j9 D; D% @8 n! S; a - }
) z9 j* D' F% ~$ g& W - }while(!((lightsensor_2.read()) < (10)));2 N1 o8 I: u) p! C+ k
- moto(0,0);1 X% D; T. H1 L2 v$ f+ }* B
- delay(500);
3 ~2 `" W/ z8 q7 A% y( ^7 ~* r - buzzer.tone(262, 500);( o6 |9 Q5 A7 b
- }1 s/ l/ C6 a3 K
* y& R1 F7 G. h# Y! b1 N- int getErr()
4 S; o/ r5 j& l$ a: N - {
4 d7 ?3 k: X2 t ~* j6 V2 y - sData = linefollower.getValue();) F8 m- c3 N) |& }, A/ A5 G
- D1 = ~(sData>>0)&1;0 i# n! ]6 f1 @+ i0 t7 B
- D2 = ~(sData>>1)&1;
+ [8 ~8 M4 ]* q# j, a& I+ I - D3 = ~(sData>>2)&1;
0 z I$ P4 @- o L, a M - D4 = ~(sData>>3)&1;4 W! ]4 e$ M) s* T
- D5 = ~(sData>>4)&1;
# p0 V8 ?' p3 ]5 b9 A% Z - D6 = ~(sData>>5)&1;6 K3 { B& u2 t3 w' s6 a) q
- int downD = D1+D2+D3+D4+D5+D6;: i+ x4 E: v1 e _% c6 t
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
0 \" s s5 s b( @; G - if(downD == 0)
- \' f# i. f9 R# x - {1 R0 c o$ p* Q/ r* p
- return 99;
4 e* l1 p* C6 K( c' P! s5 E - }
- ~. n. l+ D+ ]- a - else" ~0 r' W- j2 X/ @! \2 t
- {
# ^7 y3 G' j) a0 X5 u - return upD/downD;
' E+ A9 n- w$ l, e9 u1 [! d - }5 j4 e1 s! B% B7 v2 Y% z+ d9 I: s
- }
. ^- Z! B9 z$ l$ Y" o+ x1 Y
7 |9 [& M, w' I4 n- void moto(int Speed_L,int Speed_R)
$ I4 u; ~3 }+ I) ]9 _& M2 k3 B - {
6 F8 c% {" X1 O+ o - Encoder_2.setMotorPwm(Speed_L);. M% v5 G: G) p. A
- Encoder_1.setMotorPwm(-Speed_R);4 t M5 K" R4 B! |
- }
複製代碼 7 F0 I4 L' |& T7 Y9 |
MeLineFollowerArray.cpp* H, o2 v5 a8 h4 u) L) d; T
- #include "MeLineFollowerArray.h"0 r- ?. _, ^0 x& d! K2 f, I' `. }% R% e
- w/ f1 s# p7 H$ }; R& c. D3 o
- #ifdef ME_PORT_DEFINED
' Q+ p1 o0 Z( g! }& Y' Z( a; A - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)9 W) T5 v, {' ^9 T9 k% s# S. [
- {
% A0 c. E( f% g3 J1 f - 1 q5 ]+ m: d( u2 z
- }
# X# F/ B$ c# J; a# { - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)# q. i6 q+ ^: r
- {
" B' d5 S/ Z& O K, m, t3 i - _DataPin = mePort[port].s2;* x- C. q3 T% F% {1 t2 Z2 b
- pinMode(_DataPin, OUTPUT); 6 D) f0 }/ z5 b( J* J% v: y. F( Q$ i
- digitalWrite(_DataPin, HIGH);1 D. m1 D0 t5 R8 }& j4 O7 R% D5 \5 t: g' r
- }
+ m( d6 l- _( G( K - #else // ME_PORT_DEFINED. \. h& _" @: Z* w1 J
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)+ f# X- W8 p: E. g5 B1 P
- {
/ w0 X. Z3 N+ k& L0 A2 [7 x5 I5 ` - _DataPin = pin;; y7 k1 z6 V# G2 V7 r0 C
- pinMode(_DataPin, OUTPUT);
; G7 @6 H- K0 l5 n& n+ r% x - digitalWrite(_DataPin, HIGH);
7 b. M) c e; j4 i - }/ w) z- v! h8 M/ ~9 D' \
- #endif // ME_PORT_DEFINED2 Y5 |; Z% `- |
+ O+ `, |7 Z( Y- # Q& E4 q' I) h% p3 r* O
- void MeLineFollowerArray::setpin(uint8_t pin)
. Q _5 q/ ]. G3 M. e/ ^3 z& E - {
1 u. I/ y. f" ^2 F: M! f0 g - _DataPin = pin;% @: c* Z2 P: T% _/ n0 a' T3 J. Z
- pinMode(_DataPin, OUTPUT); 6 _9 @9 E$ _0 W/ ^1 {* ~
- digitalWrite(_DataPin, HIGH);
3 S" u2 K* n. T. O7 R+ ~
7 t" A. X) ?! P( M/ z- #ifdef ME_PORT_DEFINED
4 F2 [9 E9 B7 x: }2 I% Q5 a2 n. k, V - s2 = pin;9 M. v" Q: {; R2 Y: _+ _
- #endif! A4 v R' h& D! w% f$ h
- }
. ?4 h* q% i$ L5 `& O
' D, ~ M) R8 E a2 d0 E. V7 l9 Z3 _: I- uint8_t MeLineFollowerArray::getValue()" p# C$ K8 }+ z. u8 S
- {
/ j1 A: A7 N: | - uint32_t LOW_level_read_time;
/ ]. T1 Z- p# W - uint32_t HIGH_level_read_time;
! Y' F. j* R3 p& {* S* | - uint32_t time_out_flag;( i5 s; M7 X( @1 Z
- uint8_t Sensor_Data[3];& ?2 a: B/ S) z+ ] f5 B
- static uint8_t old_data = 0xff;7 Z9 B- D6 g6 X, E
$ Z+ t/ d y) D- pinMode(_DataPin, OUTPUT);
3 v( `0 u5 e/ ~2 {$ D- S& K - digitalWrite(_DataPin, LOW);! T2 t* Q- c. \/ M% S
- delayMicroseconds(980);- y# x5 a0 g: ]& ~: f
- digitalWrite(_DataPin, HIGH);1 y1 j9 g) D! w9 S
6 f$ L0 I* u( V) H* e" N- pinMode(_DataPin, INPUT_PULLUP);
: f0 v* U8 S+ ~4 a) x, | - delayMicroseconds(10);
( T6 O) Q4 [& e8 i
" M( k: @7 [! E; m8 D- time_out_flag = millis();) ?+ W: B% X9 o. d9 G; S2 \2 y
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
# ]% \9 z& q* x* b% M- ^ - ^' d) k+ B2 p4 c% K
- LOW_level_read_time = micros();
+ w0 H5 m/ {) m( v, R: w - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
% b1 n+ g1 F1 R& m; K- E8 ]: Y - {7 U+ C8 h$ e- F, h0 J
- return 0xff;) F1 u' m( {! R6 g" T& Y; z) B4 ]
- }9 Z% ~: U+ d/ ]% a
& s1 y& V, u. Y7 o" X- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 C6 F$ Z) x% N* S# H - : S; e5 K$ K {4 u! y
- HIGH_level_read_time = micros();: Z$ c9 t; f9 G& g0 r ], C5 g' Z- N
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level8 t% H @6 a4 U8 X* q+ b; y
" a( _6 Y1 Y4 t' ]$ j- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
* q0 E' y) R) e- I - {
: \9 ] \; \1 x6 b% R - return 0xff;$ A7 [1 i4 e* y, [. [$ u: l/ ^ I
- }
. ?* G1 e1 i5 t
+ ^, [. P9 A4 W. G- v- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
. W( z( ]2 q3 H5 w6 C$ o1 X( F - {' h; y# L( S$ a3 [, `+ H3 q
- return 0xff;) U% L( _) h) n$ v
- }
l% s \0 w0 N: b
0 |5 s g7 t" q) { u1 t) d- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );" L( U/ G K" H0 o$ E' u% k
- LOW_level_read_time = micros();% p5 X/ z& u( e% a# c4 @* D
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level2 G! t/ P0 D0 N/ Z7 v, }
- 2 _# @% o" D: U) g2 N! _7 L& ~
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
\+ ]( |7 I: H - {9 ~& }8 |2 T/ Z4 r
- return 0xff;" ]! j' Y7 m8 O, b) h
- }& k0 ]7 M, G6 w+ s" y' }. n2 Z
- ! e+ O) u- p* F4 G7 S% r7 d9 B
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
9 h# }4 h/ o e0 t+ g7 F - {
+ w( C1 ]$ l1 g. w; X0 W9 M - return 0xff;
5 M# R+ u2 ? t, L, b) v! ?2 { - }
# d( d) S( n7 }1 V
) S: ^2 a2 t" a/ ?- for(uint8_t k=0; k<3; k++)! O- c# x3 Q3 y! t! Q
- {
1 w- R# M; K" d5 ? - Sensor_Data[k] = 0x00;4 _7 ?! @1 ]# Z) T, F
: L F% O. d: v$ G' U. p' q- for(uint8_t i=0;i<8;i++) W7 }/ K# ^5 r: {. d
- {
% v& [( Q1 |0 `/ D1 \! M - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
( {: Q3 r) u% F3 `9 G - HIGH_level_read_time = micros();
9 \' V4 s( P) z5 i& j2 r- o - LOW_level_read_time = micros() - LOW_level_read_time;
3 p9 X* y: t+ k# A) i( x
0 i, {$ i: Q, i& {" z$ V5 n1 Q$ S- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
& `2 g/ L+ V8 G& G+ m2 G( N7 z - {
+ X* m6 {* J1 @$ [5 z, } - return 0xff;9 j* f3 ?% V& T( b8 M
- }
5 D+ E5 ]7 x: d9 C
+ ~% h) y' ~; L- N. T- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 D- X! s( ~5 C - LOW_level_read_time = micros();) }# J+ c2 E! s
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
: v; @% K2 p+ |: F0 m- c) N3 H0 G( P# h - # q& F- W. M; O7 y
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1& Y+ o# `3 x5 i/ Q7 F: `* h" A) _
- {
+ J8 }9 N6 J+ s: r - Sensor_Data[k] |= (0x80 >> i);% n3 z# {) h) \& c& K: L' B
- }
& m3 ~ r' E2 c% N" G - else if(HIGH_level_read_time >= 100)$ I1 O) D/ [+ g" a; W5 p/ {+ L) L0 q
- {0 ]9 k# C7 O2 h V: N3 s' x3 H
- return 0xff;% g3 ^! z3 o" {6 q2 A: h' g. U
- }% h, L( ?) `) K
. T' p: u+ R: O# o: T- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
4 s3 ~( r- e4 t5 |" [ - {2 o- ? z4 a% M0 n: B0 M4 U
- return 0xff;* C) n' e" l& k% b. h; l- @
- }
) k9 H! F) l3 i* F# [/ A! M1 l - }
w6 E& l' w$ l/ i - }
/ B; `. ^8 l. `) N ^* a9 e
% w# R1 i" I7 n. E) l- z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 }6 d- c9 n1 v$ V% N' g, p - HIGH_level_read_time = micros();
' ^, X: o% ~5 }! e - LOW_level_read_time = micros() - LOW_level_read_time;* T f: a" j8 \4 C2 U! e
- - b M r$ U/ U. K4 K
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )- ?4 T) X, e* B J, p, |% o
- {
: [' b* S" S3 G! k - return 0xff;/ [( Z1 e- e; \( U9 k% U
- }$ |! d- T: T$ T) P7 g C
n/ |% p9 n9 t, l, S- pinMode(_DataPin, OUTPUT);& F( e2 `. @: g' Z% g& t
- digitalWrite(_DataPin, HIGH);
! G0 E, i N; v; F
. G. P" W6 ]( Z; T8 I- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
4 |# @! |3 V% x$ j5 V" m, O+ m - {
2 h4 u8 J0 G- S9 g: U - old_data = Sensor_Data[0];+ V+ W0 b5 G/ T; g1 B* J- Z
- return Sensor_Data[0];- {6 M( u2 X. [* e Q
- }
* h. E& D, j8 l2 P; E; T - else
& ?. A7 _& `2 j/ d1 ~0 b- Y% G% b! c) i+ q - {7 a0 t# T; h; u
- return old_data;
6 H- R) U& h5 P+ z% b - }4 |* X) R; g, q0 @/ }
- }/ k0 C& T( d/ B8 p
複製代碼
" H4 G7 @9 ?' m5 ?% DMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
4 y; l9 h9 L# I3 S
5 I) t7 o" B0 Y' a; W1 I |
|