|
|
* m# p$ }1 r+ b# s5 G$ I
- #include <Arduino.h>- b. _' X5 i% _; R
- #include <MeAuriga.h>( l6 r/ l/ q1 b, Q8 ]7 w2 I) |
- #include "MeLineFollowerArray.h"
& b8 t& b1 K7 m- s$ u0 } - * c. J) ~6 Z! ^& O9 y. M! V
- MeEncoderOnBoard Encoder_1(SLOT1);9 w l* R$ d+ X& Z! `9 R
- MeEncoderOnBoard Encoder_2(SLOT2);) G l$ h. O! G& |- {& j: w
- MeLightSensor lightsensor_1(12);/ j4 x+ R* h! H, v K) M* X* s W
- MeLightSensor lightsensor_2(11);
3 s* |2 |# N' h* j - MeBuzzer buzzer;
' ~% e" r6 p4 F: J3 e3 @ - MeLineFollowerArray linefollower(PORT_6);
; ^1 F9 b1 ]% n& V
) L1 C7 [- c' b- #define Error1 1( M4 z+ M6 I: d) d% H; q0 i
- #define Error2 2# \* h2 X9 W9 D+ ~7 m
- #define Error3 36 n$ i1 X/ J$ Y4 ^5 `4 N4 d5 b' d
% W3 i# m3 a; I' R3 A& u# y1 F- #define Kp 154 B8 t& q0 M* F2 F9 P( i
- #define Ki 0.15
" N7 H3 ?! B; R) J; [" K9 ~ - #define Kd 0.03- Y1 q& f; R9 W" A; D c. Z
- 7 s2 ~$ o% e) \
- uint8_t sData;' ]& D3 n6 M. X0 f1 |; n
- uint8_t D1;( E) R* q4 }. z# b' A" u
- uint8_t D2;
9 b& v7 O: j1 ]( d8 n7 o - uint8_t D3;
, _# ^$ \. I& [/ g: F - uint8_t D4;$ m# a* R7 D1 p- p" i; \5 v& \
- uint8_t D5;
/ H. e! P; T5 {2 s# j$ G. a - uint8_t D6;
, S) O! d* L4 m$ |, g - % [8 p# C: r a( m5 R% L6 c
- float previous_error = 0;$ {6 p' t' w4 Y' b- O! g
- float integral = 0;
# c: l/ @) {1 ?' ~ q1 F" Z - float derivative = 0;
$ K$ E5 ?, b+ l0 U. h- L3 A - int Speed = 160;% f p/ A7 }6 I" O: `/ ]: ]7 v
- float output;4 i) v" s" b i( R: D3 c7 k: F6 H
. G% U' Z A2 K5 l6 x- byte Left;, M' r }1 A t& x
- 0 c- D2 s" e% [9 q! E3 r
- void setup() 8 a2 k$ S* j5 k- i. [1 J
- {0 @$ f* I2 u" X+ K7 O
- //Set PWM 8KHz! m3 A$ J* ~+ ~" s- N& o+ f
- TCCR1A = _BV(WGM10);
% v/ A+ d6 b1 ^8 g) k3 N" T, P9 R1 ` - TCCR1B = _BV(CS11) | _BV(WGM12);7 J0 B; @4 b7 e2 f$ u( h9 \
- TCCR2A = _BV(WGM21) | _BV(WGM20);6 `0 {! @9 p1 c- u1 E* D
- TCCR2B = _BV(CS21);
" F! f$ `0 |8 L* x) P - Serial.begin(9600);
) c5 x7 U6 Q% C- N' x0 ?% i$ m - buzzer.setpin(45);
' W1 B/ p& a2 ~' V - }, @ c& j( A& C
- ! P4 k0 w1 D6 b' E+ v* _/ x
- void loop()' u" t* X8 k; X+ o0 D1 V* f4 X, u
- {
% U* }( N% _% c) g* R- f - while(!((lightsensor_1.read()) < (10)));
* B0 f6 M d, i. J3 ^2 v - buzzer.tone(1047, 500);
$ Q5 H, _5 W& } - delay(500);
6 `; x% N; p6 A( h. d) P - do
" o4 `. z9 D( o5 M- H) e/ w - {
+ |1 x* O8 h9 W - int Err = getErr();
B+ v+ o: C# n8 ~/ b. R5 H - if(D1 == 1)
7 a/ F3 i/ I9 Y" c; j9 y& @: p - {" R6 M/ b: B! j: W% k. h( t2 ~. p! z
- Left = 1;
& m$ u: a4 d& h. Q* Z - }
# ]3 M+ w% a3 \+ o" P - if(D6 == 1)
! A6 y- w1 o& o, o0 D# n+ L - {
% Y% R# k7 o* z/ a( q& S4 { - Left = 0;; Z- o8 E5 V0 T# S6 M% S" c
- }, q1 n9 t6 k' @. @" R, F
- if(Err == 99). ^7 k Z( M: d' }$ e
- {" q/ Y2 T' P" E* S% |' h$ N' c& u
- if(Left == 1)
" h9 _$ h& s9 S, \: x- Z4 C - {
# l0 s$ |1 P6 p8 I6 Q! E9 E3 g - Speed -= 5;4 l) |5 u3 x6 r# y
- moto(0,Speed);
+ z0 M8 x# ` I: X( b0 y$ x$ h5 G - do
" q5 P) B/ x; a9 | - {+ S' ]# \, \ M6 q# W
- Err = getErr();
# K+ M# K$ X( \% B( T# Q7 ^" b - }while((D1+D6) == 0);% _" H0 \! l# A- r4 w
- }0 r8 {7 h' r( A" Q
- else0 y: C+ H- D; B* ?0 k! R0 c9 t2 [
- {- C! N0 b+ X" S
- Speed -= 5;$ M- H- O5 n( N' l6 v* m0 r9 Y4 T/ Q
- moto(Speed,0); . H f: X5 u5 y, e. _+ W
- do
# | Q& J! A0 N7 ? - {
6 @5 ?- y* ~# ~# E9 L - Err = getErr();( ?( z w, G6 z. v7 h) A/ K
- }while((D1+D6) == 0);. r( l& E' q6 p
- }! `# i7 z0 C2 I% d. S2 c
- }
' J: [* [& F. `0 Y - else$ _1 P0 J) p+ y" X5 a; d
- {
5 s' b5 O0 Y$ P( x/ P - if((Speed < 160) & (Err < 2)) Speed+=1;
' y F8 [- [/ F8 t - if((Speed > 100) & (Err > 2)) Speed-=2; 7 w, d5 c+ H& |: @# W: `& n
- integral = integral + Err;( ?5 L% I0 p; n* b
- derivative = Err - previous_error;. ]# m+ L$ }! t: w3 W/ |* e
- output = Kp*Err + Ki*integral + Kd*derivative;
" J5 j+ y% z1 r2 O - moto(int(Speed-output),int(Speed+output));. \- X2 O C2 u! v5 S
- previous_error = Err;9 O3 g; b4 L7 u( b6 O M, K/ b
- }5 L6 \0 D7 K9 X0 c% ~) y
- }while(!((lightsensor_2.read()) < (10)));4 V$ }5 U5 m$ \, X- L1 S
- moto(0,0);) V4 U! }6 z, O
- delay(500);
6 O8 h+ b& K' _+ W4 p - buzzer.tone(262, 500);6 n8 `' j" C! Y& S1 J6 g" j
- }; S6 A# n8 o& ]$ `( z, L
5 h8 M1 {+ S9 Y7 c4 P- int getErr()8 D5 E# W0 ?6 E- l
- { ( q! J: O k0 n* k, K6 n9 R
- sData = linefollower.getValue();
! e6 O |; X, ]$ |; y; A9 N1 l - D1 = ~(sData>>0)&1;
5 I0 ?1 X u1 P; y - D2 = ~(sData>>1)&1;
2 O5 ]+ L( ]) L% t - D3 = ~(sData>>2)&1;/ g. n" {$ ]& H% z( j' D9 N
- D4 = ~(sData>>3)&1;
( ?, e4 Z5 j6 Z8 q+ }# L. F4 N/ @$ L - D5 = ~(sData>>4)&1;8 {- a9 N+ i3 @- d* ?1 v
- D6 = ~(sData>>5)&1;9 u& T# @* A9 J/ u. x R) p6 j
- int downD = D1+D2+D3+D4+D5+D6;
0 L7 ]" ?7 W/ ^" p2 z - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);: J8 b4 q" T8 c7 ?
- if(downD == 0)- d% G5 H D$ H# j3 J U
- {: h5 Q: ^7 D; d" y# F
- return 99;" ^) N! [- k5 N! S/ c1 B
- }
* P* [ v( F% d" `8 a6 i& L; S - else
" ]% x' f; a0 p! W - {/ i8 s- ?$ @7 S' G! A R9 d A
- return upD/downD;
, N" d' x+ h' ? - }/ a) S: {0 }6 y6 Y8 a ^# j
- }
& `4 p7 ]1 s" S) [
, U$ |& j3 L3 F* ]- void moto(int Speed_L,int Speed_R)
! W6 n/ e _* x9 B) n - {, N- F, X% h1 m3 t7 I; ^
- Encoder_2.setMotorPwm(Speed_L);2 }; C% g/ S- I$ [3 D2 T/ o; m1 _) f
- Encoder_1.setMotorPwm(-Speed_R);
( s$ E4 X4 S5 E3 ?) l- B - }
複製代碼 $ N! ^3 f/ r! Z5 x. a6 d' a( J; c
MeLineFollowerArray.cpp
% \+ Q& u3 i; b7 Z3 J/ T- #include "MeLineFollowerArray.h"
1 I4 ~8 [ ^4 s9 [* v
! x' r5 @# {/ b4 V. v% C6 }- #ifdef ME_PORT_DEFINED
9 }* d2 D) K) T& e4 T8 n* Q7 F. J - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0); W$ ~. [5 z7 |0 \4 |) r
- {/ o4 E, D' |* y
- " K: @4 X' Q* p; I8 v8 x c, @
- }9 J2 H/ H2 A0 j
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)+ n2 V I7 S) X. t# P
- {
3 [1 i. d% e8 ^5 P0 I+ I; x - _DataPin = mePort[port].s2;
0 w, i0 _2 a" g. C X: f2 ~9 Q - pinMode(_DataPin, OUTPUT);
6 V; U8 b: ?% n- l - digitalWrite(_DataPin, HIGH);
4 p S7 A+ U1 E& ^ - }
3 y4 l+ C) D/ t9 b - #else // ME_PORT_DEFINED
, s- {3 m2 k6 s8 c( T - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
: @1 G* e b; G0 Y - {, I/ Q& R4 b3 B6 k6 ~
- _DataPin = pin;
7 s8 e+ w) A$ E& R* ~4 l( q# w. m% m* L - pinMode(_DataPin, OUTPUT); ( H7 j0 |6 \7 ?2 Y8 x: _5 f& V
- digitalWrite(_DataPin, HIGH);' \7 z6 L+ U# j: G
- }
1 i6 t+ G5 P) L+ a: ~ - #endif // ME_PORT_DEFINED
* D9 T4 s2 m4 f R9 s0 P - 2 w7 [. A3 X2 _4 O* I0 O9 Q; r6 d
% i: b n) i1 i- void MeLineFollowerArray::setpin(uint8_t pin)
: C% \' `# ~) k( p6 s# v7 x - {
1 P; \' J1 l, R5 }% M2 B+ s - _DataPin = pin;
6 L& B# \% T! _% c4 Y9 U$ N r - pinMode(_DataPin, OUTPUT); 4 k3 H- J4 X; R$ ?$ r' O
- digitalWrite(_DataPin, HIGH);
/ B& l/ p$ j; Z' |, N- C
# q0 E, r9 e( G- #ifdef ME_PORT_DEFINED7 ^6 ^4 G. i6 C, k5 [+ Y" P
- s2 = pin;1 i) h8 y3 o% _, Z. }$ [
- #endif. z% c O) ] T
- } X! F: w1 l: Y4 O. D7 y# y: h+ m
: A! `- r7 b& h: Q9 K8 r' V- uint8_t MeLineFollowerArray::getValue()
7 |0 e7 L. h1 T9 g( M$ u/ g - {. m* {7 [2 ^) f- Y( U
- uint32_t LOW_level_read_time;+ T3 n( F, Y/ E" L" t. A6 W
- uint32_t HIGH_level_read_time;# s! W7 h4 c5 Q/ }; K
- uint32_t time_out_flag;% h! l: l6 q1 ^6 v' C
- uint8_t Sensor_Data[3];
) Q; s% T( x% h& z: t1 c - static uint8_t old_data = 0xff;9 M; X- t* B! P! y5 o! A# |
- Y+ `+ F: C! s7 N$ D
- pinMode(_DataPin, OUTPUT);
* z/ k1 [) |' G& Q# D7 K$ O$ l' z - digitalWrite(_DataPin, LOW);/ j# M7 m1 u, a/ f
- delayMicroseconds(980);8 j% i' a& X) c, [6 t4 ~9 }6 v
- digitalWrite(_DataPin, HIGH);
2 F: a- \9 ]) D7 m$ Q - 8 g9 f7 _. ^+ a0 _* ^ p
- pinMode(_DataPin, INPUT_PULLUP);" ], X+ ^& J6 \+ d0 u% Z
- delayMicroseconds(10);3 H3 H! q v* k7 K$ i
- C9 Z! f5 L7 @3 {+ _; P M
- time_out_flag = millis();9 n: B) `- ^; c
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. V# P" x" a* R$ j/ ^, [7 P+ Y - - t- q% E5 _7 v+ z# a9 |* m
- LOW_level_read_time = micros();
u# _" I8 S1 d) V/ }* V0 k - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 Y K5 Y7 f B4 s" O0 j. G - {
1 ^# o2 y( B2 d - return 0xff;
6 `% Q6 D2 O/ U7 W3 b0 c - }
3 ~4 i4 k$ b! g' ]8 s0 m* Y7 e q - 3 N0 l8 ^6 A' z$ h7 p! B) h
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 Z* I! G1 Z! E% T% K" w: s' [ - l9 S: |/ I$ t8 X" Y; g! ]1 y
- HIGH_level_read_time = micros();' C3 a& E" M3 C$ e$ }* t+ Z2 D
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
, O7 q, u0 J7 t# h1 e - - Y7 K' i) t; P+ Z& S
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 z6 c0 [+ N9 A; Q5 I
- {
. P$ I, j) g% c; B7 H1 c - return 0xff;: ?: W4 \& _6 e" r/ w2 p6 X
- }
# u, |- B# l. \% o
8 `3 A2 q& m$ a' Z: i3 v- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
1 @& A6 U8 s3 k% L - {+ V* p8 y A4 p# t
- return 0xff;5 r2 T' E2 R9 K6 T2 F, m! o, D- T
- }
" W9 z- y) q; t- i w* h
/ ?& N( n% e# I; p; x' c' x$ y- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. ?6 i5 ?; X' F1 B$ B2 U! {- Y - LOW_level_read_time = micros();
$ C3 }, w: ]3 x# S- O' f z9 s$ t - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level6 G; U5 w3 l) F3 p( ?# @
- 9 @: u0 U; X# N+ o: Q9 j. e
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
; h; ?% V# g" V: T* @ - {4 ^) |; ]$ x( L3 V2 Z9 K+ I
- return 0xff;6 G; J; T6 n$ r" u
- }' y1 B3 s+ d9 J9 p/ E* ~" P
- 0 o+ b7 t f, ~, Y. U+ E, R, x
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
; |- v: H2 \$ u6 x: a( ] - {
9 c/ }0 T$ M5 r$ r1 c- L0 \6 l7 g - return 0xff;3 ]6 H; H8 I. _) f# R
- }
& e+ i- Z4 k9 }0 I% R
; I9 w! {. p6 a0 i7 m1 I# r `, `( O- for(uint8_t k=0; k<3; k++)
* y5 o/ }7 J* G/ v$ h5 T - {$ C; k9 M/ m$ M- M* t% C
- Sensor_Data[k] = 0x00;& v+ }# g ]$ \- Y
- * V4 i' F/ ^! h( P( ]: g( c
- for(uint8_t i=0;i<8;i++)8 Q, u, ]3 {' k& W; g
- {, S7 [- c6 k _4 \0 h7 s2 N, B
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 b+ J8 C. ~$ G# j; d
- HIGH_level_read_time = micros();7 P. T+ U! M2 K4 j2 [' B
- LOW_level_read_time = micros() - LOW_level_read_time;
4 d: h7 b D, R - 2 |0 f6 L' ^) T$ Y. Z; X
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): y: R2 a/ X8 l' c; H' H
- {, @1 r2 d! c" z/ E- o v6 Y7 i; I% V
- return 0xff;6 S* |' _2 m5 u' c+ x0 t+ V
- }
( S m4 @; r' ~! z0 o) \ - ; X0 z, a2 ~7 a; x" a0 h$ B7 `- k
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ Q7 V& k* C1 }# m# V9 `" a* ~
- LOW_level_read_time = micros();* ^3 A: o( l7 R' j
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
[) a# M* X" P: e, m - 1 z, m1 I, q" L" B1 I
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1) A% Q, i) F/ k' i+ k
- {
) O1 V2 Z/ U) g9 Y5 a7 ~; @* P - Sensor_Data[k] |= (0x80 >> i);
- ]6 Q4 X/ j/ }9 a1 }/ v/ B3 e - }1 y+ o' c. f, y7 W9 h0 ]- \
- else if(HIGH_level_read_time >= 100)4 o: |& ~; X+ w9 z" v6 D
- {8 u6 y) p% l, k* ^: @0 m6 `4 ~
- return 0xff;8 X- g. @( K% R T2 n( w& y
- }
- C: ~( G4 m! u/ O) r - 4 f* O$ r+ j5 V: O2 t' Y5 z k1 T T' L
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
" d; w+ T3 D2 |& R! t! p/ \' Y3 | - {
9 E `' h) R! g \+ M - return 0xff;
, s S. i3 V+ K/ O! `! B1 L+ t - }# E0 Y& V) K, Y. \; r8 D( w ], V
- }! h3 O& @0 Z: W
- }* P1 F5 N" X: h% Z1 n
- 6 v9 x6 Z" Q& m2 B
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
& g& z6 r& b3 `1 b+ ` - HIGH_level_read_time = micros();" e' }6 l* X. t' t& {
- LOW_level_read_time = micros() - LOW_level_read_time;
, F- ]7 G- B* f7 V( |' ~/ J+ V( h - ; u7 Q" v! I) J. u' s: G% {
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
6 n7 D: n/ ^% T [+ w - {
9 F( O1 r1 x+ ]8 J% \ - return 0xff;
4 ?$ X4 Y& S* D& O; S - }# b) U1 g4 |1 {7 \5 |+ y* ~
- . f$ S- u$ m" c) `6 x& r y
- pinMode(_DataPin, OUTPUT);9 |2 ]% \8 b$ O" M
- digitalWrite(_DataPin, HIGH);. {2 ^* D# p5 X' P( @# s) P, W
- 1 m6 D. n9 v$ Y
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
) i+ v5 G. n) X3 \ - {8 y+ z1 J' }# }+ H0 r5 ~* _
- old_data = Sensor_Data[0];
1 F4 m/ g4 |2 V9 Y( ^ - return Sensor_Data[0];
. |1 R5 @2 u) R- M* G - }' N8 y) p# x# o! l# c5 X
- else
5 N' b/ o/ b4 g- n, i/ c# w7 ~9 G2 z - {1 h2 U, R' p3 s
- return old_data;
! T! c' r! W$ E2 G1 p( |# V - }
. t1 k, j( r; n3 J" U - }
, L$ o) w1 M; @: @5 Q
複製代碼
+ I: m. ~ }/ z4 ^6 _MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) l9 J: g& }' U, a2 D6 W! x
2 d3 w; I, {* Y. k" r0 h
|
|