|
" G O" a8 W, X( T
- #include <Arduino.h>
A4 I @0 Z7 Q0 W# \& G" b, s - #include <MeAuriga.h>" R+ I; Y9 R, _5 ~% T
- #include "MeLineFollowerArray.h"
. k& X1 R1 e3 ^7 Q - / D. i* C) D5 M& f" U5 B; |4 {7 d
- MeEncoderOnBoard Encoder_1(SLOT1);
7 D) v! B( p0 S, x$ \0 y- l - MeEncoderOnBoard Encoder_2(SLOT2);+ l) U8 R; f' s" |
- MeLightSensor lightsensor_1(12);
0 p8 ?3 B6 M, k& O - MeLightSensor lightsensor_2(11);
* v& c7 T7 o# g: g - MeBuzzer buzzer;% s6 X3 C1 X% l- a X* ~, W: U! S
- MeLineFollowerArray linefollower(PORT_6);
9 T& t, W" }" e4 s - 5 E5 e* I0 q/ Z0 m' f0 V) c
- #define Error1 1
* Z/ O; P( t: G - #define Error2 2
6 e8 V8 o7 q3 w/ z! r1 P& F0 j$ Z4 U - #define Error3 3
7 j+ L3 _* R5 n2 [. M0 ^9 D
3 z4 h: l! g' q- #define Kp 15
2 J9 g! h+ n9 o" v$ i" ^- O/ ~ - #define Ki 0.157 }/ D& J9 Q( M# R
- #define Kd 0.03
! o) {7 E# {' X* C/ f# T" K - # V6 X( y6 @' B+ w8 E( y
- uint8_t sData;
`! E/ V& d4 L- Q: n7 R - uint8_t D1;5 V/ H! y7 k) ~: O: t5 T
- uint8_t D2;& ]7 T+ J J7 d$ p
- uint8_t D3;
. H* H/ a4 t2 a - uint8_t D4;- d! S4 H' u1 E2 g# r' ~
- uint8_t D5;
6 T& X, K) ~: W: a7 h, N4 c! G/ F - uint8_t D6;& z2 W. S9 H5 n! X$ I6 q3 ]
6 v) S' w6 K+ E' w- float previous_error = 0;
+ l" M I# r r9 y, G1 r, `+ J - float integral = 0;
9 A5 y3 p7 z' c4 e3 d2 C% b2 Z - float derivative = 0;9 l4 s) H U8 D B
- int Speed = 160;4 s# J& k d8 p7 V% J! z* A
- float output;
4 ~# ^: s' t* A- f! B: ~3 n
# U: {5 Y- r* T* e- byte Left;9 H0 p! @/ x' {* ]
3 T' Z/ e8 |5 E; A- o- void setup() 2 ~# |4 |2 l: E0 S/ @
- {
& T4 d; K; V. \) H8 H+ s - //Set PWM 8KHz
+ |8 g6 x; t' Y" o6 r( n. ~ - TCCR1A = _BV(WGM10);8 n( v' E8 K+ e# {9 u! x7 c
- TCCR1B = _BV(CS11) | _BV(WGM12);2 U2 U* Y8 n5 @! ~
- TCCR2A = _BV(WGM21) | _BV(WGM20);3 ?0 T9 r' A# J
- TCCR2B = _BV(CS21);! s( }, O; i. K& X G9 S
- Serial.begin(9600);
. W; }' C( N# R' X* u1 C - buzzer.setpin(45);5 w. R* x" e0 o5 R$ B9 E4 W: O
- }) Q' S6 F1 S% T2 A
- }2 x# O7 d+ [+ c& t- void loop()+ K6 k/ a2 U7 ^6 T
- {6 X) s+ {& O/ b/ L5 ]
- while(!((lightsensor_1.read()) < (10)));
5 v, ~5 x' C/ S2 C8 l( F - buzzer.tone(1047, 500);1 ]7 M5 d) y! p7 I% q0 S& G, z$ ]
- delay(500);% l; k" f& O8 m. k9 d
- do4 G$ e$ k0 q! a! S
- {5 s( z) n) Q8 |9 |6 y
- int Err = getErr();
! p8 o. Y; ~+ W - if(D1 == 1)& ], ]* Y; l3 C
- {
- ?! e N# h3 _9 G4 @0 w' c7 @( ` - Left = 1;
4 k' y1 w3 Z1 J& o, b - }( C2 ?. t& M0 a1 O1 o. v
- if(D6 == 1)
1 o' N/ q' o" {" ]5 B& A) I) e - {" X: G o y; c( A
- Left = 0;' ]) F/ }# e5 l8 \9 }
- }4 F+ s% f: D% q2 Z( v
- if(Err == 99)
" G1 B0 Q1 h5 a - {: |0 Z# M! G4 f# Q
- if(Left == 1)4 d, D8 a x' e* ?" L3 q N
- {: }) G; {6 o9 u2 c9 y# v# u" @
- Speed -= 5;* u. a4 j9 w" J! n. P2 z
- moto(0,Speed);
4 p q5 ~4 }1 J$ \% ]9 n4 V( D - do8 I& G) j' W1 Z# k; b
- {; X, ~3 s" Y' J/ N
- Err = getErr();
8 u! ]% g3 v9 ]8 L- F+ _ - }while((D1+D6) == 0);
# k% H9 i" \* r! j$ Z' Y - }
0 I5 q* g% V2 n# Q" `, d - else
( I. c# ]& x. `0 S2 Q3 C - {: ~) y( ?0 h: x/ q6 ~: d4 m7 Q8 w) M
- Speed -= 5;) b# j5 ?( {9 X: J. n0 U
- moto(Speed,0); 8 h1 b9 ]* ~- q7 H1 Z( O; D6 C" U* p
- do* l1 b; d' N/ X; l
- {) c) o# {) B7 G% f
- Err = getErr();! K$ P- K3 p& L# G2 \
- }while((D1+D6) == 0);
7 b( k. z: ^7 d0 s8 o6 j5 ` - }2 k3 m4 P9 F) }4 [/ C2 K1 z
- }' a1 w+ b: P. e. p6 N. P) h" {1 b- z
- else) y. g' h5 v4 l4 a1 |, k$ n1 W
- {
; L! u% K2 }, n* r: M - if((Speed < 160) & (Err < 2)) Speed+=1; * _- g3 [6 N/ O @ y( W ?5 i5 D
- if((Speed > 100) & (Err > 2)) Speed-=2;
$ ~) ^. p* D* u7 I: l+ b# g - integral = integral + Err;
, f! S7 s0 X1 Y0 B1 I - derivative = Err - previous_error;
6 F6 B* y r, { - output = Kp*Err + Ki*integral + Kd*derivative;! D: E/ \* a/ X! v) v6 {
- moto(int(Speed-output),int(Speed+output));, O% }! R6 m$ l9 h+ m
- previous_error = Err;
, l+ L9 M& s' |' w: F M3 B - }1 [4 A9 Y, y5 c& @2 k
- }while(!((lightsensor_2.read()) < (10)));/ f- J+ z/ f! T p* W
- moto(0,0);4 O7 D p4 K+ n: x. i: K
- delay(500);
3 v% v7 V# V0 T {: a1 F1 {; B - buzzer.tone(262, 500);: g' N: P1 Y+ ?5 o( V( O
- }
- S( b& q2 X* T' A6 ^- i W
) u; L( V( @- u5 z2 B- int getErr()
7 F5 r2 T: F: {# y' Q - {
1 Z/ P% N: J" a3 Z: T0 l" v' S - sData = linefollower.getValue();+ e( l4 |: p. O7 @: O* P2 O- s3 L. r
- D1 = ~(sData>>0)&1;9 K% F" p3 C! J% U0 Y/ }6 e
- D2 = ~(sData>>1)&1;
$ m' k' r5 p! [" s! u5 f. g _ - D3 = ~(sData>>2)&1;0 H4 C2 p& ]0 ~* N ]
- D4 = ~(sData>>3)&1;6 e. \0 _' l' x: b. J% g8 Y' M
- D5 = ~(sData>>4)&1;
8 ]& J/ o5 `" c; _/ z8 @/ S - D6 = ~(sData>>5)&1;
' y- ?3 Q- ~ J' g d - int downD = D1+D2+D3+D4+D5+D6;
6 {% v$ W- T5 m) q1 f - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);" A1 O; q: L+ Z9 s, A \) v
- if(downD == 0)
8 O: H% X0 M* R. J8 A6 n# q9 V - {
2 N& b' ~7 v2 K: U, ^ - return 99;* v- C6 Z, G# D( h
- }
! Z" ?, y- _/ l8 p- v( C7 G* J' | - else3 z3 g/ ]9 ~$ _" n
- {
& ^) P) L, ~' x: V1 C; @ - return upD/downD;
8 m; m) h' g, j% C; _2 z- x - }
& e3 C" t, p2 p: | - }8 N2 a% H% y/ I9 S" d, ], t
1 X1 I3 Y$ @9 W0 `, Q/ k) D- void moto(int Speed_L,int Speed_R)
) B( g+ a e+ `9 `6 R) p! I - {
# Y( W& \ u- T1 n& { - Encoder_2.setMotorPwm(Speed_L);
8 y' E" a5 _# x0 m+ p/ e4 g: ~, L* t# L/ o - Encoder_1.setMotorPwm(-Speed_R);# i0 }' L+ \2 c
- }
複製代碼 7 P2 B+ J4 F3 Z% O$ B
MeLineFollowerArray.cpp
2 E+ ^1 H) k4 O7 C- #include "MeLineFollowerArray.h"
5 l3 O: J9 C( r - 1 M! X3 h- z( l/ m
- #ifdef ME_PORT_DEFINED/ D' @: p. [3 N! k* L
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
( `, e! g. X$ V* G5 m: V9 q - {
3 P- b& g& G \. e* |( ]9 i2 M
/ Z* l) n* S4 p" N- }
8 H7 P, i8 b2 E# T, C - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
% ~2 e. i1 S t' Z - {2 ^: I9 l6 O( b2 Q; j0 [5 b
- _DataPin = mePort[port].s2;
+ C$ s% w: n$ | - pinMode(_DataPin, OUTPUT);
2 c) Y& {$ H3 S* b1 }4 H5 {/ u: _( m( w - digitalWrite(_DataPin, HIGH);
+ C0 S, Y4 i- e - }
5 X m3 |$ ^# p1 _* Y - #else // ME_PORT_DEFINED
" N5 |% ~, g$ @8 r' \ - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin). N& R3 g' e5 y& `$ I6 \ X
- {
+ j/ D# F+ I# { Q0 `% r8 B - _DataPin = pin;5 [$ v, J3 ~: u
- pinMode(_DataPin, OUTPUT);
# |" U/ x( q4 X - digitalWrite(_DataPin, HIGH); H5 v* [+ T! [* ^: ^0 Y
- }, K+ }* e R4 y$ ~/ v0 k
- #endif // ME_PORT_DEFINED
# d* x3 [7 H0 a j" a& J3 T - [% m3 j8 P9 l+ p& T
- Z+ C: R6 _, Q# p- g" n- void MeLineFollowerArray::setpin(uint8_t pin)
" k2 p. \9 [) q1 M6 O - {, L* ~0 d7 x9 I' l
- _DataPin = pin;
( g( T. R% M: y) a7 A/ Q6 j5 M - pinMode(_DataPin, OUTPUT);
1 j: v3 O9 ]) T/ C( v& o2 ]! S" Z - digitalWrite(_DataPin, HIGH);2 S; Q! {* T2 m& W2 }
2 x; o0 [5 g6 Q- q2 }( C8 K- #ifdef ME_PORT_DEFINED* P+ Y& J# C0 d$ Z( o
- s2 = pin;
2 S9 j L9 h7 t$ ~* B - #endif1 b1 A4 d2 \5 o/ h
- }
5 p4 H0 B3 ^# `9 P7 Z1 k8 K% S1 I - # X3 F2 Y& F2 P X7 ^: E" C4 r! P
- uint8_t MeLineFollowerArray::getValue()
3 H6 ^7 K# p: W( I! j1 a3 N! ?; A6 P6 a - {
- r$ z: Q2 B6 A# G8 Q% f; z - uint32_t LOW_level_read_time;! I- t+ l% t! a4 F$ L3 E8 |& q
- uint32_t HIGH_level_read_time;
3 J; ?# f6 E* j/ Y3 P - uint32_t time_out_flag;: O- w/ E' G0 h1 X1 g @( {0 W
- uint8_t Sensor_Data[3];
1 A! y1 @3 B. R# j3 }; Z4 p. o - static uint8_t old_data = 0xff;
* ~! e' _4 V2 K# w3 h0 l7 c1 J
5 {- j" N1 h; Y: {: C' F' u" V- pinMode(_DataPin, OUTPUT);( _; w& {+ p, g; f5 Y% m% w) n% j
- digitalWrite(_DataPin, LOW);1 l& s8 U+ U, D9 l; w
- delayMicroseconds(980);; l7 A+ a/ l( Y% D7 D8 |
- digitalWrite(_DataPin, HIGH); u0 v& {7 ?0 ?* R
- $ h$ }. N: u( Q5 ~7 Q2 ?" [
- pinMode(_DataPin, INPUT_PULLUP);$ N A, D+ y3 u. u
- delayMicroseconds(10);
1 W# H$ n6 |% n* q: h8 n - 3 h* p1 p5 X' p
- time_out_flag = millis();6 Y% F3 F1 c1 [8 _8 r) j. {# Y) s9 q
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );1 j0 R5 `4 W0 J
4 ~' g5 | I$ I! @- LOW_level_read_time = micros();
5 n; b+ \& M. b; l" N7 w4 D - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out3 j8 F* G3 n1 H# C
- {
2 R" B) s8 a* C" v+ f - return 0xff;
7 K0 g6 s9 D2 d+ M2 K - }# p; l% q5 U/ l
- 9 b6 h6 c* ?, N7 s
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );; s0 M1 y/ s0 s7 E9 n; W$ [4 h
( z0 ~# B" J3 A. p- M- HIGH_level_read_time = micros();0 C3 N' x h w" L ?3 m7 O# Y* _# x; k) W
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
7 `5 g! d( E2 o# y - # ^/ O% `$ t1 B' N/ h, [* H* ~
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out0 W; N; E: u4 ]. S+ j! w. a
- {' J8 `7 b; q, {: a: X# S3 A/ o
- return 0xff;
; C. x1 `8 ~. I" J3 Z, _ - }; B7 R7 y+ k. E/ v& t
- 5 p6 ~1 o) l9 f7 \4 ?
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))) p0 N0 K; \& c6 {. M( ^; @0 h
- {
" e, r6 P* o7 G5 f - return 0xff; Y! ]& g. {; y6 e' c! r# M! @
- }
1 q' W% D( V7 @- `
& W* P3 R9 e ]. q- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) y+ F( B. T) L. L$ j+ B9 m& Z# Q; K
- LOW_level_read_time = micros();! ~7 l$ D# G9 w
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level0 c! \& [( w4 y7 s; J$ h. v
- . x0 n: X7 m9 G, d$ p; C
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out% T+ u0 D2 H" C/ P. C* {* \
- {
& C; h, t+ ?: s8 O; ? - return 0xff;
, ^! @; g6 U% Q. G9 g& d, y+ Q* a/ ~ - }* _5 T( E+ |9 x' `
- 8 f9 b, |2 l. n, j' T2 S6 ?* W3 _
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
$ ^0 c& p$ c: d: }! H. n - {
s' Y# A6 l6 v; c" `2 U. A - return 0xff;- J! h3 Q; @* b; ]! E2 D% c2 B- I
- }
/ A! X8 f& J ?% k0 Z- n; m
+ s% J* V" g) ^3 R- Y- for(uint8_t k=0; k<3; k++)9 P( q) g2 P T3 r
- {* p+ a$ K3 Q. i- Q( t
- Sensor_Data[k] = 0x00;2 ?* M' K* q) f/ v# Q
) e1 M' G) @+ x: U, v2 N- for(uint8_t i=0;i<8;i++) f) c6 m& {- ?7 F8 |7 \/ n/ ^) M2 [
- {- V- }$ c1 L4 l" q; o5 k2 K
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level4 X( r) |' {* m Z2 E3 B* _, B+ G
- HIGH_level_read_time = micros();
3 D: j. h, y6 b0 N A2 f; V" \& y - LOW_level_read_time = micros() - LOW_level_read_time;5 _% [* L: t. j( h
- " x n9 h5 z5 ~* B% O! s3 V
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
2 S- q6 Y. Q2 E' L - {1 [6 E. b* p4 \2 I0 A
- return 0xff;& R% G P7 T# R! D( E
- }: E( r/ v, _/ M
' X* f3 X7 L$ D: z: n* o- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );) w- p' n# ]9 s. U# F
- LOW_level_read_time = micros();. E2 o/ J2 u) ^4 J) @! W
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level- c7 B7 J' L6 S& B5 J% }7 M
- ( K/ F& ^, |) @8 ]5 @( o
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1+ D& P& r: R8 c% V$ i3 J4 l
- {# l( D+ J4 f; _: D; m
- Sensor_Data[k] |= (0x80 >> i);
/ ]# W; K( ?* w" C - }6 @! @0 o6 m( ]/ Y0 Z' c
- else if(HIGH_level_read_time >= 100)' X* p- j; N6 U/ m$ T' r
- { M2 ]. h F; J/ j
- return 0xff;
% |7 W& J: e! b+ d! o% u - }6 T) Y0 }0 Y, [+ \) C* U. K
- ) K! }6 l3 M0 e0 k) ^. ?
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out), o5 z1 W' V& w( U+ ^) _+ u
- {: ^8 y+ x4 N: g/ d' w/ o" f$ D" N
- return 0xff;
0 l$ d+ p8 W4 S' V5 F - }' \5 Q' t7 D3 e' i; ?+ t% `
- }
: A, f( C6 X4 r1 {& E: `# W" U0 i - }$ {% J# w/ A: S5 H: ]% t
- & v" q& S4 w* H9 w# M
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
+ X1 ~; }& F: U* p- J: t; Q/ F* J9 ` - HIGH_level_read_time = micros();1 c, j4 R" V, h, o% b7 \
- LOW_level_read_time = micros() - LOW_level_read_time;5 h5 U" h t3 ?! K
+ A! r; k @+ C3 ^- t- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
0 {7 K7 S" r& X. y! s1 c - {, |, L' y$ W/ ^# J; [3 C( v1 n
- return 0xff;
$ L+ h7 d7 Z+ | - }
8 y" A$ r# [4 b; r1 a
' ]; P9 Q/ Y& m3 z- pinMode(_DataPin, OUTPUT);
) h1 X1 C& ^$ V& T - digitalWrite(_DataPin, HIGH); @8 }9 Z2 Y4 Y/ u. e0 z, x' @
$ k; }! F) {& m/ W w, e% _- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))) D! C8 y! ]$ \
- {
6 f- J+ |6 i! F0 Y - old_data = Sensor_Data[0];! } ]! [$ \; `" v+ t# S/ n5 f3 k
- return Sensor_Data[0];) h- H, j8 O2 p- t& W
- }
5 }+ ]- h, k; F) s7 @3 H - else6 L& |# ~8 I* a8 C
- {" d9 s8 C9 ?8 J$ u# [3 z2 U" O' E
- return old_data;) ?' l$ P. ]/ D& u
- }
4 ]5 Q/ W) ?7 q6 {, x% ]9 F - }
* @7 {# G6 C% G3 }$ }) b# a* c
複製代碼
1 o3 B9 O X# VMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
* Y! e: g' a/ z3 E! m* |
5 i8 o; X4 _2 T+ A7 L6 z- F |
|