|
1 P8 r+ J0 N+ k4 ~0 b0 H/ ^0 s- #include <Arduino.h>$ P* {" i' |$ j* A) Q
- #include <MeAuriga.h>" S) t( j: _4 J8 _" [0 u" j O* D
- #include "MeLineFollowerArray.h"! Y0 {9 i$ H# K# r
- 5 T8 f) }1 H7 ^, g; P
- MeEncoderOnBoard Encoder_1(SLOT1);
, ~+ ?7 }4 Y8 U. ? - MeEncoderOnBoard Encoder_2(SLOT2);
/ E" S, K# q# R% |7 h3 M - MeLightSensor lightsensor_1(12);
% g i" r, t0 p& f* R3 |$ J - MeLightSensor lightsensor_2(11);
G2 x3 T: {5 G$ r( q' b7 { - MeBuzzer buzzer;
7 G, U1 I- ?& T# J+ W, j - MeLineFollowerArray linefollower(PORT_6);
- J$ P, ^$ Z& {6 b, G
3 K; R% C; N9 ?2 K, j- #define Error1 1
$ h) s" v8 r" w/ k* ^ - #define Error2 2
/ _, B/ F6 v" k) ?. |! P - #define Error3 3
3 j% R$ W: v5 z! G
4 p! Y) {. g- l K$ U0 F: Z& p- #define Kp 15
2 }, q$ L2 g4 D - #define Ki 0.15
% N# q$ F/ X0 b2 J0 y - #define Kd 0.03
2 I1 ^: Y! }: C% k) x - % g5 z/ |* a+ v; B
- uint8_t sData;% j: f6 h# b1 T" r3 k$ u
- uint8_t D1;. x1 h) t* \6 K* w0 `* a0 O Z4 R
- uint8_t D2;
% @' {* x9 \8 K# w - uint8_t D3;6 @$ F, X% b8 e% T' |2 c
- uint8_t D4;1 v, \; v# Q% G1 J& l, w, u f, p/ c: U
- uint8_t D5;, w$ s4 I+ G7 s4 u5 \. s! G8 F5 b" H
- uint8_t D6;
* e& Y# I% D, @: o7 A8 N5 f4 ] - # q1 l0 f# K5 \0 i7 X' @5 j
- float previous_error = 0;
7 r. i" L( T3 b4 y* L) m - float integral = 0;
& g( L4 {6 B' E$ k( C - float derivative = 0;* }- M2 L6 D: S) l; W0 O
- int Speed = 160;8 y3 |" J6 S( v) l
- float output;+ d+ Q0 R* {6 n6 h' V! @
" c" L( t" p2 N/ t" N7 |% o- byte Left;
# `: o. C$ n; b# O @ - + z9 L* s8 h- l
- void setup() * j. H! E, K r+ R! E8 ~
- {* I* ~5 g( i! J( |6 D/ i
- //Set PWM 8KHz- k3 q7 J4 S( ~1 e8 F. M; b
- TCCR1A = _BV(WGM10);
% R5 j9 D5 @- X5 _7 _ - TCCR1B = _BV(CS11) | _BV(WGM12);$ f( H3 l$ K) e+ F
- TCCR2A = _BV(WGM21) | _BV(WGM20);
3 D: f8 U n$ Y G8 g - TCCR2B = _BV(CS21);
4 a6 R2 |( z+ {* p2 |. S' \# _ - Serial.begin(9600);9 A+ T8 a: A/ [4 y+ c, d
- buzzer.setpin(45);
" V1 Y2 r% m$ r; k7 ] - }- {/ l b4 K7 Z# k/ P8 t8 t
- 7 z6 c5 L9 t6 e& h. c
- void loop()' G! D( P% E: q
- {7 D; j5 R2 B* d( l7 s; }
- while(!((lightsensor_1.read()) < (10)));
) {$ M0 l7 D' a' u5 ?. i - buzzer.tone(1047, 500);
8 V- X* E* o9 n0 W- c, F7 b - delay(500);+ G8 p! t, L' |
- do) _1 ^6 b9 u/ g% j
- {
4 z q8 b3 Q Q7 R& {2 b - int Err = getErr();5 x9 w% ]* X& d# S. O
- if(D1 == 1)
8 Z9 l0 E( h: ? C/ f3 Z' _+ A: x - {
: Y6 h0 y1 F; k/ W# s( ] - Left = 1;( x7 O( [* f4 `
- }
. g8 @% i% Q. G* h! |0 S - if(D6 == 1)% W% |0 V% x. t. T; @8 J C9 L
- {
1 e" J2 C6 h; | - Left = 0;: k. |3 J5 x) ]4 w8 k
- }
* H# Q( C1 [8 j1 b0 ~ - if(Err == 99)* g+ |5 A: {0 R. O- h! d8 W# D
- {1 ` p) |0 `6 V- ~
- if(Left == 1)3 f' Z( s2 y/ g) H
- {8 r; X$ {1 S8 I5 g: p
- Speed -= 5;
- \5 ?4 s; X4 t1 Y" X5 [2 L# K - moto(0,Speed);
. p$ T$ u0 x" G# |! V7 Q - do
~& @+ i* `" e1 K" ]& E5 I - {% V- Y: h0 K) K2 v9 S! n
- Err = getErr();
2 H4 j; @2 n* H6 Y. @: K6 R) W l - }while((D1+D6) == 0);% X! K6 ]$ `! k
- }
6 e4 n4 Z" Z2 b/ G" y - else& W$ f" ~( {6 N, z- [ J* H
- {! K8 B3 w( p- r- n2 ?+ X* ]1 H
- Speed -= 5;
& O) R4 @- b% e" |% | - moto(Speed,0); 0 ]# k" k6 w0 x
- do1 n* r* A5 i. ] e& f4 o$ n
- {
; v$ F% t+ x5 E& R - Err = getErr();
5 @3 e* S) x# s; i; ^7 U2 j! g - }while((D1+D6) == 0);
- [- ~9 x: c$ f - }# x' f. \8 R* |0 z% u) f7 o' @0 {
- }
; R/ g7 P/ q+ r6 P! n - else$ N( K; J5 E# A9 @( r+ z
- {
, [& P% Q: s1 g8 M. F - if((Speed < 160) & (Err < 2)) Speed+=1; 4 J* d% c9 _: F/ A$ s
- if((Speed > 100) & (Err > 2)) Speed-=2; . _, A4 b! o' o2 l
- integral = integral + Err;7 T$ ?; l0 [9 L
- derivative = Err - previous_error;1 R: X& }! J/ s9 S: v
- output = Kp*Err + Ki*integral + Kd*derivative;
! l; _( H$ v2 @- E - moto(int(Speed-output),int(Speed+output));/ j1 }, M& q }3 E3 V
- previous_error = Err;
6 d$ x. W: F7 \" @' X - }
, w; f: H9 [9 s' a - }while(!((lightsensor_2.read()) < (10)));
* Y! i1 {4 A1 r8 x - moto(0,0);
]5 L; C/ F9 h0 T; p+ T2 D# d; v - delay(500);. x" o2 I9 ~6 a9 C& a2 r
- buzzer.tone(262, 500);
' V) q9 l, [& n9 F& d H) b, Z, [ - }+ t# K0 l' ^+ c: w6 W
) F9 @8 {' d+ N1 r' _4 T- int getErr()
+ X8 u( \ W# V7 ~. j( J - { * x/ [7 e& W3 F! u: x- H, V
- sData = linefollower.getValue();
1 C! m# p( a3 S5 p! e! _: x - D1 = ~(sData>>0)&1;- u( X0 H! Z+ v ?
- D2 = ~(sData>>1)&1;
1 A! x% j, r- [6 F/ |* E. F - D3 = ~(sData>>2)&1;
' T3 ]+ Y5 C/ {3 C5 z" y3 v) f( L - D4 = ~(sData>>3)&1;# A, I$ V' o9 Q4 ?2 ^3 c
- D5 = ~(sData>>4)&1;
+ y; G1 e' d" Z( S& h7 O7 K - D6 = ~(sData>>5)&1;
2 c% p8 u4 V/ v - int downD = D1+D2+D3+D4+D5+D6;0 y* S, K* Q' K% t+ S
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
! q$ H" q- z$ I0 L% j1 m' Q - if(downD == 0)/ d3 n% W4 e: s
- {* \) B0 I- w! ~
- return 99;6 F5 h. i. F& p# \2 X
- }) ~/ w/ m2 n9 b9 I% R
- else4 d, i% q Q" ] L" [1 i2 E
- {8 _) L: ~4 k2 j8 ~. K) ~. N
- return upD/downD;
0 K7 t- e/ V2 R2 O9 W9 E$ _) ? - }
* d- d n5 d$ x/ m) O# l" X2 J - }
' Y! [% x+ h) V m! i ]
- k% G, ?) y( ~% L3 S/ j- void moto(int Speed_L,int Speed_R)
: w7 t" A$ f M8 @* L* C - {% n- j; I2 M$ [7 h( G' X/ N8 j
- Encoder_2.setMotorPwm(Speed_L);5 N6 C9 O% y% w9 y
- Encoder_1.setMotorPwm(-Speed_R);) y8 @! A+ G# U( s: a. A7 z8 g4 h
- }
複製代碼
0 D4 I7 v! J4 _$ h4 \1 N! [MeLineFollowerArray.cpp
$ w; J1 a4 f" O I# [5 ^6 t( q- #include "MeLineFollowerArray.h"7 g- R3 I# e4 j- O, _: }7 m+ ^6 ]
6 r- Q. D4 {4 t- #ifdef ME_PORT_DEFINED& U& Y4 a) X4 h' t) Y# `! r1 P
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)9 W% m- I4 `8 f% j
- {% ?$ |) h6 p: y& H6 `& p1 P
8 I6 e' W7 q* D5 m; O/ s7 R2 |- }
* `/ n( V$ y. r, _; b6 Z+ R$ L - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
* M C3 W" ]+ J - {
2 F, A9 i ~+ F# e& _1 n2 I8 i - _DataPin = mePort[port].s2;0 @" P6 V. R& o
- pinMode(_DataPin, OUTPUT);
, i. L2 ~" ?; |: M1 } - digitalWrite(_DataPin, HIGH);
* `* S' O- y0 C, d0 G& V - }
/ _/ _3 y+ q+ P/ b4 ? - #else // ME_PORT_DEFINED
2 f% x) D* T9 T - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
4 H b0 b' y0 Y/ X. H; s6 A* u - {
6 S i* M' {6 b. Z' N) F( R7 Q; I - _DataPin = pin;
( v2 F6 c7 H$ q4 v* y - pinMode(_DataPin, OUTPUT); 4 h0 v2 O1 I/ u5 s
- digitalWrite(_DataPin, HIGH);: d* q+ o I6 Q+ V+ ^; E# e+ u
- }$ x2 W- m! e" O& Y( I: L/ |# F6 `
- #endif // ME_PORT_DEFINED
: M: x* w w8 f- D
( |% w( P6 J$ u: V- / } i3 s F8 e7 Q
- void MeLineFollowerArray::setpin(uint8_t pin)
- |# o: J. @( x5 S% M - {
# ^# W8 L6 ?5 r W5 d% G U: q - _DataPin = pin;2 w8 a# [9 @& ~+ m% @) \
- pinMode(_DataPin, OUTPUT); 1 a/ @# u4 A% @/ f" D% w# j
- digitalWrite(_DataPin, HIGH);' o& D! m' S# c" Y) \( I- S3 q
8 t9 T) j: C' ?0 v: w8 P- #ifdef ME_PORT_DEFINED; Z u$ Z6 i0 t( k4 J, A: o
- s2 = pin;( T& v! |2 V# _. G6 H
- #endif$ @* j# @8 G% [, y! b1 j9 g, p
- }
0 U7 T, ]+ d; n* { r/ w0 H/ b - / x( |$ \) R8 F( R/ w1 {# g
- uint8_t MeLineFollowerArray::getValue()
" r, j. u1 \7 p( j - {
0 G% V. B" v8 I - uint32_t LOW_level_read_time;8 I0 y2 J; \0 q5 b6 r
- uint32_t HIGH_level_read_time;, L3 E2 w9 F. _: C+ S1 P& z8 ]. L
- uint32_t time_out_flag;5 X* y6 X8 }6 H* P( e. D
- uint8_t Sensor_Data[3];" _, B" L1 A! Q; N! h7 {
- static uint8_t old_data = 0xff;3 k* b, k& z, R+ P5 ^
% b$ }' P, R- W. ]- w0 p- pinMode(_DataPin, OUTPUT);4 ^3 z8 k/ y' u9 i8 D9 |6 b
- digitalWrite(_DataPin, LOW);
6 V7 v. `" T' W0 {6 r( }9 k - delayMicroseconds(980);
% z: N* z- ^: [' Y - digitalWrite(_DataPin, HIGH);
[& ~( Y& ]* |' u# g
0 k9 o$ _- t0 w$ f! u, U# x, k0 {! q5 {" U- pinMode(_DataPin, INPUT_PULLUP);) z# v1 P" q3 C% X2 l$ A3 I2 ?7 `
- delayMicroseconds(10);3 [2 w* r/ o' o
7 o, L7 I; Z5 Z; u5 r( A. T- time_out_flag = millis();
' J o' [' D1 `9 [8 f2 e - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );7 Z# [( a9 x0 u9 w- j9 e
, @: ~! N# q) S8 b6 w- LOW_level_read_time = micros();
! d) ^) ?+ l& d7 [: p2 [ - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out# L; L3 h! V4 w r- I% H! s% y5 f
- {
0 C: ~4 p4 r/ R- w* {5 T1 J$ g - return 0xff;
3 A/ B9 g U8 R' G9 B" y0 g, W - }0 @0 `' U8 p! r4 G
- ( b8 }3 d( w. J0 ^5 i
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; F8 P; `/ a" Y4 u
- . |2 K* u+ M2 v) O% |2 g8 b- x9 y
- HIGH_level_read_time = micros();
- S8 Q4 X3 \+ t - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 E e; F% u# O. A - 5 ]& F2 t3 _ A) Q; `& N" L
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 U# o8 X' k2 H% g8 R0 f
- {( L8 h2 y2 A0 K) T' u0 w5 F
- return 0xff;$ y }, J; A, {
- }
) ]- Y1 _4 i1 m1 E' h7 s5 {. w% z1 p
+ ]! b# Y! ~; v& T+ M* S, ? f) n6 ?- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))* o- w+ M8 t3 e1 |& h# v. V
- {' o4 |2 t3 b5 ^$ u
- return 0xff;( @% m- a3 Y, n2 L' N& [ A
- }
! c, S; M; Y; j7 U- J
* v+ s; u5 p3 Y; W2 k& D. z; q! ]- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
+ Y* l. M! _) x$ a - LOW_level_read_time = micros();
' u) @( D% N4 ]2 w N; ~ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
' `. m) C# ?! a3 r
2 e8 H7 q; J8 p" Y p# ]. {" x- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out: Z5 A/ y/ H2 |8 i/ F
- {) L# U& Q0 ^$ ?
- return 0xff;
7 S5 c( s1 Z9 W# j* Q9 d" k - }/ R2 V/ d3 \* ~6 a. L
- 3 f/ H! t' z# h) R: ] G* \
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55)). r/ S; S' l+ s3 _
- {+ G. l) S% N3 [) t* F
- return 0xff;4 c K, ~, R2 k+ P3 {$ \
- }
; I$ G# ^7 z8 D9 e! `; r - 5 r6 U3 V* Q- |6 M4 p
- for(uint8_t k=0; k<3; k++)
) \0 p; c" P1 ~2 A, ]( k - {' t4 b6 s. t1 k% U' Z1 O: d W7 z
- Sensor_Data[k] = 0x00;5 O# k. N- m8 o' v/ F
) J" l" b. x9 h" x) T- for(uint8_t i=0;i<8;i++)
4 t; v7 w" W5 d/ \( W0 P& } - {7 D3 i: {# v/ K% X E6 e
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level t6 W. p0 r' Y- Q4 ]
- HIGH_level_read_time = micros();6 q, o" M3 @- ]3 \: M E! m: f
- LOW_level_read_time = micros() - LOW_level_read_time;
, j9 z- x/ A; J4 J0 m1 ]( T' p
, i z8 K; y8 v3 T3 {( R' c( `- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
" @. {: W. m. ?& F9 a5 i - {/ P- `* K& x2 e
- return 0xff;: g- ], I) r$ M8 R/ f# c% q) J
- }2 w( y9 I( j% v# P. o' K
- : O2 a: J0 l9 }; Y! ?; O! o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );* j% g' ~- O% R! {
- LOW_level_read_time = micros();
: ^, S' Z, e& Y/ C - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level. |' _2 P1 P) a I1 ]
- P& _' l! k) O1 g7 Y6 y. [& m- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1. X$ v+ a7 A6 Y+ J% ]: S* _: h4 [
- {( Q1 f% y& D8 z3 M6 P K1 s
- Sensor_Data[k] |= (0x80 >> i);
1 o9 U5 |1 }( A" v+ @ - } s& I+ Z0 h$ b$ k
- else if(HIGH_level_read_time >= 100)
5 \% b7 k8 H7 F' ^1 n+ |6 a P - {
# Z/ ?0 ~6 G! ?( m+ j* Q! P A - return 0xff;
b7 t7 M# [- h. j3 o - }
4 ?3 x# M' S. Z5 j4 M2 `8 e. C+ i6 I I- w - 5 G& J! u, g4 {2 g& F4 N9 i3 ^
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
2 T+ p) g( b# `5 l% b - {
" P5 B* c( R; |9 Z3 m! A - return 0xff;
) @" @* t' |8 s) W; s - }8 c3 n& C. F- b
- }1 ]8 L( b6 x% p. g! c: z6 D7 B" U: s5 s
- }/ V1 |2 k' C5 G; }3 F0 Q8 l
- V$ q/ h$ _* U$ A' l- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level' p/ @+ ^0 e# P
- HIGH_level_read_time = micros();
0 j) \& l4 u5 A3 B w! @) U - LOW_level_read_time = micros() - LOW_level_read_time;/ E& l/ G' D) a6 j- H
- " k% r4 _5 L) ]7 `; o
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
; l# Y0 Z( Q4 p - {
0 E$ h) M6 I) F( g) h# q - return 0xff;0 B+ ~; C, f/ ?" \% _: G: h# Y# K
- }/ Q8 v1 w" w, W o, ?" S; x, }# N u* ~% H
- 2 d2 C+ P6 W: O) c9 A# t
- pinMode(_DataPin, OUTPUT);
/ |* |4 n3 Y# V; b' s8 M! ? - digitalWrite(_DataPin, HIGH);
- T2 E+ J$ k) D0 @ h$ ` - ! w2 h0 C6 A* l
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
' k' J1 f- K4 n4 ^* f, b' A6 _, V7 ] - {' R2 N0 P9 R8 ^$ |7 q' `
- old_data = Sensor_Data[0];
1 Q( E6 N9 e* c' Z1 ?4 ~5 N6 @ - return Sensor_Data[0];, [) U. {, g( m% b$ ]
- }
. m9 F! G- o8 h - else* Z8 ]8 C3 J6 k6 |; V3 M( B
- {! l" f, O1 {4 r) q: f1 X( `/ b/ I
- return old_data;
4 P/ \4 E# e. \; |# S$ |- X - }: B! o) m2 z% G9 s* ~+ {
- }+ Q: p0 w/ c; [' D
複製代碼
1 F6 `& {0 m, ^" v3 ^MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
/ `, _1 g& p9 J9 O) V
4 r6 X! u$ ~% d7 u |
|