|
1 \( R$ N% A2 m; c. ]/ p3 y2 I- #include <Arduino.h># X/ Q' E4 s" o/ O* z
- #include <MeAuriga.h>. J$ r, \7 D6 _) b+ [) R$ J% ]9 Y' I
- #include "MeLineFollowerArray.h"1 O5 K( x7 }/ ]+ ~; y
- G, ?; u* M# N0 J- C- MeEncoderOnBoard Encoder_1(SLOT1);/ g1 L, K- D+ X" ^% g/ ~
- MeEncoderOnBoard Encoder_2(SLOT2);# ]+ ~ @! k6 w. r/ P# x9 H% k2 |
- MeLightSensor lightsensor_1(12);
0 w0 d4 d7 U) x# A( \) s- F% F - MeLightSensor lightsensor_2(11);
' K h; W( h7 [9 c' E. J - MeBuzzer buzzer;
8 X: f$ d" M" N2 ~( L - MeLineFollowerArray linefollower(PORT_6);: h; Q. l' q. \/ j) P4 s" h
1 q9 f; }6 d+ F/ u- #define Error1 1
4 \. f# }; ?5 m0 s! G - #define Error2 2! `* f3 |/ D( ^1 i4 E9 D" N: ~
- #define Error3 3
' J% D0 a( h8 x2 a M# ~; W- N$ f/ _$ p
1 L* M. n. P5 h m- #define Kp 15
( R) E/ } V O3 ^% y- P# b% N+ X - #define Ki 0.15
9 u, J1 j3 ^$ [, F. j. E - #define Kd 0.03
$ m( t" a3 a5 G7 T. f, n
# f7 `1 x6 X; n& p2 H- C- uint8_t sData;' `! `: [) E# |1 c. Z
- uint8_t D1;
0 ^$ L, {# g8 H - uint8_t D2;% u, e. y+ G/ W* T; I
- uint8_t D3;) u) X8 [' o! n! f7 @4 m% ]' B
- uint8_t D4;% n! k, Y/ @5 H! ]" d9 X; X
- uint8_t D5;" X A# x# p6 R5 m& M4 r1 n
- uint8_t D6;
9 U2 K/ x) [. K, ~8 `; x
' y, E- c% Z# s1 f) o }2 q- float previous_error = 0;
+ }8 a: T7 v: ?8 Q& | - float integral = 0;
, k% O& q' u& ] - float derivative = 0;' K5 J6 J5 M8 H: b: Q! i9 O
- int Speed = 160;# n& i: W9 C! d8 O
- float output;4 J- D9 Q3 ^4 B# u# K; I
x, t( L, f# G: F- byte Left;
0 x" T6 l$ c% s, h- _2 ?# ] - T% F; L0 d3 v0 r8 h$ V1 y7 @
- void setup() 3 s4 b/ ?: c- N4 |- [0 p
- {4 [2 N1 k, \8 G! d" M
- //Set PWM 8KHz
2 h$ ^+ E# \1 p& t& k7 A) j - TCCR1A = _BV(WGM10);
7 F* {+ K8 O- Z! M - TCCR1B = _BV(CS11) | _BV(WGM12);
9 H0 M* a6 @, ?# a - TCCR2A = _BV(WGM21) | _BV(WGM20);: X6 k4 q* }& P# y" y( t; W
- TCCR2B = _BV(CS21);# _+ k* o z! u. w9 X
- Serial.begin(9600);
9 c, m; H; j/ A$ n/ r: I9 f% z - buzzer.setpin(45);: c1 D2 c( e" w' Z
- }- E% I4 W/ h. h/ e+ F3 x+ D
# u- f5 A5 E2 E. m u8 _- void loop()
. _! w( g; C" W' ]' V( |* n - { z) e) f$ k A7 Y
- while(!((lightsensor_1.read()) < (10)));: [1 ^) z( y, R& c
- buzzer.tone(1047, 500);
* q- V8 `6 ]6 U% W - delay(500);& J/ p4 t: {' l
- do% r! E6 {2 k+ _* W- w2 R
- {! y e/ Y9 `' @& p( c
- int Err = getErr();& @+ n7 R7 d- i4 f0 ~; B
- if(D1 == 1)% ?7 ^. L% m( r8 i' Z0 P
- {6 I, D6 ], C7 m
- Left = 1;
, l/ p: m$ Z- k" L2 N7 I. _ - }1 i4 y5 |3 m8 u& r
- if(D6 == 1)
: y! e9 b" ~) k8 D; P - {
* M4 M. r4 n% B! L+ x - Left = 0;
, F7 n2 S& N' ^5 C9 F6 L/ _ - }7 {2 c% e2 E9 m. w3 ^
- if(Err == 99)
; J! q# p8 O, U; _3 a% X/ F - {" v3 i/ T* K: c# v! Z5 o) w
- if(Left == 1)8 e H: K6 `& j2 l1 g1 z- I
- {
' L+ U3 \, n1 i o+ C5 }* t - Speed -= 5;8 e, y: D+ W; E3 c; F- ?" a/ {
- moto(0,Speed);
$ z& J, k, L* E2 Z8 m% w - do% r/ y* r v3 O9 W- Y( z. l9 p
- {
- h* s: h/ H4 N a2 e, f - Err = getErr();4 n* e; c2 V7 P# ~7 n! ^
- }while((D1+D6) == 0);1 b. C) i3 @, i; B5 E
- }. i& O7 N- L2 x, c: v, G
- else/ W* a6 `$ D+ r' \& Q4 P
- {6 I$ D* Q& N. I5 g M
- Speed -= 5;
% j* ? h9 O2 d9 y8 h8 \+ M5 c - moto(Speed,0);
W* D9 t9 l" ]' N1 \6 v: A' r( h - do3 x( g, T/ y. E" C( g5 @
- {( X& p; x0 N1 k8 a5 y8 J r
- Err = getErr();( |- X7 ]* {- s6 c' t$ K0 |. I
- }while((D1+D6) == 0);
2 t3 A2 A( k& W2 M - }# S0 n9 E6 L* u* j( g$ H5 r! I G
- }1 ~0 s. {: |" h$ S. u" {. b
- else" I1 y# i2 d3 S0 f w* z
- {
6 Y& U$ x: d5 C" k0 F5 z - if((Speed < 160) & (Err < 2)) Speed+=1; . L) d* y+ j% U
- if((Speed > 100) & (Err > 2)) Speed-=2;
* @1 C1 k4 L' X3 g. m" o - integral = integral + Err;
( ~- w4 Z8 D8 Q - derivative = Err - previous_error;
6 O! B2 w& U$ [5 o, b2 \/ \ - output = Kp*Err + Ki*integral + Kd*derivative;5 B) t( x9 M) P F5 [
- moto(int(Speed-output),int(Speed+output));
3 v9 B. W% k$ P3 K* _ - previous_error = Err;
$ o' Z' X# {; R) M0 f) ?5 r - }
$ m$ k0 @& G) H" t8 Z& m - }while(!((lightsensor_2.read()) < (10)));
( i+ [9 l5 r- F. K3 G u6 \ - moto(0,0);7 y! U6 Q p& c5 d
- delay(500);
* C: |' z* n4 i8 T - buzzer.tone(262, 500);
; P0 T) `! |% Y( T- D; Q3 c - }
$ Z# l9 w& Z: F
4 O3 c% o. Q- @$ S' D! H- int getErr()
& d7 P- k$ P8 G, { - {
# m3 p0 v$ b6 W' | - sData = linefollower.getValue();, `3 R2 ?9 z" D5 P0 j
- D1 = ~(sData>>0)&1;1 z8 k0 o% W4 ~2 | y O7 ?
- D2 = ~(sData>>1)&1;
6 \" m8 d* F$ |. I" R8 t$ @0 _ - D3 = ~(sData>>2)&1;
# Z0 M# e6 u. E6 N4 Q: p - D4 = ~(sData>>3)&1;/ {# y9 S' h# f+ ]" F
- D5 = ~(sData>>4)&1;
- {; o( b, B" F+ Q7 j' `6 i9 o1 U - D6 = ~(sData>>5)&1;
% t! X7 g$ |: o; f0 { - int downD = D1+D2+D3+D4+D5+D6;
2 j8 j- N- f9 s# g9 B' P( o - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
5 R5 c, H' L9 { - if(downD == 0)) g Z& U0 q* ~4 G, W7 f3 J
- {
* _7 n2 ]1 K- n4 q - return 99;# ?9 V" y; r: y( e3 Z
- }& f+ P+ ?7 o7 q1 y0 @( S! J
- else/ \! ^# P; y1 ?/ s$ @
- {
& J& S, U; f' i& z7 u" O3 A0 s - return upD/downD;/ f! z: D/ i% O$ |, E7 k. ]/ y
- }
q" d- z4 f) [ - }
0 v9 t$ X9 o2 c9 Z c c: Q - # ?( M5 B) U8 M- v6 e3 U
- void moto(int Speed_L,int Speed_R)
8 h# [, W6 ~6 R: n; T: I - {
0 n* X; W1 _4 @3 u: _ - Encoder_2.setMotorPwm(Speed_L);
/ T7 a: C b+ @+ T# w, \( q3 \ - Encoder_1.setMotorPwm(-Speed_R);- k. Z' {& f g
- }
複製代碼 1 q( O5 S6 |4 P
MeLineFollowerArray.cpp
" t8 u3 K/ f5 ?8 X) G- #include "MeLineFollowerArray.h"
! Y$ r7 G% b1 m7 C" k5 b
a- k3 D0 ]1 N0 I7 w v" W V- #ifdef ME_PORT_DEFINED
( M) g! k4 d& q2 m) L - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)1 Q7 O8 @: D w; u5 p E3 P
- {8 u, `+ V% C2 r+ Z1 G
- K) z+ U& {7 z
- }/ p0 J9 l) e% G: `. K
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
$ t- ?! h' s/ M G2 [1 G3 ] - {
: G3 { G# d) U/ ]! h - _DataPin = mePort[port].s2;: j/ v5 ?" I* q* S2 N# c
- pinMode(_DataPin, OUTPUT);
+ }- b+ ?& [- ~: a& N0 N$ x - digitalWrite(_DataPin, HIGH);
) M/ B8 e* V+ K- F% Y3 j! D - }
5 f0 Q9 q7 N( L- R$ {1 M# z0 z1 t - #else // ME_PORT_DEFINED
* P( K. q. H) P% Z8 s - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)% `/ |7 Y7 T; j2 _
- {, l" x) M E1 l( Z2 [
- _DataPin = pin;1 _3 h7 [& Z4 M
- pinMode(_DataPin, OUTPUT);
. |4 s! l9 f2 K H- j( Q6 v9 u - digitalWrite(_DataPin, HIGH);
0 d; p# o( ]6 T) k, \. ] @0 S - }
. M" [: t0 D' h. \% b2 ]: p& g - #endif // ME_PORT_DEFINED
8 M3 y. Z* |6 N
7 E) l M/ C) O5 {/ |2 o1 Z- 0 ]$ ?- r4 U4 s5 y+ `' P$ x, ], l
- void MeLineFollowerArray::setpin(uint8_t pin): W2 Z/ o5 T* n0 D5 t4 Q& g
- {
, ~/ M0 A& y6 L1 B. N9 z - _DataPin = pin;
2 M$ @2 W/ ?0 t: m - pinMode(_DataPin, OUTPUT); * i/ S! r P3 \6 j) G
- digitalWrite(_DataPin, HIGH);* l( [. V7 [3 N; m# o; Y6 {
- ( b& M! b5 k5 r$ {8 ^$ K' b3 X7 R& z
- #ifdef ME_PORT_DEFINED, e% n! i! b7 r: {: d* N
- s2 = pin;
- K1 L! E% [7 L9 Y4 g; Z+ w7 C t - #endif I5 g e4 O3 }
- }7 u9 Q; u" s( T
@1 l- Z+ }1 r- uint8_t MeLineFollowerArray::getValue()8 O( G; Z" ^9 Z8 ~
- {
% K+ k4 u" P9 n4 W4 T - uint32_t LOW_level_read_time;
- D0 [# I8 P6 ]. ~6 Q( u+ r - uint32_t HIGH_level_read_time;
5 s) n. F( M: d - uint32_t time_out_flag;
4 n- j; q [8 F- B - uint8_t Sensor_Data[3];
. V2 D0 `9 k7 ?: Q2 p - static uint8_t old_data = 0xff;
* q( T. D$ h7 M4 B! u" Y# {
6 c8 d, z) l, Y$ y$ X- pinMode(_DataPin, OUTPUT);
0 J: r/ p i+ P q) K, _1 R - digitalWrite(_DataPin, LOW);
; B) z2 r! b- X" E7 y0 R: S - delayMicroseconds(980);
- ^1 o. B0 Y+ J - digitalWrite(_DataPin, HIGH);
( ?5 V# S( p: \
& { i, s8 j8 q* I7 J! C5 b- pinMode(_DataPin, INPUT_PULLUP);
( U+ Q @* c% G4 U8 M/ s - delayMicroseconds(10);
( O& d. Q- o# ?( z+ u- w$ Y B
; }* ]/ v; l# Z$ A5 L8 a3 J- time_out_flag = millis();
6 |6 X0 ?4 p$ S, i - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ P: w+ Y9 i- g+ E' Y, v- O* _* D
- # j& e4 p. w2 v5 Q; f7 Y. {4 ~$ u
- LOW_level_read_time = micros();2 U9 f: o+ G) t- L( w6 D$ V
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out& }3 M+ n# L3 y3 v/ W: n6 S! c: ?
- {! E9 _. U# V/ x+ ?3 _
- return 0xff;, y& ?- Q& a, o/ }
- }
: p9 N$ \- y$ t! \
3 E3 M4 }) z3 w) B+ ^- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
) U y; L% z) R- G- k/ h- Z - 1 W5 w; L+ E' t1 x6 U4 k; @" @' o5 {
- HIGH_level_read_time = micros();
; |) U$ c; K" `* p# T- {! z - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
. A! T' P k3 L3 ?/ o/ E' W - ( v; A9 R: X! V6 f A; _5 v3 _
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
5 C) G9 A8 ~, X w; o - {3 A) G ]4 n! N2 b( e& ]
- return 0xff;. |0 v ~" U% W' Z H
- }; |9 a, K+ e( m! O
- ! e. c( ^9 b% r: S* R" O/ o
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
4 i8 g# T! F5 K; C$ K2 A - {
4 d! V$ G! y. ^- p9 j& M9 }) U+ K - return 0xff;+ L* ]* X5 M/ i0 W
- }
a' M7 f# E& E' X& s. |! l$ j
! ?1 p. ?- S0 E8 v- y% {- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 b- A5 d! ?% p' @
- LOW_level_read_time = micros();7 M0 x& S6 K6 T5 v4 H/ H) Y% n
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level2 e) `. C6 P% _: R4 T/ g
! P( |) p! @: k% }, Z7 R- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& A6 ~% `' [* I4 E) ` - {. ^6 J& W) k: \/ Z3 i( X
- return 0xff;* V9 E' v X" t( g+ w5 W+ z% q
- }9 t9 N6 \; L: x
- ' z3 H6 o; F, G+ y6 H' [/ O
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
0 m$ i7 S( j0 W! J# `, s - {
+ a0 Y! ]7 L3 L) o) f' I - return 0xff;' c6 Q9 U7 C; u5 i! d
- }
! d8 s! f, |7 E7 _- G d6 a - & [3 a9 b$ I% ]" V" O% l
- for(uint8_t k=0; k<3; k++)8 E8 z5 c6 R! O
- {5 h1 [& N8 c2 y% a$ L1 `1 v
- Sensor_Data[k] = 0x00;
2 _% b; {4 i" J
+ S& ], o3 _$ j+ h7 @( x6 w- for(uint8_t i=0;i<8;i++)
% q# b, u6 c, b8 U - {, T5 D Y( H: X+ J
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
* z0 `" {: F& n: e' {: d2 ]$ V - HIGH_level_read_time = micros();
6 T( _. U! H/ @9 F2 \' x! Z0 ~ - LOW_level_read_time = micros() - LOW_level_read_time;
1 y- z6 s. A, y; A" w& p - ( x) {0 H5 z) w2 y6 Z
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
8 T9 Y2 u/ t5 R - {) {: M4 {8 t( j
- return 0xff;! E0 M; u) _" I8 e4 f: t9 |
- }
( w, q6 O, c7 F
( \2 c$ R" x y3 t# m# M- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. o6 n, a7 W0 b* o3 V# f8 ^$ }. S
- LOW_level_read_time = micros();5 w4 p& m1 t/ q. ~/ z
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
# n4 \! }5 y2 p- e3 o - 8 o% {$ [0 y$ o2 }5 k
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1
% b/ D. J: c1 x3 c) b - {
; S6 n+ `( F B0 b8 l2 P - Sensor_Data[k] |= (0x80 >> i);4 h) E# }7 r6 z+ u9 ~+ }
- }
9 v( ~+ _% G- [' K+ D. {, ^ - else if(HIGH_level_read_time >= 100)3 Y9 \# b+ J @/ a$ v! ]4 \8 y% C4 N
- {
. W' ^- ^5 p0 B3 ~% J - return 0xff;
0 W% C7 K2 w1 u1 H" \ L! A- O0 S - } o5 \8 K6 \0 M3 a; ^! ^
- / B+ g4 {: ~, d' x5 b: T
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
% R I$ t, s: K6 ^ - {
/ b8 ~& w+ q: Y; F$ p; { - return 0xff;& s* U) L( m" o0 y
- }
4 s/ s# L* b( j, [5 V- s f - }) a& w: n+ h F
- }" P# z3 n) B9 {! R
- 2 }+ U2 A' P& @3 F4 a% e
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
1 h" c$ L. E. p& {# D) \0 e& } - HIGH_level_read_time = micros();
) d; \% |( |% v5 _3 P6 j' g - LOW_level_read_time = micros() - LOW_level_read_time;" X4 S* X7 o3 X5 ~+ k2 a L
- 6 `: e+ t2 ^9 W' D* g! t
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
" M, A! b0 k) q - {% n0 L2 Q+ e( |. \7 c. C
- return 0xff;
/ c- l _% c/ H) N& Q9 d: i - }
9 @$ m4 \+ O/ N# Q( i+ y( d+ C - / r& A. a h9 w' h
- pinMode(_DataPin, OUTPUT);' j( ~ j- l' _% v2 |+ G
- digitalWrite(_DataPin, HIGH);
6 K( E7 i6 u, g: X6 ~, G' b - ' z6 C* P" o. V. j: g
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))- K9 T# [! W) o w& ^/ ^
- {/ x& B' U$ r" t' H) I0 Y1 n
- old_data = Sensor_Data[0];7 X. U- q) Z- x- n
- return Sensor_Data[0];
" P- R) R7 ?& F, \ - }
: Z0 p$ |. @$ Z; m - else/ `& M) w! j' N
- {
" v4 V& y( q2 C( x - return old_data;
. O6 j3 L% `& L* q: K - }3 O" ?: k5 y# c( `0 b
- }
1 _3 n! H4 h2 I# o
複製代碼 2 H! D0 M3 W% J2 r, i* U* b
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
, O; h1 H! E) i$ L
$ D5 Z/ c2 i+ x& y1 b6 k |
|