|
|
! T# `" q) G( d$ F/ G
- #include <Arduino.h>
. I4 p2 w/ [0 }5 P& q: L - #include <MeAuriga.h>
! k- v5 p6 R- [! D9 h - #include "MeLineFollowerArray.h"- \9 J! d- q! `7 |5 h( I u
! D) S. s; J4 A i- MeEncoderOnBoard Encoder_1(SLOT1);. s, J4 Y. `: w1 T
- MeEncoderOnBoard Encoder_2(SLOT2);% l; x3 L0 F. P2 S) [4 n; y
- MeLightSensor lightsensor_1(12);9 {9 i! S, a$ P3 h
- MeLightSensor lightsensor_2(11);* t/ \% R8 ]: k
- MeBuzzer buzzer;0 n3 U+ A* a* {% J+ i% ^% Z
- MeLineFollowerArray linefollower(PORT_6);; p' _$ R, w* j1 [7 |5 P
- - o5 w" \# T5 z ?! ^2 v& x
- #define Error1 1$ H& I9 f0 P3 F4 F( t
- #define Error2 29 [! ~& b2 z$ Y+ {
- #define Error3 3
9 ]2 V. `2 |. ?1 ]. {2 e( s - ; K- s; |4 T: a
- #define Kp 15' t4 p" e4 v6 U% o; B5 z
- #define Ki 0.150 l8 {3 r, C( T% i
- #define Kd 0.035 L1 x0 ~/ a! R+ y9 Z
- - s0 Z1 L. g+ _# q2 B( X
- uint8_t sData;
# M' t/ o, a& X3 ? - uint8_t D1;
1 Y# S! _, p: ~2 ]5 o2 ^ - uint8_t D2;; ]% w0 Z* e3 f6 C. X% [, Z
- uint8_t D3;
2 H) `* q. @; b0 a - uint8_t D4;! V9 f/ I5 x# \9 h3 o( R. ~
- uint8_t D5;! v5 O3 W. n$ D+ ^7 v0 d( C9 ^( g
- uint8_t D6;
3 Y' j. |6 u; }" S - 5 q3 o; B% \# E0 `2 C3 ~' H
- float previous_error = 0;
& y6 o2 Z; B' J/ ]4 |! ^ - float integral = 0;
1 |9 M6 ^: G' h$ h4 v" Z - float derivative = 0;
) O- X) I3 O0 l: T% l( L/ c - int Speed = 160;
* v; B$ K5 T- W* Z$ v( A- @ - float output;3 Y" m7 {5 }1 J
- ) X, O- `- A$ b! H* h
- byte Left;3 B1 \1 w3 G6 x" a2 Q" l
- , p* `7 _2 p/ d8 y' n
- void setup() G; B& Y) L# u) _7 T8 Y! v# ]8 e
- {
) l0 \6 }9 F5 K+ V9 L1 W7 B - //Set PWM 8KHz8 ~4 S+ t" F4 X) S; v
- TCCR1A = _BV(WGM10);
( |* t/ u9 p% V" c8 _ - TCCR1B = _BV(CS11) | _BV(WGM12);3 ~! }2 n6 R3 O
- TCCR2A = _BV(WGM21) | _BV(WGM20);
2 {" s/ H+ u$ k% S* f& M' k5 i. r - TCCR2B = _BV(CS21);
+ N$ g+ C0 A$ N0 U2 G: z - Serial.begin(9600);# a- Y3 ]# S% e8 r! z, N
- buzzer.setpin(45);" a/ N" i7 X ~. ]+ Q
- }
4 _; V' G: ~5 e! U/ Y) D; G - & B b: `' z7 o3 L) h M
- void loop()& N+ W: ] d$ q+ A- B, \
- {
$ m* F0 o$ g4 V - while(!((lightsensor_1.read()) < (10)));. o! Z( `1 B# ]/ k6 U1 Y& r
- buzzer.tone(1047, 500);
/ T4 W1 l5 B6 ~% [ - delay(500);+ {) ^0 E' M+ i* S0 b
- do
7 E4 `( _! S& T - {
4 }2 |" R! N9 ~2 t3 ?5 X7 o5 ^. S - int Err = getErr();
5 l/ J% t4 h& T6 C - if(D1 == 1)- {* D1 E" U' J# K
- {8 [4 i- m A& q: T
- Left = 1;
0 B$ w2 V! H8 c# P n - }
! a: X- o2 d6 ^' B/ B - if(D6 == 1)
% {4 u5 l4 Q3 x( P& h" i5 G - {; x' u6 |) I1 b! H
- Left = 0;
6 o" l* I l1 J9 q! ]8 F; _ - }
) D# L: g8 W6 l' E# X9 n8 v+ |5 M - if(Err == 99)
/ T# `( `0 Z- o. ^5 N/ j8 { - {
9 P* s; n) S! B ]7 V" v8 Z1 t( L* ? - if(Left == 1)
" o* Y- T. E H. d2 K - {: M) q/ v. I$ d' k& K# J/ `
- Speed -= 5;
: B% G( y0 n! w: w - moto(0,Speed);. U% d4 ^/ i* x) ~4 L- T
- do
% t3 [& |" e: Z6 f' a- j$ q - {7 `) q( Z( J/ I
- Err = getErr();' z# ]: O) U2 l5 k8 L( s2 }6 y
- }while((D1+D6) == 0);7 c, d2 e" R6 C e4 _
- }
( w- L* I( I8 ]# V! h - else
' k N' I- V) h, ]5 Q7 n3 l& g - {" @) S2 `/ K% b% a; q
- Speed -= 5;- k& t7 u: t: z+ ^# D8 m0 G- [
- moto(Speed,0); $ e* I J) q9 `5 u4 d; ]
- do& h: v$ n( H4 n& P! s0 k
- {
0 ]2 G% b6 N M& @; s2 A1 a' t - Err = getErr();1 f1 B4 P( S ]0 {+ f6 g
- }while((D1+D6) == 0);$ K0 j& O! V1 ~. ~8 F: U
- }- \4 Z8 B8 f7 y) s# |3 Z2 q( v/ ~
- }. I7 j# d. |/ @ u3 d) v
- else
; \& k' n4 c- z - {
2 v; i! R1 A1 J, D - if((Speed < 160) & (Err < 2)) Speed+=1;
% ?% I" h6 D8 d! ]4 g - if((Speed > 100) & (Err > 2)) Speed-=2;
% q% |/ z$ Y" L( f, ]% R0 ? - integral = integral + Err; m+ [$ z" Q) T2 O1 c, z" z9 x, t
- derivative = Err - previous_error;5 o! p0 s" ~+ h1 L) T* w7 ]" i5 A
- output = Kp*Err + Ki*integral + Kd*derivative;5 h4 w1 Z5 b& I! e s5 u# ~
- moto(int(Speed-output),int(Speed+output));
6 F7 y& W# ], j - previous_error = Err;# l3 v( j. ]1 t- h( \# u9 g
- }
7 m2 J* s" K$ p; I/ r9 ?7 ] - }while(!((lightsensor_2.read()) < (10)));
$ ^0 G) M1 n8 s* r - moto(0,0);) W% L6 k/ j! j2 d0 e9 Y
- delay(500);) t8 ^$ c# Q" t6 t3 M b8 G$ w+ O/ c
- buzzer.tone(262, 500);
6 J; o" s6 d* _7 d( i - }0 p# F) \0 ]: R' \0 Y
1 j+ L$ n- l1 e5 D; d- int getErr()
( d& i: P2 S& Z' } - { $ s) m7 @( A+ `8 d# U" a! T
- sData = linefollower.getValue(); T6 B8 \8 j P3 c2 Y" f, N1 @
- D1 = ~(sData>>0)&1; i% ~' M/ F$ @' F2 ~: Z) K& K- Y
- D2 = ~(sData>>1)&1;" F3 I& k! R$ S O, B# Z
- D3 = ~(sData>>2)&1; M, v2 V- x% l: n
- D4 = ~(sData>>3)&1;7 a ?, d4 l: F! O
- D5 = ~(sData>>4)&1;+ {" X r+ I# P" i& A
- D6 = ~(sData>>5)&1;
" t$ k7 ]2 }! d1 ]2 M+ q - int downD = D1+D2+D3+D4+D5+D6;
; g" t* u- Y {# d0 @* _5 F - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);8 X3 W8 C, [( o) D2 U3 o
- if(downD == 0)+ \( e2 j! V! A# ^
- {( ?# }' d! q0 ]
- return 99;
1 U! x8 q$ D* Q - }
) K6 i# n5 j; _: ?! H, ^ - else: [& j9 U' V2 Q/ I! J: r
- {
U3 |- Q" i, a) s - return upD/downD;2 @ G6 s1 q* P* o
- }
! q c/ @5 @7 `0 m/ _/ u - }
5 U& m+ J2 } v
, u8 q, R4 W0 E, \/ k- void moto(int Speed_L,int Speed_R)
. `+ Q4 }4 y- q* }5 x- g - {
5 q$ l( ^" m! x" p - Encoder_2.setMotorPwm(Speed_L);
J, d& m |# x) S L - Encoder_1.setMotorPwm(-Speed_R);
! K0 t! b/ p6 @) r. r$ ^* F - }
複製代碼
& D6 _) L* B9 ]. O0 YMeLineFollowerArray.cpp
* O+ \2 h% T ^. S0 _4 v- #include "MeLineFollowerArray.h"
; }# n; Y; B0 j( {8 r
' i& @* m# e$ X1 ^ m* j- #ifdef ME_PORT_DEFINED
" V6 m. {1 X5 Z+ M% h( e5 Q. V1 F - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
; K6 a. ?& |0 j5 F! O3 v0 v5 Y) K - {/ @' a) L) B2 r' N4 z; f i
- + y$ ^: Z0 G" z0 _* b6 m
- }, l; j. Y& ?, j9 H. e7 j' C
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)1 j7 V2 o$ ]9 n; X' m
- {! \* U# P% I5 p( e+ ?6 [; p0 u
- _DataPin = mePort[port].s2;
5 Z0 r+ d8 |: t! y. g/ F - pinMode(_DataPin, OUTPUT); : ~( R: _- g$ O- H a
- digitalWrite(_DataPin, HIGH);0 N/ S) B5 r" b+ K, _# Z
- }) a2 P! ^& \1 i/ L
- #else // ME_PORT_DEFINED8 n, g" A' y" g- A7 h4 Y& h5 D; N+ l
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
* n4 i8 E* }2 w, R" O* R6 J - {, q+ W# l! ?! w; T1 ^3 A2 r7 t, f
- _DataPin = pin;' J- Q2 f' Y0 Z+ h6 l( E$ E
- pinMode(_DataPin, OUTPUT); % G0 @% p1 t" n2 a% x
- digitalWrite(_DataPin, HIGH);7 h! c7 ^, s, ]' G
- }: U8 z3 \6 Y- j, P' s: Q& b- t% [
- #endif // ME_PORT_DEFINED
) t' `" {; ?5 _- w1 k% j
$ {( L. t4 r% e0 y, H- " Q+ U* W' M1 ?- I
- void MeLineFollowerArray::setpin(uint8_t pin)
7 Y2 |3 e6 T/ b - {
9 r J( g- F" q9 H5 B - _DataPin = pin;! p. ?& d @; |0 i+ p1 x9 `
- pinMode(_DataPin, OUTPUT);
+ r+ @7 m1 A5 [5 V0 D0 A" p( s/ @5 { - digitalWrite(_DataPin, HIGH);+ f$ W5 M1 |; S7 c) N
/ R! h3 [1 F$ y Y- #ifdef ME_PORT_DEFINED
. o" h; l' {: G; p$ S/ l - s2 = pin;3 d9 \& Z A. c; x5 X- j
- #endif
* f4 i# N2 y; p% D; z; Q - }
# I3 R: e- Q6 S1 U5 v' u& j8 R - 7 u) w8 t. n# u# C
- uint8_t MeLineFollowerArray::getValue()- a! c3 X# P, o- S3 J8 \6 J/ u
- {
& R/ Z' @2 C3 C- H6 n - uint32_t LOW_level_read_time;
5 H- Z! ^8 H& [, j - uint32_t HIGH_level_read_time;) L' s# j L1 l. h- ~9 X
- uint32_t time_out_flag;" u9 R# N/ R, a. \; r
- uint8_t Sensor_Data[3];7 L: k7 P! S$ a' R- b2 ~
- static uint8_t old_data = 0xff;1 d! J" \2 m) |5 {; A0 ~2 a& C
& e5 n- m( v* v W- pinMode(_DataPin, OUTPUT);8 y. F9 K8 n |: W3 Y/ c* a4 Y
- digitalWrite(_DataPin, LOW);$ g3 y) V r) e3 H# W/ Y3 }5 ?
- delayMicroseconds(980);9 b/ t3 Z/ U) D9 g8 |) @* t
- digitalWrite(_DataPin, HIGH);
, t* ^6 B3 p# B! Z& ]/ f5 Y8 Z
8 H$ y! V& P- z4 W8 M8 y- pinMode(_DataPin, INPUT_PULLUP);
9 n( {) b, |- ~( ^, `" \6 s - delayMicroseconds(10);& U6 g) Q7 Q6 ~( H- u
8 x3 ~* m+ c, j, Y# t: Z0 q/ r- time_out_flag = millis();
( X+ W: h8 |+ I! K% D1 C3 d - while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );$ z, n# C% J. {- [
- - @1 x& g J9 ]8 K% @& [; p
- LOW_level_read_time = micros();
' }- R5 e9 J6 b7 s( l/ a - if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
3 S. y% e$ k5 l i6 a - {
% f4 D# n( I4 f! i - return 0xff;. R$ X3 i# `) K" T9 q& z
- }8 U/ _( e6 i3 n% g1 L ]
- , p- x0 s4 d) u6 h8 j' [* Y3 O
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
9 a9 P8 [ _# m* t) r
/ u6 \9 B( q9 P0 c; I' S* {; L- HIGH_level_read_time = micros();* H! L+ l' o3 d, i" A' V! b* G
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
1 x1 j" k% R% i
4 |- m4 V4 r, p3 c& t0 F- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
) |; X! M+ |! \- p5 M5 `) ]* x - {
8 X# h" V. r/ o - return 0xff;8 ?2 Y4 k( ]" Y1 S( {1 f# j
- }# O; q D" S$ A: J
- ! }- S! I) M0 E+ |* k
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
7 m% ~5 p% ~( q5 \" N2 p - {/ s6 D5 E* ?5 c% l8 w
- return 0xff;) \2 J8 w6 G# n* s
- }+ W( [- w4 w2 l' S u1 R2 Z" I2 N
2 H& M9 y' k9 |0 }/ Q- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );2 n" S0 q: f; t, D6 p3 k! E
- LOW_level_read_time = micros();
4 S' A1 @$ R: I) f - HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
# W/ z2 {, y/ W, g( f- s8 ^
: q: R4 u: r& E& Q) W- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
& _$ l. ?! t% `0 E/ S - {0 l& e8 E. |5 u( n8 G
- return 0xff;0 a$ t5 }" c8 V2 k# q& E0 V# s# L
- }5 A1 z6 p6 @2 o# b
- 7 A; b2 w, ]6 v* Y' `' N
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))' p+ D3 U7 N6 O* N4 m! C0 V
- {
' _& F+ \3 J& P2 l: k; m - return 0xff;
! z# R3 L. ]$ o7 ~# G3 f" I* p% _ - }' x2 b& B7 k7 r
- 1 i7 t' T% _% {" Z
- for(uint8_t k=0; k<3; k++)
0 y6 m3 q$ S: i8 x4 d6 `7 u - {6 x! B, `7 {( g, R
- Sensor_Data[k] = 0x00;
0 O; {/ Z- B o' ~& y4 d& I
+ O6 t, M* m/ L" R* X) X* M- for(uint8_t i=0;i<8;i++)4 p6 ?: p$ _/ A9 v) t& b2 V
- {9 d% H$ F ~7 o' L) @
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
6 _+ X U' Y( v+ ] m3 [) Z. M - HIGH_level_read_time = micros();2 y' u% p! |( B4 p
- LOW_level_read_time = micros() - LOW_level_read_time;
# c! O, k! v) R$ i$ w- K - 1 P' d4 w8 y7 K
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
9 C6 L* Q i c6 Q+ g& x! ^ - {
. J# {0 x) t* S8 K+ [& j% o - return 0xff;
# S6 O# U J4 I: \$ R - }) e: m/ M) E# g% I
( n$ s9 b; W8 v- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );' U( N5 j' c7 |" {
- LOW_level_read_time = micros();
; T: A; \8 L- d3 w/ H! | - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level
% l8 N& y4 Q3 z, {* K6 x4 B$ x - / j/ U5 n: _4 B( u" ?. Z+ Z
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 15 C4 B; X& Y$ O* j4 b) `4 y
- {
4 a4 y1 o* U Y - Sensor_Data[k] |= (0x80 >> i);) j. @! e, D. j. J. j$ z1 [9 j2 w' w, b
- }0 l% d4 l( h$ n1 Y! } k
- else if(HIGH_level_read_time >= 100)
* B. w3 d/ n0 Y1 H8 { - {
, \7 F8 Q, w/ z, n, t) J - return 0xff;
2 s* V- L! h$ m, d! m - }" g3 Z( M# e, F9 V4 W
- ! P4 c( k9 @7 h/ a9 F( z7 F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) I, V9 L: Z% c# [: X; ?% v
- {
: `& e% K( e- C - return 0xff;
- v! O! ^1 [+ e5 q$ L - }
" j( V; O; [: K3 }5 o% b - }
) i% Z* F `- ~. g! A: q; c - }
. ~, s- z( f) W+ [
" ]* [2 |: E( @- p! E3 \& {- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level" d+ x" Y+ {0 x0 M g
- HIGH_level_read_time = micros();8 p, Q1 P1 x5 e& v
- LOW_level_read_time = micros() - LOW_level_read_time;
0 t: a4 P4 T$ ^* m# P0 k8 ^
' }! k$ m2 C/ \1 F- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )) W$ T3 J- C) k7 x
- {
- e: W# V) v& O1 F- {' V4 B - return 0xff;
- M5 `$ \ O, W+ P - }. r6 h+ L: X5 K* D2 L0 z
- 0 Q Y5 q$ S; a9 d
- pinMode(_DataPin, OUTPUT);+ t( G4 }' R( i& k' K
- digitalWrite(_DataPin, HIGH);
9 _8 H- b2 E( j5 ~* m# s- I n5 ] - o7 E* I7 G: Z) L% W5 O
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
& Z% r7 S. o+ U+ R$ f9 R# G3 t - {% L9 C6 c/ D0 k/ `4 W7 L M
- old_data = Sensor_Data[0];
4 f! u8 b5 q# n; y, f - return Sensor_Data[0];
& j' x/ _4 }( T - }
4 r8 w- L8 t$ A - else
" c. p% M0 V5 |( ^: P, m1 l - {. r# b) C' J+ d* R* m
- return old_data;! b9 r/ X/ q8 \1 u; G
- }' n$ |2 V8 V! U0 w" v- p
- }2 ?- y) a1 w$ Y5 s& y. a0 o
複製代碼
$ z* l. o% f% c. OMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
o7 I& B4 N% c3 t
8 ~7 i; e* D: W$ O; I, u |
|