|
. E* m' f1 d' L0 L& `8 q' D' `- #include <Arduino.h>
' R! C- x- [5 Z9 u - #include <MeAuriga.h>3 V4 A, e' ]2 o+ j l: W
- #include "MeLineFollowerArray.h"
5 g0 t- f. n/ T( C% B1 Q1 I
$ Z3 p2 A2 }; z6 V8 V7 D- MeEncoderOnBoard Encoder_1(SLOT1);5 |) s# T1 J, L! w8 H( k
- MeEncoderOnBoard Encoder_2(SLOT2);
6 O1 `7 i. q* `7 [7 u0 j) k" E - MeLightSensor lightsensor_1(12);
. \8 g$ i$ ?7 g0 @$ Q - MeLightSensor lightsensor_2(11);# T% L* W& t1 i& V
- MeBuzzer buzzer;! a# X3 @ [8 L. z6 Q$ `
- MeLineFollowerArray linefollower(PORT_6);6 P, x m; x" B. P+ ^! a
- g& R; c k% Y+ }% d" y- #define Error1 1
" X! i( {( z3 Q4 ^/ z - #define Error2 2
3 Z/ I8 }1 X) p3 B- F1 m - #define Error3 3
' d6 T& ]1 Y0 T9 |) M( Y: h0 s. u
2 _3 a4 ~, g q* d* B/ J5 r& D- #define Kp 15. h- o$ P+ r% B- R& N0 J
- #define Ki 0.159 D' {) J) y/ C
- #define Kd 0.03- w7 o$ W8 ?9 \+ g- B) R$ q" Y
; m8 O9 Z7 n8 k6 g- uint8_t sData;$ A0 ^& P7 x( d6 r4 x8 b
- uint8_t D1;3 ^' o: B& R) z5 W. G
- uint8_t D2;
/ n+ }( L ]# r% M c% w - uint8_t D3;
6 h* R* q2 U L - uint8_t D4;
( t* d- v8 [& U0 U1 L# x4 ~ - uint8_t D5;
1 m& O/ a+ i7 N) d) o - uint8_t D6;
$ O' d5 w1 _& Z) d: g' h% H9 ^ - 5 f0 t% t- T; J3 ]$ J8 N# D. \, m
- float previous_error = 0;# R0 R8 @: Z A9 E6 g
- float integral = 0; t/ Z- d1 C! B6 f
- float derivative = 0;# R2 |8 G* Q9 a7 ?2 _; A. I
- int Speed = 160;6 }: b8 X, ]8 X
- float output;$ J: Y+ I. g2 b. A
- " D% Q7 m' g0 S9 C, t3 b
- byte Left;. ^. _" M7 g2 Z! h0 M( n
4 N) }% C9 h. u6 z- void setup() * X7 N. j, x. W1 s( O( W' d
- {( Z1 K5 T4 |' \; L% M9 V; i$ w
- //Set PWM 8KHz* O4 R2 H" u! k3 @5 K
- TCCR1A = _BV(WGM10);( {$ }& a9 I R6 q h" }0 @
- TCCR1B = _BV(CS11) | _BV(WGM12);4 |$ ~% `7 n, V% k
- TCCR2A = _BV(WGM21) | _BV(WGM20);
, ^( y, z2 A# @; J) {; \7 J - TCCR2B = _BV(CS21);
2 d' Q1 s! @1 N3 G9 h- n6 c - Serial.begin(9600);
3 F0 m( R( I, y% K3 ~ - buzzer.setpin(45);
" \: s g, D5 m - }
7 a2 u) P/ j, t; H n
# q2 _7 F# _; b: v8 U( ]- void loop()' L$ W" e. l# _, y! B
- {
- @" ^ b/ i8 r3 F( Z E - while(!((lightsensor_1.read()) < (10)));6 [5 A- z+ ^5 t1 u9 o5 V* |
- buzzer.tone(1047, 500);2 x$ \7 \7 t s% H. c; \. n
- delay(500);
2 W" K, E0 _2 @7 `4 }3 `+ N - do1 W! O4 T- x0 x: m+ x& A
- {
, [+ | z! I, K0 L7 S p - int Err = getErr();
c/ g( K8 h- N$ V - if(D1 == 1)# e* t8 T+ h' K
- {
5 R7 h% S' n; i; }7 p* y1 J - Left = 1;
" T: w1 S! s, ?% V* w - }2 W7 E) R, M* z1 R2 j
- if(D6 == 1)2 R$ N5 `9 e5 ?
- {: G5 s- P7 l, ^
- Left = 0;
4 g4 N/ D% e6 a0 a8 J6 V - }
% U" j& s0 w; Z5 G2 b: m; S - if(Err == 99)
" i$ E& M+ z9 S- j( ~7 ^7 O - {
) |/ y! U( i% P" X& R. P - if(Left == 1)7 j: X+ p7 A$ K/ t; Q
- {
3 L4 P8 u% n$ ]; Z" h, ~3 @9 O - Speed -= 5;/ w2 X. b6 W, ]7 k& S
- moto(0,Speed);
) D' y. L3 Q+ \. Q - do; p6 ?) }& z4 q- P
- {
% G% k, U! n4 [# a( D! D2 U - Err = getErr();
( p' S1 d! }+ V( o; p! [ - }while((D1+D6) == 0);
8 O6 g1 u) ?: Z2 I3 e" y+ j - }( X* K- _8 q- G R, m6 O
- else
% G* z" A1 S' w - {7 T% N6 G$ g: t; g
- Speed -= 5;
3 q; i! h! {6 a1 H+ {+ [ - moto(Speed,0);
2 m, |* ]: p+ G: z: i. I - do% B. A( L5 E7 X# D8 `8 l
- {
! H7 `" k- G$ @7 ^1 C - Err = getErr();
4 {5 l3 i5 v7 k+ Z; c* t - }while((D1+D6) == 0);
* {# ?! W2 w! }& ?* {- j - }
* J: U. a$ }# A6 ?3 Y - }
" O# B l0 O0 ~% W1 p' Z+ Z - else
( l5 ~- N2 U5 z) o: H `; @ - {
" Z6 v D& n# Z$ z - if((Speed < 160) & (Err < 2)) Speed+=1;
/ Y' r# d0 Z( C& A7 |3 }5 b - if((Speed > 100) & (Err > 2)) Speed-=2; 1 \% i; z; L K T) `- C
- integral = integral + Err;8 J0 B2 X/ X+ s1 t O* y' S
- derivative = Err - previous_error;
; E$ W3 A8 q3 p# T0 R6 Y& Z - output = Kp*Err + Ki*integral + Kd*derivative;* \" _ H9 o1 }8 ]3 w
- moto(int(Speed-output),int(Speed+output));+ H+ m9 N; y3 }
- previous_error = Err;
+ S: `" D8 I8 h, v( Z2 c V* B - }5 I' I6 l1 b6 g1 T* g/ B! a* F
- }while(!((lightsensor_2.read()) < (10)));
- Z0 X1 K3 s, U8 D' p& e& p - moto(0,0);) P, V# _( e; F5 T8 p
- delay(500);& M) g6 e4 O W1 [5 [1 c
- buzzer.tone(262, 500);* V# b: _" @6 D$ M
- }
/ ^3 u+ d$ Z, k& t1 D4 E
9 ]6 @1 \# |: j$ n4 @! E, O8 l- int getErr()4 y9 a6 {; V& f; r5 ^* g* D
- {
% T' T: i7 z- q) B9 c - sData = linefollower.getValue();
* y, e; Q; m3 l R; Q - D1 = ~(sData>>0)&1;" E# [/ ]6 ?( y) g' a
- D2 = ~(sData>>1)&1;+ B R3 V+ `, N/ v! X6 T- k
- D3 = ~(sData>>2)&1;
- o, d9 J7 D- ^6 x' V - D4 = ~(sData>>3)&1;
. ~0 }5 l! F3 z; A1 x5 E$ k0 I - D5 = ~(sData>>4)&1;
& [7 Y4 {% I: r+ Y' b3 X9 i - D6 = ~(sData>>5)&1;
( q! O' W) `" | - int downD = D1+D2+D3+D4+D5+D6;2 I6 Q J% ]4 z% p( V4 d8 K o
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
3 a- Z$ p' V3 |: k - if(downD == 0) q, C8 A# W/ \1 I* j
- {
, m0 T6 F5 l* l' p6 X$ l4 U6 c& e t - return 99;! d0 {6 b/ f0 |$ \3 b+ @
- }
0 X6 m$ V) T8 t7 M) k - else9 W! | w1 |! F: Q' S' u! _. t, L
- {
, R$ Q$ O3 |% V0 b: P4 r$ i9 L, W - return upD/downD;+ v! Q# Z+ L) o! S, P! o
- }
/ l4 J8 v; y) r% C - }
+ Z. D4 l. g: \' i% @
1 ^3 H, d( c+ g' m r5 `3 k6 p* U- void moto(int Speed_L,int Speed_R)
1 n) @; r" Y( m. p8 v - {9 h7 K3 E! g: G& x6 F- ]; {
- Encoder_2.setMotorPwm(Speed_L);
9 K8 C8 O; U& V) G0 ^4 m5 g' N - Encoder_1.setMotorPwm(-Speed_R);
1 o+ l$ @& V& Y5 @+ }: Z: G5 R - }
複製代碼
% G: P" m* N6 U: O2 SMeLineFollowerArray.cpp
, M$ r( ^( G4 o* C% N1 z- #include "MeLineFollowerArray.h"
+ } T& C, b6 [3 n) ~7 h
6 }6 s- K$ p, L( [3 Z! d- #ifdef ME_PORT_DEFINED
/ x, z, K6 |, w! E8 N - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
/ u5 K, r% b. h - {: |5 B# E7 Y1 s/ ]9 ]
# b3 A# c/ ^/ K- }
# W. A9 O7 i% |9 Y+ \9 m - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
3 Y4 d r0 \+ Y" e - {
% t" G7 ?0 i4 K& v6 x) p - _DataPin = mePort[port].s2;
9 l9 k* b0 v+ x6 w0 ^7 ? - pinMode(_DataPin, OUTPUT);
8 ]- q( C' q: y - digitalWrite(_DataPin, HIGH);7 j; I: ~) ^: M. K
- }
( E/ F) j1 m: m - #else // ME_PORT_DEFINED
5 t2 _7 n+ z8 Q7 {; B% N$ P - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)) W! ~5 l- t7 _5 L' }
- {
3 p" M( ?! c. d) v - _DataPin = pin;' O* ]$ \2 A7 o' P @
- pinMode(_DataPin, OUTPUT);
- j' h4 J. z; r3 r& X: {2 N) n+ } - digitalWrite(_DataPin, HIGH);) e; o4 j9 g2 K& T, o
- }- ?4 W0 v% O8 J( ^' u7 P! O
- #endif // ME_PORT_DEFINED
* g$ T9 b/ ^$ g" p3 P: X - - Q2 t- Y& b4 J: O
- 6 q# c3 H/ D h# V% p& ?8 Y
- void MeLineFollowerArray::setpin(uint8_t pin)" t3 i9 {/ q5 ^6 m4 Q- y6 J) q+ z
- {
) `+ N& C7 k" u - _DataPin = pin;5 H6 v, M0 e7 K+ z# y
- pinMode(_DataPin, OUTPUT);
1 a' L, c) L; V6 Y( a - digitalWrite(_DataPin, HIGH);! d8 @( t5 u0 J5 G' ^0 q
- : k) v0 e" ?. z3 E6 G! ]1 k
- #ifdef ME_PORT_DEFINED
+ b9 k/ `8 |5 z ?. _! p - s2 = pin;
4 V+ V5 F% ]" v: u* D6 v) D6 I. {& k8 } - #endif/ `1 c& x. v2 \1 I
- }) Q1 r, C8 t5 q" l4 L3 a
( x, v* q1 P& f* ]* @. ~5 m/ e- uint8_t MeLineFollowerArray::getValue()
! Z+ e& v, c" @ - {
) D* Q; T0 ~) ?# z - uint32_t LOW_level_read_time;0 q8 H9 n% N! H. g8 Y1 N, }' h
- uint32_t HIGH_level_read_time;% e) S$ Y0 w1 ~1 E$ a
- uint32_t time_out_flag;( M/ ]6 _ {1 |! O, m; u
- uint8_t Sensor_Data[3];
4 O1 |5 n8 i2 m& { - static uint8_t old_data = 0xff;8 w" ]7 s# ~) v0 C- r0 `
! k1 o ^* ^' ?* [) `7 F- pinMode(_DataPin, OUTPUT);
3 Q7 Z& d( z2 e5 Z, b& b7 V - digitalWrite(_DataPin, LOW);
* @& S' _6 Y+ n+ V - delayMicroseconds(980);0 t+ i- G& |7 h, Q
- digitalWrite(_DataPin, HIGH);1 v: m' s" |3 E
5 G9 c, S, ?! Y+ z0 ^& Q6 L- pinMode(_DataPin, INPUT_PULLUP);8 v! p, o+ k) A& J
- delayMicroseconds(10);3 {( J% I0 {8 b: c* x0 u
- 9 @# |3 h8 t# @ T
- time_out_flag = millis();
1 ]/ U7 f6 ?4 J& C* R - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );: Y+ ^6 d. v2 y! T$ ~5 y P. X
! X, i& {9 P; ^$ t. [& B- LOW_level_read_time = micros();
& D- k; t, \! ]# ^" g. U! K - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: c/ d- Z3 U/ v- E9 a1 v
- {/ y+ i. ?; t6 F) O
- return 0xff;
# ^; I/ N w9 h6 D% ~" P G - }
: v; J8 f, m1 B, g" a: j4 ?) w, T
5 L7 l6 @3 g5 I) s7 A( R/ H0 W' @- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
% p3 b: l* y! C* N$ r- B: O - - O \% a7 e( o; k, | F, n$ e
- HIGH_level_read_time = micros();1 T, k" }5 b. @' s+ }4 ?2 |
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
4 z6 N3 b' P+ v! h' X) M - d' ^, S/ Z8 P( C6 M
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
$ Y# q' [* _" ]2 n& G, o - {; ]3 D! ]/ u- Y& {/ N- K2 d9 F
- return 0xff;. B8 ^2 P. I* k3 r$ H( Z3 Y
- }
+ y+ S) r: \( Z+ [( K8 X/ u+ ~
: w' t, ^/ j7 z- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))0 _( p% |+ j" b x% L* S
- {3 f. G( K1 B! p; A; K1 L; E$ e. L
- return 0xff;: C6 W. J: y4 _* m
- }/ N" @9 B& g$ N- p" M$ E3 g" I
) E+ E7 b8 Y/ F: v2 w- a- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 `% I, ]/ g- @! C& ~! h - LOW_level_read_time = micros();: @4 M6 I5 x7 Y+ Q5 t) J% C
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level4 L( y _. B/ q4 G) ?
- , T7 N% i6 r/ p' a! ~( P& V. t
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 w9 @0 B1 K" u! k3 n6 R; m - {
3 Y) l% L) M/ o( X! R - return 0xff;
5 A! i' }. P# C! f - }7 h: @) o; j9 h" _9 C0 W9 g
2 c7 Y' l% ^* I6 H- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))+ `8 k2 y% i+ |
- {6 c3 g3 a* b6 T* O$ u. F
- return 0xff;' e, l) `* R) s; X# v
- }
- O$ X7 O6 ]# e5 ?/ Q2 k* o6 a
% c% v" V v6 k( W- for(uint8_t k=0; k<3; k++)' w! m5 h5 t) t s8 G/ R+ S
- {
! l; @! z/ [8 B9 {- }: b/ ` - Sensor_Data[k] = 0x00;2 w- S% f* i0 F4 x( m
& r" j& J! V3 A' N! x) K. {; Y- for(uint8_t i=0;i<8;i++)
Q* Y( m% Y* g' M1 ~9 x8 n; C0 E - {
: J9 c- O' D) T C - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level$ h5 k/ {+ R4 A; ~# h) c3 l
- HIGH_level_read_time = micros();
0 q' v% N) l% ]6 m - LOW_level_read_time = micros() - LOW_level_read_time;
( i: h7 l3 O4 D
- e6 G" q) ^5 q! u% P8 q7 ^* |- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )' S# v, M$ o, N; C5 Z
- {
! }9 _0 ~6 j3 f) F - return 0xff;
) P/ F* ~3 Z+ g5 t; ^& U - }- W, M+ b* d/ ?' C
- j6 p- w/ ]1 R1 R M3 H- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );( ?9 v3 R- W5 w ]: f
- LOW_level_read_time = micros();
. k. i! G) X! T! Y3 [0 V/ n - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level! Q7 X7 h/ S# n) H/ T/ Q, b
1 K8 U$ g3 O" ?! O: ?; x- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
( I. B! c( \, r; ]* ?' b - {
# M3 S! Z4 w! k7 h8 P/ o - Sensor_Data[k] |= (0x80 >> i);8 `) {( f. ~/ P1 k# J% @" O2 q
- }
* P: O# r- e* h( b - else if(HIGH_level_read_time >= 100)/ x1 ^2 \- t8 k h9 u# i$ f% Q g
- {+ \1 k5 g7 t+ \& j) D% C
- return 0xff;. o& T" ^8 P. P: g1 _; c
- }
- ]8 `. u& r- {5 [3 N0 _' E - 2 S2 l, K r( L* t, B+ T
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
3 Z- E: C9 k& V' q* k' r* u - {
- D8 H) j3 s0 b: x; O - return 0xff;
1 S7 c5 }4 h/ F2 ]' ]# e1 ] - }- \7 {6 o# p/ L/ N, |
- }
) \6 u* F' x9 j' L F' U - }; V. F" Z5 q5 f0 J) g! T& {8 P) d
0 R1 J" J" R" t3 @/ U; H- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level: y/ r, X2 k0 m- T
- HIGH_level_read_time = micros();
' f d6 F. G0 E/ e - LOW_level_read_time = micros() - LOW_level_read_time;
. }8 Z. `! U) b# ^4 g3 S0 X; P, P - % o o o/ f3 s, J6 `/ D& u$ L
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
) [9 ~( V6 ~- x' K6 W) R( w# S. J8 V - {9 O p! i, C$ B! a* N1 @. ^
- return 0xff;
, K l6 k: T/ O* n1 R+ O - }+ `! K# N& v* p+ U& n! M9 C. s- L1 J! p' z
- & ^0 G- ?! x5 x6 q0 s
- pinMode(_DataPin, OUTPUT);( v; W7 p, I6 c& a7 M" Q# F" y5 x
- digitalWrite(_DataPin, HIGH);
9 J* D7 q2 y0 i2 W( i5 k - . x$ i# q0 w( Y9 O
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
/ b; p# H9 S/ Y* y* D& |+ l2 r/ w; U i - {
: X0 O$ h/ v, T* B6 o+ a) o - old_data = Sensor_Data[0];
4 n/ s) q8 ]# \. ], R+ N - return Sensor_Data[0];3 `! d' Y; P: Y; B% m
- }2 Z9 V+ ^2 u" O' @0 T0 p: t! _
- else
; i7 N' o6 C }/ S - {
7 X0 |; x9 _+ d# Z3 Z0 w - return old_data;
' F8 u5 z: R! n0 Y - }
" P1 N, K* r* c. ` C2 p6 t$ ]9 f - }
t3 X$ E# |) B; @: P+ K
複製代碼 ! B8 X* O: o: Z7 h, G
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 r0 T7 j2 M+ Z8 B+ t
/ a% j/ d4 z$ }: u% Z |
|