|
|
( ^9 {4 \7 o: j* h& i1 V& A
- #include <Arduino.h>
$ D% J( _! g' K4 @9 U* g8 I - #include <MeAuriga.h>
& o! ?3 |' X1 @# N) F4 T - #include "MeLineFollowerArray.h"2 w7 X) U; e4 n8 q! u
0 {4 }/ Y& ~- U! X- MeEncoderOnBoard Encoder_1(SLOT1);) B+ ~1 l& A6 T
- MeEncoderOnBoard Encoder_2(SLOT2);
, ]/ V1 R% @* U5 t1 d" e: |- z* e" I - MeLightSensor lightsensor_1(12);; g2 x8 J' J2 [6 v3 ?
- MeLightSensor lightsensor_2(11);6 D5 q, o- y) i5 j5 l- k. J$ u
- MeBuzzer buzzer;' _* L( V* U" T/ y
- MeLineFollowerArray linefollower(PORT_6);6 K+ }8 z" E- J! e" [6 t2 M0 F
7 }/ K/ R3 o6 F$ g& W) ~! N" O- w- #define Error1 1! v8 \8 E; y6 S
- #define Error2 2
: u' c; \" t1 l: E' h! i - #define Error3 3* j7 |5 c8 } N# P ~$ P, B8 v
( t% d) Z) R4 }3 P5 C& v; Q- #define Kp 15
/ U) n. G( b) m" ^8 V - #define Ki 0.15$ D# q3 o/ P! J7 d o8 ~
- #define Kd 0.03
- h; N* Q% x" x# l% J7 h2 V- I6 i - - I9 ?0 B$ @4 v }6 ^: o+ E
- uint8_t sData;
* z+ q9 Z" P$ B( A' U2 H- d - uint8_t D1; @5 p& n" G: s# X5 ^% m k1 K
- uint8_t D2;$ O- H p% N# \2 y" g1 h
- uint8_t D3;* i; r$ q' f8 o1 |
- uint8_t D4;5 |( {! X. B$ l! S2 g
- uint8_t D5;
" z6 U2 s* o' [0 m7 X9 F1 m0 X Y - uint8_t D6;
: y* d4 X6 s7 Q& S
5 L' w) B8 H% R- float previous_error = 0;
( Y2 O" o1 E0 N4 g - float integral = 0;: }1 b' S# W) C- Q! c
- float derivative = 0;1 |/ I0 ?% x4 d8 `- G+ I
- int Speed = 160;
9 ]) |, F; S5 b5 G% x* l - float output;& M% O# S' p! Q: D# {! O
- ( R4 i9 _0 m( w, _3 b
- byte Left;: G8 w0 x& }2 t3 N8 P
; \2 o! R8 F7 m" j9 Y7 B- void setup() . p5 p* M' @+ t' @# w/ O
- {
3 A2 Y$ }! e5 L' ^, y8 E - //Set PWM 8KHz! W2 k# d: Q) H( [# i; C
- TCCR1A = _BV(WGM10);
5 ^" b) D W4 s( x* y2 | - TCCR1B = _BV(CS11) | _BV(WGM12);- x y, }4 m4 \. c2 M& H: X$ X
- TCCR2A = _BV(WGM21) | _BV(WGM20);) |1 [3 B' \- ^5 v8 P$ L
- TCCR2B = _BV(CS21);2 s y$ {9 S, B$ i
- Serial.begin(9600);$ B; J0 r4 V) i
- buzzer.setpin(45);% r* h$ B0 i3 [) a% b6 v/ I
- }
% L. x! n5 \& d" ~4 E e! z
0 ~9 H7 b, M' F2 [& ]- void loop()
0 K; |% x2 ]' a0 I' R7 P - {
0 g* e) ] `9 j - while(!((lightsensor_1.read()) < (10)));
& o+ S$ X) ?- X, J/ u7 D8 L - buzzer.tone(1047, 500);
! v9 x7 h0 Z- ~1 Q; V - delay(500);
$ D2 c2 A$ N1 y" n8 [) k/ `; J - do
7 \6 X0 c/ a0 \2 b1 p* e) ]( V; Y - {# C" O' k- D! L c
- int Err = getErr();$ m2 N0 m' u% |- T- _8 v# N
- if(D1 == 1)
, B3 J" {2 b5 u% C6 m* {2 g3 r9 u - {
9 H! G, o, C- m y - Left = 1;2 T, `' j2 V8 N0 `- [
- }' _3 y' Y" m$ H( \# Y, X3 N
- if(D6 == 1)3 F5 i% c) C$ f4 H
- {- t) e- S( \" B7 N0 K. `
- Left = 0;
" R9 s0 w& l4 T3 d: ~9 S+ M - }, E" e$ ~ w. ?7 S0 ^ v) T- ^
- if(Err == 99)9 l! ?, c4 f8 r: U9 B# [# z5 s
- {, i6 {' b, x2 }+ r/ L4 }* D8 j
- if(Left == 1)
3 w0 u( _1 J$ u% R1 W/ m - {+ a( N* j/ O* y: v
- Speed -= 5;9 [* g" c% D) o: S8 C6 u
- moto(0,Speed);2 P& O" f6 f0 x4 c# a, p) X1 @
- do4 ?7 w! L+ t+ d8 A; j( b
- { o) E* p. R' p6 n
- Err = getErr();; o3 X7 ~, C; y
- }while((D1+D6) == 0);
1 _" L1 \+ I$ C9 o - }6 p" p( V7 i9 f7 a( ?, e$ \
- else
- M% m' g+ a J+ V* i& r+ m9 L - {
+ O7 S Q& i# h& J. n - Speed -= 5;( q3 h) N* b* T$ q; L5 b
- moto(Speed,0);
3 t |: \+ R2 L) B& U+ Y- T# ^# [ P- @ - do
. {) p+ l, s: j - {
/ v$ R' e( g* K5 V- H7 P) v - Err = getErr();
5 {' v+ N3 k1 P* ~) g _ - }while((D1+D6) == 0);! j6 C2 i4 N& d! l
- }
( T5 ?4 C9 {, o7 V) ` - }) `# C9 Q0 t& w. D9 t; \1 i
- else, \- P m" j# t0 {8 p6 O! j/ Z" g
- {
1 ~6 q& m- p/ v) z9 k7 {, ` - if((Speed < 160) & (Err < 2)) Speed+=1;
* s+ I' c! }; K% [7 \* h - if((Speed > 100) & (Err > 2)) Speed-=2; 3 U! C+ r. m Z, Z
- integral = integral + Err;. D, F8 h/ E+ C
- derivative = Err - previous_error;
$ _2 B. v9 j& M- ^; x8 o - output = Kp*Err + Ki*integral + Kd*derivative;9 v% a- ?$ h, c3 Z* [2 ^ X
- moto(int(Speed-output),int(Speed+output));
+ O( A1 y6 c; A t8 q3 i9 ^! V - previous_error = Err;/ B3 w3 S( F# K/ \$ p, M1 M \1 I, x0 \
- }
( ? d* d0 Q; e3 o - }while(!((lightsensor_2.read()) < (10)));
" F6 p+ c4 x: \' I& [# n3 a3 ~2 \6 _ - moto(0,0);/ J* m( j+ m4 t6 V8 L+ A! D n
- delay(500);6 W6 x+ r" m, k$ G+ y0 Z# ]
- buzzer.tone(262, 500);
) m; m5 Z4 a; K* J$ Y. d# \' a, k0 Q1 K - }
1 O" B% |/ {* f- b8 X7 y8 v* w - p% O+ [. k; o* H+ d6 T8 p
- int getErr()
' g5 r0 X' ~) |, i3 r R2 N - { 7 ?4 W' b& P0 w3 b
- sData = linefollower.getValue();
8 I3 ~& [; l- } - D1 = ~(sData>>0)&1;, T I6 \# [2 m. `0 H8 D5 o& N
- D2 = ~(sData>>1)&1;" N/ O$ n4 H! O4 s0 `* }
- D3 = ~(sData>>2)&1;
3 h4 d5 R7 d, o. `0 ` - D4 = ~(sData>>3)&1;
1 T( q$ Q- i; P1 V - D5 = ~(sData>>4)&1;
( h4 O6 p1 M" r4 ]. w8 | - D6 = ~(sData>>5)&1;# H# }- o$ A0 g8 Q
- int downD = D1+D2+D3+D4+D5+D6;
) D2 ~8 L) f+ C& ? Y( ?! W5 s - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);. \5 [* k* [2 x% W; J
- if(downD == 0)
3 Q9 h1 d9 A! G* k - {
+ w) ?: l' i4 \/ b& m& ~ - return 99;
* ?8 h' ?! u S( K9 E' }) x - }" r+ v; d# c! f& {% P
- else
2 M- m4 _" J2 {* a5 { - {$ E: ~: j* l; c1 ~% @1 A
- return upD/downD;& z% s" \: C' o$ C5 k% q& F; Y. m
- }
0 _ |+ g/ M3 k0 H- T - }4 A' ^( B* k9 V' i/ R- i
- 6 P/ m. D! S4 E2 o5 \
- void moto(int Speed_L,int Speed_R)
, d( u( @( o1 E5 p2 I/ c - {
" a8 W) L. C- H/ v - Encoder_2.setMotorPwm(Speed_L);+ R4 ]- D* K4 o |0 j
- Encoder_1.setMotorPwm(-Speed_R);7 h5 |4 F( {- q8 y5 D
- }
複製代碼
7 G5 Z9 ?8 C0 d G/ r& R! l4 cMeLineFollowerArray.cpp; K' j5 G' u) ~) @
- #include "MeLineFollowerArray.h"
: g0 ~ j% a0 a" k* g" L
2 S' `4 A2 L. ~) ~3 V- #ifdef ME_PORT_DEFINED
0 r7 Z' P* y6 G6 V9 _7 I2 c3 `& y - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
3 w! T- \# v. M' c+ U' ~ - {" P3 \. b% p7 u9 O
' R8 \) Z9 |) R; f3 z2 a, @- }
' z/ Z- ?3 L) {6 R% _ - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
, d0 L6 ?% C1 h" k' d - {
' M' a7 {$ h# a - _DataPin = mePort[port].s2;
# {4 ]0 Z- f( L- [* L5 X2 j - pinMode(_DataPin, OUTPUT); 7 n9 E4 O- e o6 t; l6 u/ I
- digitalWrite(_DataPin, HIGH);
. m1 O, ?, u4 t/ D3 G: _& @. K - }1 B0 [3 }: N1 U$ C: P! y
- #else // ME_PORT_DEFINED6 J- @2 N/ \8 r. ^, \0 r, A% x
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
/ E# K# {* J. I - {/ m5 ^/ b% u* z$ D1 S4 e
- _DataPin = pin;
. o( Z* s# N9 H - pinMode(_DataPin, OUTPUT);
7 b8 |- u5 b H; d6 b- C; V- m - digitalWrite(_DataPin, HIGH);9 ^& W+ ?' p( ?0 _0 ^
- }
# d" C, q+ z% W7 B5 B - #endif // ME_PORT_DEFINED4 L+ Y$ _, K( Q2 b
- & P V i. e" _3 R% P
- % C: R9 B" r. p3 ?( ]9 H2 d* B: g
- void MeLineFollowerArray::setpin(uint8_t pin)
/ _( q$ Z/ z+ f& z5 b: j - {: h- k1 H9 P8 K
- _DataPin = pin;9 n* ?" o" a; o" [7 H. R
- pinMode(_DataPin, OUTPUT); , c3 P8 E3 I! g i0 C
- digitalWrite(_DataPin, HIGH);5 d# |8 I& y2 i9 Q& \
# u% d* T, C! T, D. B. w- #ifdef ME_PORT_DEFINED4 R. Y# [4 g7 X: J
- s2 = pin;
- Y E9 B" z. ^2 N - #endif4 b, ~. X+ g4 c# o4 ~1 _5 t
- }5 Y$ u/ Z2 V$ A, [* O% L
" O' K. L4 d7 ~7 h s5 c( L- uint8_t MeLineFollowerArray::getValue()
# D2 w5 v) [# j8 j( d - {$ _2 V2 h8 n l2 x( K" o e- E
- uint32_t LOW_level_read_time;
: n6 {: I2 D$ g6 j7 S+ ` - uint32_t HIGH_level_read_time;; i2 X% q8 \; t3 y. b1 t8 Y h
- uint32_t time_out_flag;
$ C) M- p. N# `# k" P5 m1 P! G - uint8_t Sensor_Data[3];' Z& w' r' m1 z/ G1 j4 s
- static uint8_t old_data = 0xff;
, d( L9 A9 ]8 o7 I+ M
3 R8 K4 B" |: \7 z9 Z( T+ j' }( B- pinMode(_DataPin, OUTPUT);, d, J' A& y7 G$ i
- digitalWrite(_DataPin, LOW);# Q3 g1 `# }& V) g" z) u' v& _
- delayMicroseconds(980);% P5 H4 Z' O: A* h% H4 a
- digitalWrite(_DataPin, HIGH);
- z0 R }) l- f, s - , Y4 U! [9 i1 _& h
- pinMode(_DataPin, INPUT_PULLUP);
7 c0 V0 ~7 O$ ]: i! E' s! r - delayMicroseconds(10);0 y [. e9 x1 W. c
; f( m4 _0 M4 e2 i- time_out_flag = millis();
. b1 w: f! p( U, d - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 d& S8 g p9 Q9 @( N. k* n
1 G& C# @, h, @) T) k- LOW_level_read_time = micros();$ M1 n) P0 ^0 t3 U7 {+ f
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out* U! r8 M& O5 s$ _" ?8 q
- {9 _! S- C6 Y6 y; L/ }" l( E: v( P
- return 0xff;! y W# [8 }8 E7 s1 \: f
- } h- C7 Q; J% J% M+ @- L/ w
% `& X. ?8 ?" {7 ]& c9 f- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
* r' v, ~2 }5 ~7 Q1 a/ D4 P
1 W% ~) ^9 r. l8 H% f$ j- HIGH_level_read_time = micros();, ]7 J/ B6 T" G
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level( |( t, h9 j" d$ `! t
, i6 C, V: X6 E) k7 U+ |& _5 X- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
! ~; V# U8 }. G* B/ ^' @2 {- [ - {
7 g2 ^" E& w: Q6 Y+ ]8 m - return 0xff;
9 B3 L8 b0 \0 g- c9 B9 M4 p - }
1 o' F" g: O. c1 b$ l0 s6 _! f
1 A/ p/ M8 W! ]4 B1 r- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
; A% _' a8 K2 A' k - {" Z% J$ Z9 S0 f- W+ p( K! U
- return 0xff;
6 D2 S/ ], l' H+ R- v - }& G# {) ] n* K, P8 j
( m z$ Y3 d. r, W( Y' [% }8 T- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );, `- C/ d5 b* l1 K j+ A) a
- LOW_level_read_time = micros();. ~8 Z, d; F( C9 C; P$ q
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
- g: x* L2 Z4 X - ( {3 e( N6 G4 _# D* ]& L" c
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out/ ]8 E1 a, |. o/ S+ m0 {$ n l
- {
; R+ C* P% o/ ^& j) z( f - return 0xff;* J9 R: ~7 t/ f9 H* T5 E
- }
) B& Z) V g6 s) j) R7 Q# E4 o
: O2 u: v; u9 j/ |- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
' l) z0 W$ ?4 a1 Y4 S - {0 j* Z3 f5 ]9 }( W/ ~8 n K6 Q3 R
- return 0xff;
; H* j2 U4 k# J7 b0 ~' P* N( |& F - }! y3 B# g; G# K% I
1 I8 A% m' |! q% G8 N# z1 s; Y1 n- for(uint8_t k=0; k<3; k++); m8 o4 |2 w8 e+ ]
- {
7 i! ~! g1 s5 w: `3 C - Sensor_Data[k] = 0x00;. Y5 U, A" E% z: N
$ h( G# w8 O2 V- d0 L& ~# X- for(uint8_t i=0;i<8;i++)1 m [ z p9 c) u- Z7 M
- {
& u, f, G# X3 k9 E! r, |6 B - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level* E" W* x- s" J& Z. F
- HIGH_level_read_time = micros();
; Y. r/ A" f& h) R - LOW_level_read_time = micros() - LOW_level_read_time;
9 g( m' k( J) x
& i c6 L% L$ R- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
# C. t+ s5 y) \1 E& s# r. p - {
& [% L: _ P( ^& p) Z% z - return 0xff;/ r, `; \" S$ _6 N
- }! ^' R+ H0 a l7 I1 e7 X8 n/ O
- $ y( q7 M- U4 U2 p9 r
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ {2 w, b" Y* Q' x1 v0 P% M
- LOW_level_read_time = micros();
9 g% I: I& B, E/ U, {7 u& f# t. _) ? - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
7 `) c! t/ z* B/ g3 S9 _ - ) s/ b+ `7 E' C: g3 `& l% q
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1# F- E# \$ I, {, D
- {
, E- n4 {# M3 y6 t" \$ P- C - Sensor_Data[k] |= (0x80 >> i);& S5 {1 s1 s2 H' @2 ]% I5 W$ D* Y4 f
- }
5 A' m1 U0 ^0 N/ }0 J4 } - else if(HIGH_level_read_time >= 100)2 l; i, w. S! A8 \
- {# q Z8 J. E! F* \% U" g
- return 0xff;
. {. n9 q4 y/ y8 e5 r0 N) Y - }
4 |: y& F! W# ~& i0 \* Q - 7 o6 K0 p9 }+ E9 v; H5 K
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)2 z: f5 N" i* N* w; V. `- ]; F, f d
- {
( r/ m W* O, ] x$ R. \ - return 0xff;
8 n# ^2 d7 D2 n8 O# X( M& W/ z2 X( A - }' Z$ s1 S4 g$ ~/ h7 M
- }( ?) x* J- y+ W \3 Q% ^1 X" a, P
- }
! C2 X; ]# q/ t7 A - ) n4 q0 j$ O4 o, I# _( O8 J
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
# v$ i) q: }( ^1 }7 S2 q& }6 N. S - HIGH_level_read_time = micros();
/ a+ C3 W3 ?7 F* a - LOW_level_read_time = micros() - LOW_level_read_time;
1 s; Q7 g6 a/ T. g( L6 c3 O" d. p - 3 Y8 s2 b" K9 L) A
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
2 Q8 D/ y. _) v* f& ]0 Z% N - {& J% O S6 i3 N; r7 x
- return 0xff;
; V* C8 q4 Z, h5 V5 L/ c- z - }
( X2 L8 j' f( L5 U" I1 C - 5 p0 }" k+ m' \8 w7 M( @" N
- pinMode(_DataPin, OUTPUT);
* _: g; D; L# V7 a& \$ a - digitalWrite(_DataPin, HIGH);
. e; F7 g# {& C7 J" N
! s# f/ ^ u4 B$ ~, F0 a6 O+ L0 ~- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
. Z, d5 H* I+ \" [5 u3 r - {
" J) P6 o7 a$ [% q* u& S) o - old_data = Sensor_Data[0];
e3 ~, X) f/ U$ t \6 H - return Sensor_Data[0];, K h- z0 t6 H3 V" b: v4 M1 C
- }
, T7 G4 t: L; F$ I- D - else; c, i+ ?) p$ O! N5 u
- {
! }# y* m) q: \ x - return old_data;5 `7 G. ]0 g& u# w( v, N
- }
6 J9 N) T( N( b, q9 {0 q% C4 g9 Y9 ]3 K - }; a" T4 ~6 ~: _/ X# s5 V. I
複製代碼 / S# N/ F6 ?. b% P+ j" d
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
2 R! \! W! @! w+ [
$ R& N9 d6 A* V9 {, g" k7 h |
|