|
|
3 y* N8 n3 S% W1 Q# Q L- #include <Arduino.h>+ m3 @$ X2 y6 ^
- #include <MeAuriga.h>
3 }0 Q& ?; X# `, b; l$ w3 B+ X0 [- l - #include "MeLineFollowerArray.h"9 G6 B" t$ t: `
- # A' p' k; @" o( |8 v0 f
- MeEncoderOnBoard Encoder_1(SLOT1);9 B ~! O9 W/ F2 r$ }- \
- MeEncoderOnBoard Encoder_2(SLOT2);
9 e! z V" ^9 }! z1 |/ ~7 m - MeLightSensor lightsensor_1(12);
7 p5 U6 p* U# s% Z, T9 h k - MeLightSensor lightsensor_2(11);! `( k2 w1 o6 R
- MeBuzzer buzzer;" O* c0 z! h& q) j$ q9 G6 {
- MeLineFollowerArray linefollower(PORT_6);1 Y9 T7 T7 Z& X4 j5 y' A
- 8 F9 p; @4 U5 p& i: I
- #define Error1 1 N) h7 r; A5 |2 s8 d
- #define Error2 2
: W) F' i* d" g& @" {0 {1 L7 ~ - #define Error3 3: j3 F8 f7 L6 O2 _( M! q, Z
- " g$ ]& A v0 ^& G
- #define Kp 15
# j' ]: G" v4 c - #define Ki 0.153 x) I1 u5 c; v# D7 W" F
- #define Kd 0.03
. m2 Z" ~/ a' Y; c6 r4 q& ?
1 j J; `# }+ X- uint8_t sData;, D( M' s( {1 m6 v) {7 f: K9 Y" _
- uint8_t D1;! F: _# T/ ]0 ]2 \
- uint8_t D2;
+ y& j1 e9 A4 ~, a1 j8 k$ [; s+ @8 P - uint8_t D3;( |$ ?3 K7 M5 {
- uint8_t D4;7 e8 ?9 d/ K" J1 |
- uint8_t D5;
; R+ G( f7 i8 E& C - uint8_t D6;9 G2 ^" J1 R: `1 d* g
- # ^% z: W# O5 a. r; n& p
- float previous_error = 0;
0 y; ~2 v8 `$ P! Y) A: w - float integral = 0;
9 O' ~, E k# i - float derivative = 0;
( g" P$ u6 \ ^4 s - int Speed = 160;
! O' R2 c- }) @: ]. c/ @6 E9 ? - float output;3 `2 P; U& H' S- Y7 k/ p
- 4 c+ u8 D8 i# F
- byte Left;0 b" N R0 c$ p4 L4 Z9 ?
- , {9 @8 y# q# I+ [: y8 p
- void setup()
+ t) M' S6 F* f2 L l) H - {
W+ L# l5 i3 a7 S( G/ f3 v - //Set PWM 8KHz: a* ^$ X. e! w% t
- TCCR1A = _BV(WGM10);! R$ G% Q+ D2 P) E4 R# F
- TCCR1B = _BV(CS11) | _BV(WGM12); Q, V. p; N: }' I; T; K/ s
- TCCR2A = _BV(WGM21) | _BV(WGM20);& T" Z8 X, x) g5 \" P- p
- TCCR2B = _BV(CS21);4 H3 Q) y+ _% t. [0 j, j# j7 X
- Serial.begin(9600);
% ?% Q; |: A7 n1 L - buzzer.setpin(45);
5 G# H* p) r7 o* O2 R7 I - }* t& r. \" o B* C) D
: V7 L' d! G0 E* u) [- void loop()
) V( E6 M6 B3 @5 B0 H - {
1 I; P3 J. T# z9 s: y, M* g - while(!((lightsensor_1.read()) < (10)));( B7 b' r& |# J7 d" r) w' {
- buzzer.tone(1047, 500);
& @4 ~3 @7 k8 T8 V3 O; ]" C - delay(500);
, y/ H# E- O" W" l! c2 B - do
3 F' J! f9 j1 C5 w8 ?% T' R0 [ - {
& n5 g. Z9 V, Q2 F - int Err = getErr();
4 V; i. _* T; w% c% x - if(D1 == 1)- Y+ C6 k5 u+ s& t
- {5 ~- ~' k9 a! D: d3 I
- Left = 1;/ ^" k+ `/ a! r9 x- o/ e$ F
- }
# {) D8 {/ @5 t! o' ~% H2 I9 N - if(D6 == 1)
+ V. t2 l2 w& T1 ~$ o8 G' D: f - {6 s/ d+ f; X0 i( L6 l4 A! e
- Left = 0;. p v4 t% U% J0 n( R% M) k" a
- }
. P7 V) A% C, m" Q1 [' P - if(Err == 99)& S7 n$ D0 o+ e6 b/ U/ ^
- {4 O6 I0 }& {! X: n
- if(Left == 1)' w( v# ^/ P7 ~" E3 A) w
- {) V6 F2 @) Y' b
- Speed -= 5;
" R2 Z9 w+ x& m( v% c - moto(0,Speed);
" z0 R' u/ m6 B5 V$ ~ - do. \3 I! j" C, j
- {
/ J1 R4 K5 n9 r! W' ?6 D - Err = getErr();
[ F2 t) f! |8 R - }while((D1+D6) == 0);1 |2 g2 Z% l: P# R: a, \; P
- }
1 v3 ?2 U; L1 r+ k, G8 N7 Q& ` - else
3 p4 v. e8 Z9 E - {/ J4 H5 K8 u+ k) ^, F
- Speed -= 5;
& b2 X$ c H) t3 v/ f - moto(Speed,0); . f1 [% _9 z0 h) Q- \, z" F7 j
- do8 \1 b8 `! |" H
- {
4 C. f# H" H9 T+ F0 x! {: Q {9 c( e - Err = getErr();
( @2 `7 k* P8 f; E. X8 ^0 E - }while((D1+D6) == 0);% @0 M% i9 s9 g- Z4 |
- }) k/ v3 o0 W$ w1 T. o* r" j$ L/ F0 O
- }
9 Y) ^- i" M; |2 H" g - else
7 Z1 A W _! M/ P$ w O - {# r- L1 R! @! R+ B! R3 n/ E
- if((Speed < 160) & (Err < 2)) Speed+=1;
1 r5 {5 S1 h" M( t8 o% L - if((Speed > 100) & (Err > 2)) Speed-=2; # t: O5 [0 F6 L0 M
- integral = integral + Err;, t$ g8 H' G- M2 e' i
- derivative = Err - previous_error;; p; F) q. B$ D0 ]8 d: [- M0 t
- output = Kp*Err + Ki*integral + Kd*derivative;
: o# p4 g% g/ a+ D @2 @5 o7 b - moto(int(Speed-output),int(Speed+output));
% o0 C. }5 E S0 t; v' l1 R8 Y$ {: }0 i - previous_error = Err;
' V" [* r+ j- ]5 q" W1 k - }2 ]8 k$ `* A* ~' A$ E# ?
- }while(!((lightsensor_2.read()) < (10)));
2 D. f) k0 n5 ~9 P - moto(0,0);; I; X3 m- O9 @* s: a
- delay(500); z: Q) N* F7 ^" ~- u
- buzzer.tone(262, 500);
- U: J; d( G5 K& g, e - }
( H$ v, t' r) p# R+ [6 J - ( D: U0 W3 q3 G- `5 a
- int getErr()* {- f6 C4 m( d4 g& @
- {
0 R/ N- D& F% [0 m* q" c# R8 x - sData = linefollower.getValue();6 m3 q- ^4 D* U$ e
- D1 = ~(sData>>0)&1;: b1 m. K7 d- H9 j; y/ P
- D2 = ~(sData>>1)&1;
' ~* @& E5 l( q1 K- X+ Q - D3 = ~(sData>>2)&1;
0 I6 k. m- L1 I' i - D4 = ~(sData>>3)&1;
5 d$ G7 B0 O. e) G( x# r1 t8 g - D5 = ~(sData>>4)&1;- D8 ~' R' o) L- g
- D6 = ~(sData>>5)&1;, w( E( e9 j% s; D& N; _: Z
- int downD = D1+D2+D3+D4+D5+D6;
; e* _% ^" X5 E8 n - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
. z6 s8 e, t1 \4 N2 R* z - if(downD == 0) x$ t% q$ X! X! k
- {* a5 r$ t! U+ M9 [
- return 99;; W7 S" ^ M4 V& j: w# {" o
- }
* \- A0 u1 i0 f* N- V( L - else
" K" U6 c" F \. ?* U8 T! `3 R - {- Z% _& y0 L- e; \% j
- return upD/downD;
$ W0 ~- l' g3 u" a* |' \! U - }
* ]$ a z6 y" \* p E3 E) S" I$ d- H - }
1 i1 F: z$ O& `0 f0 ^
/ U: a* x) s1 v5 h7 S0 r- void moto(int Speed_L,int Speed_R)4 u3 m( d6 P# l" M0 Y! `9 r
- {0 s( v2 ~8 O8 K5 v. Q
- Encoder_2.setMotorPwm(Speed_L);
5 x* J; Z; x2 \4 I$ i' g4 @. K+ Z - Encoder_1.setMotorPwm(-Speed_R);) l3 |, l9 p+ r& o i
- }
複製代碼 + t7 m) G- y9 p' @: a
MeLineFollowerArray.cpp
/ R0 G. L [' A% S/ W3 Y( p* W- #include "MeLineFollowerArray.h" G3 X' C( K( d% A2 E$ L6 p4 Y) @
! x8 W1 l; M% H. U) }- #ifdef ME_PORT_DEFINED
7 e" n' C( P8 F# N! L, _ - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)( G" _, k' P$ c! L0 S
- {
% U' L9 C+ ~, L: O
& X. a$ X& h6 X3 l5 A/ h- }
; G) z3 j- b* d- @4 V - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)* {" l6 V6 ~, W2 A& H; r0 M
- {# |" d* x3 U* w: K/ Y, `( Z
- _DataPin = mePort[port].s2;
3 z9 O% q- R, h - pinMode(_DataPin, OUTPUT); 9 }7 i, E! V/ U R- O# o4 Z2 L/ H
- digitalWrite(_DataPin, HIGH);5 q( y2 ~+ X1 U* N; E: T7 P
- }
& v- M! J' c4 e9 j1 R. L- v/ s - #else // ME_PORT_DEFINED) m4 z2 W" V9 F- T
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
! _/ O0 `; ]; I) l0 R - {
' t- N0 f' u# _7 F: `. H+ t - _DataPin = pin;6 a; s. a! ?9 O) {' W* U
- pinMode(_DataPin, OUTPUT);
: {9 i& K' H% c8 h5 m% b$ u r- @ - digitalWrite(_DataPin, HIGH);
$ J$ H# X: I! D- C& g" q0 c - }
0 j$ R( }8 [0 R* [2 n - #endif // ME_PORT_DEFINED, Z6 m7 H# h( Z8 {2 S2 `
3 z# P) a* k, X0 L3 s( m9 q
# H4 y) z! S: k* G% q- void MeLineFollowerArray::setpin(uint8_t pin)9 h" q% K; B3 B6 p9 Y
- {3 y' `+ k/ ]+ v" H4 _6 E; ^& d
- _DataPin = pin;
% b: R$ |- T+ x+ v5 Z - pinMode(_DataPin, OUTPUT); 0 L% a+ V+ k2 k t2 p8 x! g/ b
- digitalWrite(_DataPin, HIGH); ]$ _% O5 m1 P" y( t
- , c ]% ^# B% `" a& a
- #ifdef ME_PORT_DEFINED8 D( F' z S8 D2 O
- s2 = pin;6 q5 x5 B7 f3 I% s) u
- #endif" g6 K* y. N. C' O
- }
$ Z8 a0 r$ ?, l/ ^( X - 3 u0 p' D l) o* ~, [
- uint8_t MeLineFollowerArray::getValue()
$ Y" t; s3 t. y7 N6 {9 g; X - {: z, i, D& x9 t. H& }8 f2 ^. V
- uint32_t LOW_level_read_time;
3 F$ p- f) Y) o& e) U. a0 M$ w - uint32_t HIGH_level_read_time;9 M* Q; c% W, d, {
- uint32_t time_out_flag;
# U O* E% ~9 s2 C - uint8_t Sensor_Data[3];# [; f. r7 u" A7 T% i S1 ?3 N
- static uint8_t old_data = 0xff;
4 y- `8 m# u: l7 p" ^4 ?4 M) e3 E& E - 9 C# j$ K1 j# h6 A) c
- pinMode(_DataPin, OUTPUT);
. Y& i9 a) u) ?% V6 e: w9 U# H6 F, Q - digitalWrite(_DataPin, LOW);
0 D" o; b- R) n: d - delayMicroseconds(980);
4 f* d9 I$ I1 {1 f- P5 Z - digitalWrite(_DataPin, HIGH);
' ?' w$ `& f3 f o1 |' ~/ n! B - " v: z( e l; `9 B6 R" p+ K( n1 n
- pinMode(_DataPin, INPUT_PULLUP);
1 T0 C( \- k; q3 s - delayMicroseconds(10);
& H Y6 n% U! D+ i. [% | - 5 E; r) x0 D' s; M1 A, @
- time_out_flag = millis();6 w) o4 `9 a- O6 F5 q+ ^/ G9 b: ` j
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 a% i H# B! Q7 g5 l
- 3 Q& l" X6 h) U/ ?5 a
- LOW_level_read_time = micros();1 f- `5 L6 @/ L3 d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
8 n- d" P" v# ~$ H0 P# Z - {9 W/ s; \) r/ X$ @& o3 j
- return 0xff;- L5 c- k" K& ?! j7 p- p. K: p' p6 x
- }
$ }5 N" d. p$ v6 f
* f$ n$ t! k9 o/ B6 `- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 F+ |- w% ]0 G( ~- c2 V) p
- 1 g& N1 q5 H* o% j, |3 H" W0 s/ o$ Q
- HIGH_level_read_time = micros();
& _: o1 a+ { ` - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
5 q7 J& q' d7 ^! ^% c( X1 W - 9 |' n8 B S4 o O6 U
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out7 K8 `" z" [6 R. `6 }5 n9 J
- {
& m0 }. t, ^( }( q; g4 [: e - return 0xff;
1 e" t3 `, K# x. s1 j" X$ i) B- W - }
! C7 F- ~3 n: x4 G# {5 z. g2 S; ~ - ; N: b/ b/ q. @# y4 J9 g+ s
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))2 t0 C* O+ Y7 |1 ^0 L! _+ b$ @
- {
5 y. `7 \+ A7 U6 y; t/ { - return 0xff;
; j ^9 t) f7 s% H' G - }6 l1 o- q% J- @
P. W3 ]' k9 h$ y) g) W7 }- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );9 L; p& x3 h6 X- J l
- LOW_level_read_time = micros();- I. _2 S; G8 q4 t
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level. ~1 N( w3 G/ ^8 P
/ |5 e8 k. u1 h4 n! R* t. Y6 R5 c- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) o! Z( j& r4 U4 q9 B - {
. m! a& h3 @& x1 l - return 0xff;
8 V7 V7 S, [; [: q( { - }0 M- {' X% g# F, u9 ]9 @& L( L
r7 j9 G" n' F3 N# u- f+ v- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
: H# }: D; V3 w% A" ] - {0 o7 y% D2 J: @" \, D7 G
- return 0xff;
5 a8 p* C, j0 M7 i- H2 J/ v8 E - }' j, @8 O- p' Z8 l3 K- r
- 1 t/ L G6 s6 @& ^9 y7 o
- for(uint8_t k=0; k<3; k++); f# @0 @; G1 ~% ^0 O
- {- U2 h; c; \+ k) p) H% |
- Sensor_Data[k] = 0x00;' _) R7 r, D0 {1 D
& H; {* ?1 G& |) m4 Q- for(uint8_t i=0;i<8;i++)
" n4 u# c j5 ? - {
; P0 }) c" f- n! w- _# B- @0 V - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
# w. X9 v8 e7 s1 E9 @ - HIGH_level_read_time = micros();
5 y' X4 d+ d9 \3 `: [0 A3 \1 x: \ - LOW_level_read_time = micros() - LOW_level_read_time;
& |( |3 R6 ~0 n6 p2 a3 p
% |. m5 ^, L- W/ Z$ `6 Q& p$ }- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )8 E- \0 Y6 y3 w1 P$ l, ~7 M7 }* V
- {
" ^+ L4 W, }5 a, {5 C - return 0xff; q. s- Y" ^7 f# O1 G6 F
- }; `9 R4 g+ u, }1 P
- 4 _* Z* [, k2 R- c0 N
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
6 I1 n; V* w$ c - LOW_level_read_time = micros();* O8 m$ w5 B# ?6 `, U
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
" E. e. O0 g: s' k. w; {/ {1 B
' Z" T4 K; n; X9 W1 S4 m0 G4 W2 R! H- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1- |/ A8 i3 q0 D
- {
+ V T; N6 u$ G/ ~3 T4 L - Sensor_Data[k] |= (0x80 >> i);. ~. Q+ q. ^, Y: Q3 q
- }; T! n4 c. N7 x/ m6 I2 B" X3 c% A
- else if(HIGH_level_read_time >= 100)
. f, k! o7 B8 D- e - {
* h1 u! j9 R3 [2 T$ W+ Q - return 0xff;
1 R- Z$ n, W* T# f: J) o! x# ~4 y - }* P9 J/ ]. I, S" j3 e
- 3 u8 A9 S9 s* a D4 o
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): P0 @' @" Y% K! g7 s2 J
- {; r) q" E+ T! ?2 l) L7 N! I
- return 0xff;
. p: f r$ c- h! J3 t5 U - }
5 p8 q( I5 b, q6 E - }2 q i" s/ m- {3 J, m1 F: ?
- }
' B* r- O* m' z8 m% c# A
3 ^6 {5 V6 L- v; z* z- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
$ A% n* z" U A* E - HIGH_level_read_time = micros();
: j( O$ R+ }" e - LOW_level_read_time = micros() - LOW_level_read_time;
; l- P! K; p% x' u - " z& a# e, R0 T) K# i5 A
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): j6 r1 q, ^( f4 ]
- {
8 d. d, ^& W! z8 ~" v( h9 y - return 0xff;
9 `0 p. Y8 X0 d+ Z - }7 W" h8 F9 ^# X8 D# n$ L8 y3 ]4 z
- 3 N7 R8 \& g0 s( {. ~
- pinMode(_DataPin, OUTPUT);9 z( d: u5 a1 r3 {: q' s
- digitalWrite(_DataPin, HIGH);: z! s( A1 M, C2 c6 t1 b+ k4 o) s
- ! r- o3 j$ a$ n3 |2 Z, C$ W9 X* C
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
3 s) H; l1 _" K! |1 R; x - {" H3 g0 B3 y' p* F# w( ~
- old_data = Sensor_Data[0]; T# }; V- i" i, \
- return Sensor_Data[0];: q5 L. G" {8 W5 b6 U% g$ r
- }+ ]6 J% u9 o) X0 k! M: ?: R
- else
5 z8 b; e) ^+ ?! b - {- Y' G, t$ _" f Z3 a3 `& A4 P
- return old_data;
0 T Y: f; [1 d' F: i r w - }+ j, }. @& q8 g$ O6 f B/ F/ i" @
- }
5 ], X3 z& U$ @# {" v$ ~# i: r$ P
複製代碼 9 H1 b& i! S' g/ h
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
+ L# |" L, c+ A& Y; L6 ?) ` y* B. k2 L
|
|