|
|
: C) y# c/ `4 X' h2 B" P- #include <Arduino.h>
: o/ h" R) [5 U6 T! o4 i0 d0 ?1 S, b - #include <MeAuriga.h>
9 U' d: s8 B O* }9 C - #include "MeLineFollowerArray.h", M) O @3 j# ?8 K
. W, i- E8 S& ?$ e" I& O# Q7 Y- MeEncoderOnBoard Encoder_1(SLOT1);% b0 Z! a; e2 d
- MeEncoderOnBoard Encoder_2(SLOT2);5 a* x9 v4 ]; x8 u6 `6 K* U o
- MeLightSensor lightsensor_1(12);! \" t9 r9 j* e
- MeLightSensor lightsensor_2(11);+ ^* q6 ?4 D; m
- MeBuzzer buzzer;. L; A/ O0 J7 O2 Q
- MeLineFollowerArray linefollower(PORT_6);
' L1 K/ m' e% I; e# q
" S4 u5 u0 m4 W3 }8 ~) C- #define Error1 1
( m5 y, {5 P8 [; E3 ` - #define Error2 2
9 u8 W* v9 K. q - #define Error3 3/ v6 t/ S6 U5 {
- 1 @* D7 l+ H+ K& O
- #define Kp 15
* Q3 J8 p: Q* `; q, Q1 W1 B - #define Ki 0.15
. s Z! P9 R5 `+ u6 _4 o3 Y - #define Kd 0.03
6 V/ `# x* x D% w - 7 R( z% y0 d" f
- uint8_t sData; w3 D% ~6 Y* L3 k% }$ E1 p
- uint8_t D1;
) y, m( z# @5 N+ d T K - uint8_t D2;2 X: L0 P3 Y9 I: g# ]( v0 }3 }3 P
- uint8_t D3;
1 I1 S5 b! D+ u: f6 [8 r; r - uint8_t D4;& Y7 {- Z" Q! P
- uint8_t D5;
+ ^, L7 |7 K7 {/ X. L: U4 e - uint8_t D6;* G5 `+ T2 ?7 H; {1 e
- & U) q9 v3 ?4 m
- float previous_error = 0;
" W2 D: C# q# l& e* B - float integral = 0;
$ ?( l9 V8 A- l' K& u - float derivative = 0;- e+ @9 t9 U' Z# Q
- int Speed = 160;" z- A. o7 e4 u: g/ F
- float output;9 j) M' t% b% d8 N
- * ~8 y n" E. q) T$ S7 J2 L" Q
- byte Left;
/ I1 i% d. V) n5 P, I3 |% w
# d. m' ? N3 @- void setup() " ?# y2 |3 i& i9 Q0 {! V) o
- {
# L- s& O' b' ~7 f3 b' P - //Set PWM 8KHz
& h0 q; K+ c9 f6 X6 e$ D - TCCR1A = _BV(WGM10);, x7 w+ ~+ N4 T0 l" n6 T) ~
- TCCR1B = _BV(CS11) | _BV(WGM12);5 I3 J# w% K6 L1 ~8 }3 g6 e8 T
- TCCR2A = _BV(WGM21) | _BV(WGM20);
; t. w% u- J- C- z4 j1 }8 ` - TCCR2B = _BV(CS21);8 i6 L4 C' u6 ^
- Serial.begin(9600);8 X0 ]5 K& j) y, H$ a
- buzzer.setpin(45);) i# B, v6 E( s3 |# E
- }
5 S8 o% N- O5 [9 x9 O9 M
" R; j1 `! h, V, q+ E% F5 Y5 @- void loop()
4 h0 h( s) _. m: i - {
' A5 ?3 C$ C! Z( ?5 I! o. p - while(!((lightsensor_1.read()) < (10)));0 J1 D) U7 x- d1 T
- buzzer.tone(1047, 500);5 Z. y# d+ n8 n7 r. X! W
- delay(500);4 }7 ~: H4 S" \# P
- do
$ C" N8 k& u' Y; g% P% B - {
; h- P) \1 S) i* D0 l2 | - int Err = getErr();" ?" ^. E- n/ j! O
- if(D1 == 1)- v2 d: s% ]" G& p
- {
- U: R1 t* t; H) Q/ ^+ o - Left = 1;0 c; H0 B0 J/ E/ X6 _' L5 }
- }$ S/ ~# e$ P; ~& H0 a
- if(D6 == 1)% {% `& j) Y3 Q& S* G+ Y" c' M' R
- {
- O: k4 k; Z) \ - Left = 0;% M* x" Q1 r( W% L$ _! w Z% X- U
- }8 H! m0 z* B+ f
- if(Err == 99)4 w7 e) k% A; C; p" Y$ ^2 m
- {( i& S0 a* W" ?, F; F. E8 _" y$ o
- if(Left == 1)
9 m! U5 D ^" T6 w1 [0 f0 N* K - {7 H$ Z3 X$ d, A4 P, I
- Speed -= 5;$ b! }0 a) W* P9 {9 K) ?4 h( x8 h
- moto(0,Speed);
' W. a9 e4 ? o - do7 [- m1 f$ ]( L# K9 a
- {, g, a8 K) h8 t$ t7 y/ w' |
- Err = getErr();
+ T5 H% m! r9 c" \( B2 T) R - }while((D1+D6) == 0);
* Y T0 ^, ?$ ] v A# _ - }
6 l$ k) j3 z p - else: W7 o1 H. N( U
- {
: w8 H+ k$ u N* `; F - Speed -= 5;
- Y" j* ?3 M9 `% `0 C3 n - moto(Speed,0); / X' {5 |- h" A7 ~. f
- do
2 M9 ]! q4 }" H+ _ I5 W. t - {/ P6 C9 g3 j& Y# y8 Y1 }7 \
- Err = getErr();6 D' r9 g) ?4 W; m
- }while((D1+D6) == 0);0 }3 ]# T7 H& @
- }
) }& G- L0 R7 \* F4 L, [ - }
3 J+ ~3 A; C' b/ k8 U6 e8 [ - else
1 J; x5 G( Y8 R; F- O - {
4 ~6 n& M1 h8 B4 f* D( Y - if((Speed < 160) & (Err < 2)) Speed+=1;
* t, G: _3 }, W5 `( H# ]0 [' } - if((Speed > 100) & (Err > 2)) Speed-=2;
4 ]' b; M2 y4 ?1 ~1 D - integral = integral + Err;
1 k# y0 G7 s6 F1 ]+ B - derivative = Err - previous_error;. ^" g8 n) g: \
- output = Kp*Err + Ki*integral + Kd*derivative;
1 a4 R& {. E/ @/ J' b - moto(int(Speed-output),int(Speed+output));
/ s) R1 f8 U' J. b- l - previous_error = Err;
1 X* p0 g3 P2 {, N( ] - }' m3 g1 s4 n. I9 ^
- }while(!((lightsensor_2.read()) < (10)));5 s7 T; M: Y! v5 {
- moto(0,0);2 x% t. Z! V: F6 ^1 }! s
- delay(500);3 N0 b8 V8 C3 x
- buzzer.tone(262, 500);
/ L( B+ ^( a F/ n - }
6 }- f9 |% t3 j- r9 L& l/ ~ - : T; @% N9 a5 _; k
- int getErr()/ u9 v! @( A7 Y! e
- { * m" B8 ~: k* B0 d5 S, O
- sData = linefollower.getValue();
4 \7 h: z- J9 G# G4 y - D1 = ~(sData>>0)&1;, l" m; K+ m* a' {; K0 p1 `/ ?; Y
- D2 = ~(sData>>1)&1;
# f6 Q7 E$ X( F D: ] - D3 = ~(sData>>2)&1;
( e, h# X. g: Z) R# O9 j8 c - D4 = ~(sData>>3)&1;1 T! L! H2 O; H+ @' g% n8 k
- D5 = ~(sData>>4)&1;0 _& q" X' j$ I
- D6 = ~(sData>>5)&1;
: R6 G% Q( k5 a8 |2 O) b. u - int downD = D1+D2+D3+D4+D5+D6;* h7 L" l8 Q- L9 L7 \4 i
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
8 A) v! @4 B( ]3 y; j - if(downD == 0)
, J% ?5 |* s: q - {) t& e4 N$ h' {/ t* u- `
- return 99;
/ {& ?9 z/ Q+ n3 B8 t9 n3 D' X - }
) T& ]/ i# n+ E2 _ - else
( o9 w7 ?( }; Y: a- e - {. R& J. A5 \0 E6 b/ d
- return upD/downD;& C) C5 O2 L, |* ~$ s' u
- }
4 M8 b% m; r4 W+ T# R - }' i+ ~7 K6 L1 U/ |( M( v1 }
4 @$ e% ]6 m& ~6 e- T- void moto(int Speed_L,int Speed_R)( c: M7 S. p+ E" {; V8 R. b
- {* O5 H: i& v' d! ]
- Encoder_2.setMotorPwm(Speed_L);5 ~! H- p8 l. T1 U7 J% G/ \
- Encoder_1.setMotorPwm(-Speed_R);
7 N7 b( B+ q# b1 @7 c - }
複製代碼 : I% R) j- s& o, X8 |5 Y
MeLineFollowerArray.cpp$ ~* U1 U( L; n
- #include "MeLineFollowerArray.h"2 C1 ~$ D/ m9 U B8 I
- # R8 _+ Z- } _
- #ifdef ME_PORT_DEFINED
6 [1 t+ w& G, B& c - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
5 X: I$ l( ]6 I( ?. H5 `$ p - {
5 D4 ~/ t& g5 j1 }( B, l8 }7 W+ \ - ! F1 Z1 ` Q0 I
- }
H( O$ _, L9 v {5 v - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)7 f, f: A- b, A8 O$ A+ |( ~3 O
- {0 m2 {& p0 b. Q/ {
- _DataPin = mePort[port].s2;
2 X- l$ H9 ~+ d& K' q - pinMode(_DataPin, OUTPUT); * O) U$ d# e9 \
- digitalWrite(_DataPin, HIGH);; s! ~- K: k0 Y! ^
- }- k/ `3 m+ T1 v( E( w+ I X. h
- #else // ME_PORT_DEFINED* {1 x/ ^4 C2 [1 I( m% `! Y3 x1 ~
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)$ T& x( U/ H: a1 p0 Z) r6 z
- {
+ v# F2 x5 ^3 Y. j; T$ }' T - _DataPin = pin;
: ^$ b }6 Z) w; N - pinMode(_DataPin, OUTPUT); 1 V8 T a9 [2 n' A
- digitalWrite(_DataPin, HIGH);
8 S4 d2 k) X% ]7 m - }
5 T" o/ V+ v3 r7 W L7 v. z% z - #endif // ME_PORT_DEFINED
6 @8 Z9 v$ E* j& ^
$ x* i; }7 Y/ D9 U, k6 D
* S r% N$ E" v7 W. B- void MeLineFollowerArray::setpin(uint8_t pin)
) O6 E" A! R% h$ y7 R* v/ a - {
7 T8 q# e7 C1 a! b4 l) ]8 x - _DataPin = pin;- p2 g- P* A2 M7 `# Y0 r6 t7 z
- pinMode(_DataPin, OUTPUT);
! r( z$ s+ a) b1 Z! R. Y - digitalWrite(_DataPin, HIGH);
# v( E9 D/ W+ p9 N- K4 l - $ T2 J8 Z$ [4 j }4 C& P4 q3 }& K
- #ifdef ME_PORT_DEFINED
4 |" O, O, Y1 Q( M, B2 I; K - s2 = pin;& H% }" ?8 d9 A! g# l; m% q4 J" ^4 ?
- #endif5 D; c# |! u" t) @3 {% X) t
- }
- u5 |4 _7 I2 Q- E - - {: Q7 R. \0 D* G0 n3 G
- uint8_t MeLineFollowerArray::getValue()
& T9 Y2 ]3 U Y( L: x+ o% _ - {( O& P/ a |1 }9 E! t, |
- uint32_t LOW_level_read_time;
7 O. l8 b) E) O) z( G# k - uint32_t HIGH_level_read_time;* m+ c9 i- O% Z% g8 n5 Q3 S2 ?
- uint32_t time_out_flag;
$ G; y8 W/ S0 i% E# h - uint8_t Sensor_Data[3];
8 ?9 ?+ v) d8 L0 }7 I - static uint8_t old_data = 0xff;
- X4 e& H& `- d- W+ @
: W) ~1 N j/ n) r- pinMode(_DataPin, OUTPUT);: H+ B: B$ f6 y2 m/ u" _
- digitalWrite(_DataPin, LOW);
3 ]# R9 y. y* E6 q, } - delayMicroseconds(980);" M4 d0 z/ |, I* O( ?9 ]
- digitalWrite(_DataPin, HIGH);1 B5 X, \3 v6 |4 d; O
- & x3 ?; u# S; J+ N5 J- s( ?2 Q9 h. \
- pinMode(_DataPin, INPUT_PULLUP);
2 J6 s0 X- O8 \7 }- I# G9 Q v. `) d - delayMicroseconds(10);
7 S O1 g# `. o: H: k& R) } - 7 _5 m! A: d5 Y
- time_out_flag = millis();0 t# U" k3 [9 a# Q' `9 X) E' p
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. |) G9 A5 E$ d Z: F7 S
6 W9 C+ i7 Z+ J- LOW_level_read_time = micros();6 D5 F0 E+ G& n4 P; R, P& y
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 Y" M. ^! u4 [- S
- {
! V, W1 R; M; } K; k. H) `6 I( l" W: t - return 0xff;
# v- K* M8 v; l8 ]( E; {' | - }/ \: @8 J) J7 e3 _
# A' v. p" B4 N$ K, V* o) D+ w- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) ^# o, w, t+ `+ `% ?! Q: R8 k
- 9 i7 l3 o7 ~7 |
- HIGH_level_read_time = micros();
# [$ J+ V0 k- O) r - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
; J! |, V% H9 }
1 O) }2 V, s0 \& U$ ^- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out B' x5 L; A% H; c- W
- {7 G3 U' h3 U2 c9 Q8 X4 k
- return 0xff;$ v; {2 P2 o8 J. Z- E6 E8 V0 n" _
- }
% Z' d$ n# d) `7 J% C5 A% n3 p+ I - - ]2 v, m8 i3 G! c: p; U5 K
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))$ W3 V( V: g3 p8 I% i$ O' K# H
- {
" g, _+ v5 ~8 A" j* q3 X$ L7 ` - return 0xff;, Z5 Y |0 f6 n
- }
k% S- Z5 p( }" S" N
- m' R- P) m/ y6 \4 V7 K! x, f- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
" s* b- ? M! A, Q0 U) `! K; i, v - LOW_level_read_time = micros();& N9 _& D+ Z4 h- g+ w; u: N
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level* u( m- R ], |6 a
5 {: s0 p8 c0 O- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 i6 o+ M E( a
- {
% a4 {3 z! H4 \- G- B - return 0xff;
5 m" g' A! E" A/ F - }
( n8 f; }+ m+ X1 j) d% ` - # { P+ R: j/ B8 h! `
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))6 G, G- s+ k0 D! s( U$ e9 P6 r
- {1 C0 G O' A9 _/ C3 U5 N0 n! z
- return 0xff;8 m$ @8 B w6 n# s
- }
# x& P6 J5 Y8 T6 o" N9 G5 ]; _ - . K1 [8 A, o$ O/ J
- for(uint8_t k=0; k<3; k++)
# x: F3 ^7 A( r( k/ W( b v) X - {! ?% z! Z N% p9 [. I- v3 p
- Sensor_Data[k] = 0x00;1 p) T5 r2 M& c) n
- 1 y* h/ J0 J7 T
- for(uint8_t i=0;i<8;i++)
" ^- m# w- T0 |9 x. M! `* c1 V - {% W1 m. |# v+ D b0 z
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level3 x. r; v6 @# F) I8 C; j
- HIGH_level_read_time = micros();# A1 Z+ _" k4 B. h9 M4 i9 E& v- e
- LOW_level_read_time = micros() - LOW_level_read_time; U0 Z2 {7 s; X
- 8 O. a6 X4 t: E/ g$ U5 m( U
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )8 Y" j( m" L& ?
- {* v: k. F+ o4 S; e
- return 0xff;
6 K2 n Y# q ^0 i9 u3 E g - } t* I5 G+ s2 m8 {
) m# L; o# [$ d* p& p- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
5 g# }* g, D4 q) N+ v/ U9 p - LOW_level_read_time = micros();
) b2 i* Q6 F( X- k( a4 I: ~ - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level) Y* d6 M0 {/ {
; R" l! p& B) d7 A- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1. V3 a4 B$ L; ~, v, H
- {
' v. F( ^2 S) W/ E. N9 J - Sensor_Data[k] |= (0x80 >> i);+ {9 o9 B9 u: c6 ^, B0 c7 }; D8 v
- } Y9 o- y7 V1 l6 W7 X6 a2 R) v
- else if(HIGH_level_read_time >= 100)
. H. S0 {- ` e, _/ T/ u - {) r. t2 @! x! d2 m2 s6 S" h
- return 0xff;" T0 w6 s! }- W6 _# w t% i7 z
- }
: R, y) q1 |% H9 t z( V( h
" R2 G# `! W# |! }- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)* A5 Y/ J+ U5 F+ C8 }/ o0 F1 y
- {+ ?9 v( U) `) N, r3 r
- return 0xff;
6 }1 l4 x: @! F6 E - }4 W" L7 b( N' i6 G4 x' H. N% r
- }
: V6 f: @7 y, k Z7 ` - }3 n% x6 g6 @. D+ D o# P% u
- , H3 D4 d" {$ N6 v# k
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
. J: Q0 z6 b! z1 _! p: N: G% M - HIGH_level_read_time = micros();* P) h$ b/ t; ~2 e
- LOW_level_read_time = micros() - LOW_level_read_time;
( o4 V P1 ^$ O2 Z$ K6 z$ @4 A - & W5 N7 l5 g6 E9 E. y" M5 t9 P
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) u% y1 [! A. H8 }, d N
- {! c2 O5 U9 D8 G. [4 n# T8 W( |
- return 0xff;/ Y) `: K: g' r7 L8 h
- }* s, ~0 t; `6 S" S( g
- D" ^8 t" [% P4 y, r
- pinMode(_DataPin, OUTPUT);( p. `6 l Q+ }, S( M. G9 J5 `
- digitalWrite(_DataPin, HIGH);' o4 N& H9 G9 [2 ?; ^
- ~: O% F8 \" r5 L9 i6 S- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
1 r) e5 y, L$ [/ G a; m - {, s! d7 a) I, X1 Y+ |
- old_data = Sensor_Data[0];
; a( M7 R0 A/ Z1 w' K - return Sensor_Data[0];
/ {6 d# i$ Y, T3 G! ?% F$ } - }
1 D' R$ \: c5 z+ F" p0 F p - else
- T' }8 u2 @. x- s3 n - {" P7 S- s4 C7 E
- return old_data;% n9 C* V* y7 b8 k3 V
- }
. G) I1 O5 K' B1 U9 o/ {" z - }
- K R2 o5 m& N$ {$ \
複製代碼 ; ^$ _. N" V3 q4 R7 f- o- [/ k
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
3 w% w: n0 ^# U2 i: _' {! ?0 Y
: ?9 @# o' k. m( }" m |
|