|
|
: J* [$ R6 j, |; Q+ O- #include <Arduino.h>
2 B8 o" `/ c+ Z1 X0 e! b - #include <MeAuriga.h>% W- \- {9 [* @/ ^
- #include "MeLineFollowerArray.h"
; s9 S" I% a1 L% r* P
4 K5 D* C! I; D( b- S G3 f- MeEncoderOnBoard Encoder_1(SLOT1);
k! l4 V! Y4 o, d) B; g - MeEncoderOnBoard Encoder_2(SLOT2);
) T8 U+ c$ u4 k( Q$ s1 | - MeLightSensor lightsensor_1(12);% N) N$ a' h7 v, {. y4 g
- MeLightSensor lightsensor_2(11);/ u) S- q; A/ x- }/ }
- MeBuzzer buzzer;
' H" A4 t! t" a' u2 @, ]. @ - MeLineFollowerArray linefollower(PORT_6);3 u# w+ E$ ^9 p4 Q7 A# Q# K
, W, B; m, E" h5 K: X$ N- #define Error1 1
( Y" b4 _! ^7 `* ^9 |# X - #define Error2 21 L; X2 n7 l# T. S( A6 P4 G
- #define Error3 3
3 \2 c2 m- \: ]: c, n
* R6 {4 w0 x4 U0 x6 q) Z- Y7 D- #define Kp 15
$ ]/ k- M6 x! D) P7 c - #define Ki 0.15# ]! Q4 o$ E. x* g
- #define Kd 0.03
( z4 `5 E( l' @
% M! l D2 k' r1 v, M8 r; E* F. f1 H2 a- uint8_t sData;) ]5 F* r9 a2 k+ |& d
- uint8_t D1;
" M& C9 G# W" O7 N% P - uint8_t D2;
5 v( d' Y6 O; M7 B- t - uint8_t D3;! g: I! }2 [" D p) W- A* g
- uint8_t D4;
4 y0 _ g! u# G! W; x - uint8_t D5;( \6 I- |) X; _; y7 q& }
- uint8_t D6;
* \5 [& E6 M" d- a- v+ l
5 K" Y7 Z) Z* o3 c, ~/ ^- float previous_error = 0;
3 C8 V( U) X) m1 ^7 {7 p2 e - float integral = 0;
: k! U7 \# s1 L1 x - float derivative = 0;
* J+ J: J9 s4 c5 |9 K$ o - int Speed = 160;* Z' [% ] g9 a' \7 ]7 v
- float output;- S1 R6 |! Q( R! m* a2 p! Y' \
- . C1 D' f- Z1 ^' I6 q
- byte Left;) J9 q" f) @& m* i; T) d
/ i% ^6 m8 h2 `. S6 _7 o' f" d- void setup() ; o3 y& B! O% z7 h: Z& W& J
- {# B0 C0 f- X6 M
- //Set PWM 8KHz
* ~' y+ z$ S- O$ F4 P2 s - TCCR1A = _BV(WGM10);
: J! E3 j, A1 y7 O3 X' O - TCCR1B = _BV(CS11) | _BV(WGM12);. h2 W! }' N; D7 S! y
- TCCR2A = _BV(WGM21) | _BV(WGM20);2 L X; w$ x: ~, _& d+ @0 P
- TCCR2B = _BV(CS21);
% r2 h; b, y6 { - Serial.begin(9600);+ B! V& S+ K$ Q8 H/ \8 Y0 V
- buzzer.setpin(45);
8 v( e8 n: s5 k; h4 f. J I - }" x/ _* h1 [6 n8 g
) H. j- ^7 c1 P+ u6 G- a: F; S- void loop()9 ^/ s0 |& |* |- f
- {
1 r# ?+ Z: `& Q# Q8 W( E - while(!((lightsensor_1.read()) < (10)));
3 W4 ]3 Z% T1 R; ?4 O - buzzer.tone(1047, 500);% ~: O; @) d) n' f$ V0 j
- delay(500);" g7 u$ K6 K: E/ [! l
- do. f) x7 t9 w8 D* g
- {5 l- S2 |/ b8 U( T; h
- int Err = getErr();
H5 |' k! g8 k: ?% [ - if(D1 == 1)7 N& U* T* r/ U& v/ x
- {
& M) P2 | y; }, t3 I$ U - Left = 1;% ]2 C9 Q5 d1 i% R" b1 r
- }' Z% r; U. g5 G& G
- if(D6 == 1)
0 w# P' E& N6 A8 s& W. L - {
* y& I9 B3 e. C! \9 F+ S - Left = 0;9 b3 l. D) D3 ?3 ?
- }
& {/ A! Z! G+ { ~ - if(Err == 99)
2 D& B1 j$ {+ S3 n L8 N/ _ - {; L' J0 `9 ?% m" Y" G1 V
- if(Left == 1)
. w' k H% p" D+ ? - {
$ o5 K; w% O* s _8 N7 ? - Speed -= 5;
" i8 S% P. z2 `5 A, A2 O; u - moto(0,Speed);: L, c1 Q+ J% t
- do
^, E; [" F2 K9 j - {' u: {1 Z4 A% i+ ]1 m! J, K: J+ b
- Err = getErr();; P, a" `* \" e/ H% I: A# Q! N( ~% D
- }while((D1+D6) == 0);
+ x( F" I* H" g/ C3 q - }
1 ^" d$ ? e0 M2 h - else1 `" J1 Z1 n8 s$ w6 k9 Q
- {
& W. t- K/ f& a' U' Q - Speed -= 5;
4 ?/ k, f% q% Y: M - moto(Speed,0); + j+ @2 I/ _7 x) u, ?/ D% z c% v2 D
- do
2 b4 P9 c* k8 x2 U- i: ^1 M - {
+ W( t& E- v- T - Err = getErr();
2 X3 w+ U$ B# [# B! Z& Z' O N/ H - }while((D1+D6) == 0);' f/ ^9 s6 R9 G- P% }$ K
- }
6 l Q3 T5 v$ u; `* g- T - }, l9 g5 D; r! ]) q6 f; X/ x; L) A( p
- else
! ?2 ~# h* l2 a8 U, l4 a! r - {
: G: _) ?, N8 E+ j - if((Speed < 160) & (Err < 2)) Speed+=1; 4 ~+ W9 R$ y, v% w- D
- if((Speed > 100) & (Err > 2)) Speed-=2; 8 v O- q/ {! w: d+ ~+ k7 c# v
- integral = integral + Err;* _2 N9 I$ |" u% q
- derivative = Err - previous_error;4 ~& c7 @5 w% t
- output = Kp*Err + Ki*integral + Kd*derivative;, x* C1 h. j8 O$ _% C
- moto(int(Speed-output),int(Speed+output));
& N' h# M/ S# h/ M: o, g - previous_error = Err;
7 H1 [% U2 Y( j2 d( @ K - }
8 A7 t* e3 d Q8 j- R: y9 E - }while(!((lightsensor_2.read()) < (10)));
0 t2 F# R+ j+ H' p" u; l - moto(0,0);
, i P) N$ g8 Y. W$ o, [ - delay(500);$ {5 n6 C6 _" Q& y5 r4 j
- buzzer.tone(262, 500);
. D5 O4 ~% ^& @' N) P2 B - }& k0 I2 @9 }/ f q9 g
- ! K3 A3 `( N2 ^8 T
- int getErr()3 f6 U5 ?; b/ y3 u. M
- { ; [! x w' ]- G( F) f
- sData = linefollower.getValue();
& F! q, o# V6 ]% h: ~ - D1 = ~(sData>>0)&1;' N% G& B) {+ P4 D/ m/ F0 r
- D2 = ~(sData>>1)&1;9 M' x7 p' S) p, V
- D3 = ~(sData>>2)&1;
; q' @% h L; B) X5 Z( Y) H1 b0 P - D4 = ~(sData>>3)&1;
4 H3 I% E) T1 L, M. P) Z - D5 = ~(sData>>4)&1;
6 E: h) p O0 J7 N; A% ~ - D6 = ~(sData>>5)&1;
: V0 m, M: @! E7 O% V8 M - int downD = D1+D2+D3+D4+D5+D6;
9 e* V/ a7 R3 H& ?9 F1 t: L2 A - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
. J& ^0 ]2 J2 @, z/ Y6 `( d - if(downD == 0)
4 Y! p3 A5 }3 a, t' P - {
) T2 K2 S$ P( S4 K6 J8 k- i: D - return 99;2 k, \. A+ i0 I5 n' n- ~
- }' x7 b H# `5 R$ E
- else" S. b/ N) Z1 n+ \7 |) X v. L
- {
+ C; S7 I, ^! H% L - return upD/downD;
/ o4 N; n* r7 ? - }3 n* s* C/ h" v, Y7 s, R
- }
; t" Q' d5 M7 G: @5 H0 S8 M7 _ - , W/ t; q8 g; B' ~3 ~3 k
- void moto(int Speed_L,int Speed_R)- N" v6 e% ^) b- n+ R& |& d3 Q ~
- {
" w$ d9 q H& [7 Z3 i - Encoder_2.setMotorPwm(Speed_L);
0 x9 Y4 G' o* V$ r: z! Z4 G - Encoder_1.setMotorPwm(-Speed_R);/ j. S( f( _3 \3 Y5 H5 |& e# `
- }
複製代碼 ! m2 m0 E9 e5 ] I& T
MeLineFollowerArray.cpp; f* `5 a4 C1 C8 g& y9 F$ N( s
- #include "MeLineFollowerArray.h"
- `6 z- X& @8 E - ) s' d8 z" V+ N: G7 x$ p Y
- #ifdef ME_PORT_DEFINED( c# C! K' Q) O) j4 L
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0). c8 z m+ X# P6 t O$ F: g A
- {
3 n# f2 [! B2 |# k6 T3 v/ I" w - 9 \5 ?! Q! {# d
- }, B' H, M. R( m
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
; z# ~0 b& m# O ~- u - {8 _. K# P0 D2 R/ s" \0 ^2 U
- _DataPin = mePort[port].s2;
" e7 u6 j6 i/ O7 o( c/ G! ] - pinMode(_DataPin, OUTPUT); 3 I/ A) C% G9 O1 d/ D8 s; P) p
- digitalWrite(_DataPin, HIGH);
- }# A. W; q, i% H9 O, ?( O - }. d* J4 [3 D1 z) z, m
- #else // ME_PORT_DEFINED# E3 }* ]; C/ x7 ?
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
p* _' g2 v" F h - {
4 |2 M* E! |. |# [" Q! ~" V+ q - _DataPin = pin;" S; Q$ O4 W/ E7 n5 a7 e& e& D
- pinMode(_DataPin, OUTPUT); * c/ s, q& n. g7 a9 c3 J* G3 c
- digitalWrite(_DataPin, HIGH);2 x; K {9 Z j7 B" b8 l* ~0 ^
- }
- u9 u; y3 {: j! T' @5 \; u1 W4 A - #endif // ME_PORT_DEFINED
8 ]4 }5 Q0 l$ w" w' N& B- }
h8 q) n/ E) a, J) w
, a2 ]0 X1 V# Q9 V$ k- void MeLineFollowerArray::setpin(uint8_t pin)
& p- W0 t" P6 l" | a5 U - {
. z/ B( d7 j: x - _DataPin = pin;
' o/ C( d) M0 L# W. x: v ?: ~: d - pinMode(_DataPin, OUTPUT); 2 W% G1 P) F4 ]; q
- digitalWrite(_DataPin, HIGH);
- Q. F* r) w0 O' I- Y6 [3 N - " A" W/ p7 I/ P7 n& N6 S
- #ifdef ME_PORT_DEFINED4 @ d8 \. e/ O- q( r
- s2 = pin;3 R9 n6 O8 d2 f, M
- #endif* P6 ^1 T' k7 F9 Y
- }% V7 f4 u. u; u! w% A% {; L& S5 S Q' N
- / d* ^2 R/ K5 Z3 j4 P4 _- N. e
- uint8_t MeLineFollowerArray::getValue()
, L1 R7 a6 g8 v7 p8 G1 @ - {& s. ^; R6 M! s$ o A9 F" w% }
- uint32_t LOW_level_read_time;
+ @3 U" U" z" ]3 C. ]5 ? - uint32_t HIGH_level_read_time;# E8 y& M8 S/ P& Z @# }4 v" j
- uint32_t time_out_flag;
* I1 z$ A$ K! \% `2 g6 W0 ^$ L - uint8_t Sensor_Data[3];& x _% L+ c1 T, a1 |
- static uint8_t old_data = 0xff;
" y3 k" T( c1 E% L3 W; X( a6 [) {5 L! \$ }1 F - 9 z) P7 P! T! _# q3 Q! P8 ^
- pinMode(_DataPin, OUTPUT);9 o) a) o+ c( U9 ?& G
- digitalWrite(_DataPin, LOW);3 U" n( s" T2 V( ]: Q
- delayMicroseconds(980);) e& r. x8 R, z8 }7 n4 V
- digitalWrite(_DataPin, HIGH);
- L% X5 f9 w5 o2 N9 ?. U# w8 J/ H
, Z: X+ H8 O) p/ X* K; P- pinMode(_DataPin, INPUT_PULLUP);
$ R% E1 O2 u9 l- V- j# F. m/ h - delayMicroseconds(10);2 \, a2 y' k6 c4 |
- 1 d9 a0 A2 }: z3 H/ i x
- time_out_flag = millis();1 }' g2 t& `' N, Q) ]7 X
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 C8 |; [" i& a9 r# y' J - + E/ @3 b* n' A' R0 ^6 S
- LOW_level_read_time = micros();
1 u" J% r# P5 g) z; C5 ^ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ H+ v7 y: ?* H% Y6 G - {
0 Q( a7 N( e; @5 Y$ [ - return 0xff;
3 V$ v+ G' R. Y1 b4 W9 i4 q - }
9 m, p1 S( j6 h4 q8 [$ q - ) ?1 |+ K/ t/ T2 D5 g5 B
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. i/ q# e2 ^4 J( X
- 1 G# b9 v) M' ~$ r9 }
- HIGH_level_read_time = micros();, p# s; L+ t: M
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level3 ^+ D) T$ x2 |' \" o$ P
- 6 w9 g( J0 J% K$ X& q6 e2 x. o
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 y/ M) b8 F$ z$ x, z
- {
9 _$ k% m/ G2 f% _' |5 J8 Z - return 0xff;
0 d" z+ U7 h) M D4 ~6 H a4 W# v - }/ P+ U2 e5 l {
- # r$ W k- V# A$ [
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
+ ~) Y* C& d) W1 l - {
" ?; J7 X$ ]! u( w) P - return 0xff;
$ ^- x4 V% d1 @1 o" S2 Y - }8 y: [ R0 }4 f! C2 f
! [0 G8 ?5 j6 f! `8 j- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- P- s# ?8 k6 K J! L - LOW_level_read_time = micros();* u7 X( F: {# A2 o( r
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level4 h& N6 v3 m% R$ B+ z
n& R6 x/ V$ O$ J3 R0 L, x- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
' @2 s4 P ^. |' { - {0 g" f! B% a% C0 X/ P! f
- return 0xff;0 P# f5 Y3 W, D3 |7 ]
- }
, L) n5 D# c7 `6 K* K1 a - * N. C6 `; ^+ S/ {7 y
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
l1 A" t9 q* N, R, H! ] - {
1 ~% D5 ?2 k9 b/ l& j e8 e2 e/ d - return 0xff;6 ^( i3 f: g7 f. s+ x8 b
- }
$ N3 f. R! ?& z G3 N
# X3 G) f: g! j, I& H% _! k- for(uint8_t k=0; k<3; k++)% N; s8 p( e& C/ R- {8 _
- {- O3 \) s c0 g" S& d
- Sensor_Data[k] = 0x00;& U& r0 r& |9 }6 F; U+ J) e# e
- ' k# S1 d8 ]) Y7 T1 Z
- for(uint8_t i=0;i<8;i++). [* j! [$ T( ?; H+ @ l0 M
- {5 T! f4 d; ?: J$ r5 A9 C
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
. l& H1 M% J% M m) M5 r" O# h+ ? - HIGH_level_read_time = micros();9 o( n4 s9 y$ G
- LOW_level_read_time = micros() - LOW_level_read_time;
. Z5 J" S& _6 L0 Z' x9 F6 [
8 U' A# G% ?& Y! K( |, @- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )* t! L! ]7 E' @$ K0 b
- {
* L. _2 _+ P0 q/ z3 _ - return 0xff;2 C' ]+ F2 b* d7 x% g
- }
# n: U. F8 D0 v+ ? - " E/ B& y7 `/ C
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" z7 M# }# w1 m1 F$ B - LOW_level_read_time = micros();1 `! a2 {) n# S- k" D; e
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level" p! _: @& N$ i2 R6 k
& w B5 y& K3 T z* [3 Z n- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 |( w( e {4 s# O: D3 {0 h$ T( b
- {% W9 L( z5 E$ F" `7 `& ^
- Sensor_Data[k] |= (0x80 >> i);
9 a% ?6 U1 \6 H$ G0 \! L - }- R9 P( W5 W' \. S3 Z7 r+ Z
- else if(HIGH_level_read_time >= 100)
f2 O4 o% O H0 s: v - {. |" ^: a8 u$ ]! H0 g$ U- g. i' Q h' ]
- return 0xff;
$ G6 f; \' K; z& X) L% W/ O - }
8 J- u& m/ C+ _3 O! Z - ' l9 D8 z! p: I7 c
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)! I! R6 R9 ~9 N; E2 x
- {
4 @2 j# @9 H+ e! l - return 0xff;
9 ^9 O4 W# F8 m. q8 p, \ - }" w k( o5 G; h, f* f' R$ y
- }
% }8 W( J) C9 o2 k: L$ @, l/ u - }' O; m3 }/ D5 `# ]9 e: M5 S6 o8 q
- - n6 C6 Y, @# M. @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level' c/ e! P( R# g1 r- y
- HIGH_level_read_time = micros();
; | [. |% I5 y3 H8 w2 @ - LOW_level_read_time = micros() - LOW_level_read_time;
# @5 ^' O7 Q1 M; ~& [3 R
7 K. W) F: N3 k- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )6 k; n2 Q( y) G( @2 G5 g* m
- {
9 c+ v" y- a9 {2 Z4 S0 Q - return 0xff;
! d: e( A+ U, \1 ? - }
8 p% L3 b0 J5 E( X& u; z - . p1 ~" R l6 n) X. x. t
- pinMode(_DataPin, OUTPUT);
A( }4 r' v. W - digitalWrite(_DataPin, HIGH); p2 X+ b' W2 M/ J) m2 C# e. S
! `& {+ { O D8 V2 O- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
2 ?# u% p9 |' |1 |: @7 C: J - {8 v6 v7 Q1 K/ }
- old_data = Sensor_Data[0];2 ^$ W3 I; b& L
- return Sensor_Data[0];7 H8 S6 D* B8 R
- }
4 y0 b; a" q0 I( u' o4 ^/ r - else
3 I9 \3 @. Y1 H: S- w' a - {
; s+ }5 u" }& s - return old_data;
9 I4 d7 ]; z6 d& ]+ h - }" V. K: L3 K, B3 p: {7 B% W
- }
! N3 t( X* p( @! w
複製代碼
5 N3 u1 S# ^/ oMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
- f% H$ T/ C0 d1 Y' U1 V
9 X) p! g: K4 P$ y+ s' n; D$ @ |
|