|
|
( [- b5 ~9 C- ^" | |7 M9 Y$ R- Z
- #include <Arduino.h>2 O! n6 P3 N# d: M8 T
- #include <MeAuriga.h>
- I1 k# ~( z" ^4 r" i( X9 A. _$ [, f - #include "MeLineFollowerArray.h"
0 T0 m8 H# |; h- M! m - % {' W5 o! X. |* g8 d1 @- U$ [* ~. J7 V
- MeEncoderOnBoard Encoder_1(SLOT1);: X" W# |% D1 ?& b
- MeEncoderOnBoard Encoder_2(SLOT2);
0 z2 R% Z1 U' p; X- ~$ Z - MeLightSensor lightsensor_1(12);
) j M7 P1 C. K& X! U6 b" Q: _ - MeLightSensor lightsensor_2(11);
- } R6 s$ p7 j# }! q, o7 b - MeBuzzer buzzer;# B4 ^2 f1 b2 K4 u8 i
- MeLineFollowerArray linefollower(PORT_6);
7 r2 n ~% _6 s% m. D) x - $ Z0 a% I7 Z+ M4 B" {
- #define Error1 1! A- M. Y. n9 Z5 @9 ~! l E
- #define Error2 24 g% m- ^* j+ [$ ?: K
- #define Error3 3
, t( i( a8 g1 {2 T9 s4 \0 a
' Y( W. p& ]: l- #define Kp 15
; `& z. U# {% J' o% D - #define Ki 0.15
- V" _2 v1 R3 N( [7 O2 A8 i3 H7 _ - #define Kd 0.03
/ u: m) c7 Q3 r1 n3 n, `% D - ( G( ]! _) Q! a; j9 r. K+ o* X. Z
- uint8_t sData;% J6 n3 }" x: k% Y/ w3 N, K; ?$ G
- uint8_t D1;) ]( n4 ]: j ~% f( ]
- uint8_t D2;
1 Y* j, y1 K, a - uint8_t D3;
, f& C/ d$ b( b2 z! @( h( e - uint8_t D4;1 M9 x4 i; u5 O( o9 k; M
- uint8_t D5;0 K2 h9 j# V6 G. q
- uint8_t D6;2 ?2 Y$ x$ o0 Q9 I6 ~
2 \9 m2 ^# M7 T$ ~3 m* f- float previous_error = 0;3 Q& Q9 c4 Z( ^" {9 f, C/ m8 @
- float integral = 0;
* g/ C+ _# v( a0 S: Z - float derivative = 0;
& [# J3 f- l; \' ~# K& P5 Y - int Speed = 160; W3 m" \/ k5 T/ ^0 C
- float output;$ K l8 b& D B4 E; V' ]! l
- - F! f7 ?# M0 _4 ^* {5 Y
- byte Left;5 N' Y6 u8 T, i9 {/ b! v0 S$ Y
- $ M8 _5 z! {2 c' V
- void setup()
6 }! U* g# H* J - {: O- U; L2 \0 Z
- //Set PWM 8KHz
7 q9 w% L% G) ~2 w( V - TCCR1A = _BV(WGM10);, X+ F0 Z0 z! i1 o
- TCCR1B = _BV(CS11) | _BV(WGM12);; x; ~ {% x h; s
- TCCR2A = _BV(WGM21) | _BV(WGM20);$ Q( K- U( R0 S! P: S1 o8 P
- TCCR2B = _BV(CS21);) H% s: o/ o; @2 k7 C( l
- Serial.begin(9600);
- Q0 x% D( [' ^- O/ A# n - buzzer.setpin(45);
+ b" A5 o) d6 w$ }8 }& F! K3 \& S - }' q% v; G" K, A
/ t5 T' ^9 l2 s1 }+ A2 Q3 o i- void loop()4 s& d6 e R! \* G3 z6 u$ }! ]1 |
- {5 N# y n, Z2 h( n/ _( F. `
- while(!((lightsensor_1.read()) < (10)));
7 S Z- n4 a: j2 e - buzzer.tone(1047, 500);: _2 c4 u+ Z/ U$ r& w4 V
- delay(500);
, T! H3 c0 t" Q" n D7 v* \. s3 i - do$ o# R& _" N; S3 Q
- {
9 A- B/ k+ l4 @1 ?+ f - int Err = getErr();
& K2 N0 k1 v! M) A2 i' u - if(D1 == 1)
7 y) Y, ?+ l T: n: X/ N; s - {
0 z$ E9 h2 F3 q$ C1 Q - Left = 1;, n3 Q$ J7 @6 B5 ]6 F+ `
- }3 D, u7 e' [; Q( V9 s
- if(D6 == 1)9 m" v& c4 R, O6 f! g6 c2 @
- {
& Q* Q4 F' E3 J; e/ d8 ~' ~$ B - Left = 0;
/ e8 o6 q% f* d; Q$ o) f; K# a8 q - }
4 z% X4 r8 R" S+ X5 R, \ - if(Err == 99)5 e: G9 {0 ^! ^8 W: z
- {
4 } n7 F- D- w - if(Left == 1)
6 }' ]# A! O6 _ f& Z* n3 s( A - {/ T- x5 r0 u$ q* p, H
- Speed -= 5;; c- ~3 y, Y; t3 S3 o
- moto(0,Speed);0 ^* g: a' d9 p! W8 b& S7 U
- do$ J% B5 ^! Z) |% n1 [# z1 w1 H
- {
3 d8 X7 P$ \9 D+ x* l! k1 J - Err = getErr();
: M$ j& Z0 @3 N - }while((D1+D6) == 0);
7 l- E: {' O; l - }
4 i; A- }0 |; N! q - else- d! K3 z3 v. i( y% G8 t- f4 G" X
- {. k. Z7 H# U9 ~1 N5 l3 c0 S! M
- Speed -= 5;
2 }) L6 h/ r1 o1 ]6 a1 I2 ^ - moto(Speed,0); 9 A: a- t$ r7 s" ~4 ?
- do
6 d" m- K5 F) |6 p - {
5 T( @. S6 R& v8 v" {; `( o" w - Err = getErr();
1 m! Y( g; V0 g6 ?' D3 K - }while((D1+D6) == 0);
$ z1 j1 F8 M* J6 ?9 W - }
$ V2 x5 S& ^3 E+ R6 X8 G8 ] - }$ Q g& e7 q* S1 i; \& x. k
- else" ~5 o8 u0 s( m$ S
- {7 L5 w. v9 u% [# S
- if((Speed < 160) & (Err < 2)) Speed+=1;
! t( d6 `' Z, }/ K" Q - if((Speed > 100) & (Err > 2)) Speed-=2;
1 f6 r, q4 w, I5 E1 h1 W - integral = integral + Err;
7 K' y( \! f1 w/ j: }- S - derivative = Err - previous_error;2 c9 Z% w6 B! h' _. M I1 G4 l2 t
- output = Kp*Err + Ki*integral + Kd*derivative;& E% d6 K0 ^8 U
- moto(int(Speed-output),int(Speed+output));+ {5 d: ^% Q" L/ ]1 `# `5 D8 a
- previous_error = Err;: x0 X+ U. U6 t% ^9 n5 G, p
- }6 a. B( Q: I9 ^0 |; m5 Q m0 e: \& d( \* X
- }while(!((lightsensor_2.read()) < (10)));. \7 Q# {$ q* O2 g/ a
- moto(0,0);; ~! b: I. K, ^1 M( L
- delay(500);
& `$ U9 M v, f0 S3 y - buzzer.tone(262, 500);
^0 X! B% ]6 U( X, ` - }8 v8 l0 z$ { j( c3 c
5 E3 u9 k u8 V! p; R- int getErr()) u& M( |2 Y' L: _) B; j: y
- { - A9 g3 K" X. s0 z) w3 ~0 b+ @; l
- sData = linefollower.getValue();
5 ~7 t- J/ M" {: S4 ?6 Y4 ^ - D1 = ~(sData>>0)&1;
- Z9 C u7 k) b4 L7 s1 m& l - D2 = ~(sData>>1)&1;9 {# ]: h D; ^8 E
- D3 = ~(sData>>2)&1;* D4 t/ i2 \' M! I0 c
- D4 = ~(sData>>3)&1;5 x4 Q! Q, m! k$ A3 c1 z" Y: x
- D5 = ~(sData>>4)&1;: \# e( ?2 c: q
- D6 = ~(sData>>5)&1;
; z! H b3 q# t( K) y# ` - int downD = D1+D2+D3+D4+D5+D6;
) K2 Z, W8 o8 Z& i - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);! u# E9 W4 ~8 Q* ?) P! ?
- if(downD == 0)2 H: m6 T4 q2 |# c1 h# B* K0 ^6 Y
- {
$ g3 E/ j; S7 i- {+ D" a, @, g - return 99;
- o& |5 V6 k' K; O: V8 x5 i - }2 J& W1 c! c1 q! Q
- else
9 P8 C; b' }+ b& k& G4 o/ X V4 N - {) y1 y a2 P( [4 B4 g6 H% l
- return upD/downD;
# W6 D8 d( E) o& w. I - }
# `$ @/ o/ Q% m( R/ @$ }; O4 r - }
1 n7 \1 a4 M7 s
) Z( _/ l7 Y" m- ]8 P" M+ C- void moto(int Speed_L,int Speed_R)1 j" ?7 I) O9 K" [# G( W! U; P8 L
- {: Y( @# R/ V4 w2 |2 z5 r
- Encoder_2.setMotorPwm(Speed_L);
. a$ X9 W/ Q9 h% ^( E% E, A - Encoder_1.setMotorPwm(-Speed_R);$ n- c& ~& c2 f
- }
複製代碼
( {$ r: b# \5 j# F5 K# h% k6 jMeLineFollowerArray.cpp' B* x7 `+ q+ x3 b' V3 _3 W
- #include "MeLineFollowerArray.h"
3 x* b3 }4 Q. Z( w0 l! n/ m7 \ - ; E1 B& R1 ^1 C
- #ifdef ME_PORT_DEFINED3 N; D/ g2 ?( }1 C1 R# [
- MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)8 L; f' l/ F8 ~, k# ?4 \! P- r2 K
- {) o, W" L7 {) x5 @6 z; q* k2 F
% J5 `" p2 [6 ]2 e/ f8 g- }
6 M x( H6 C( ^( a( X! Z: J - MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
/ F# l; h4 M2 k; R3 c" P; t, x - {
/ K7 P' u/ {4 A$ x8 z! a1 p - _DataPin = mePort[port].s2;/ _% z3 S4 ?4 M4 {% W
- pinMode(_DataPin, OUTPUT);
. z/ \! e z- I: [: D0 P - digitalWrite(_DataPin, HIGH);
: G; C2 X0 f0 [8 V8 m5 K - }1 i$ m4 }5 P6 o4 h
- #else // ME_PORT_DEFINED* b) Y) S# y. N
- MeLineFollowerArray::MeLineFollowerArray(uint8_t pin); \% i l) F, C8 N! h- x
- {9 q0 o3 F8 p# q$ P8 i" ~, `
- _DataPin = pin; m( O0 B1 y: z( T
- pinMode(_DataPin, OUTPUT);
9 v" S0 h' N. S" T9 K - digitalWrite(_DataPin, HIGH);; r4 i2 o/ F5 B& G) `: p
- }
/ W4 c }2 f( r! G1 E" p - #endif // ME_PORT_DEFINED6 e" V v& f3 U+ s+ g
7 R' [: l- v. Q/ _/ s
7 i4 @7 }; J" K7 G4 a- a- void MeLineFollowerArray::setpin(uint8_t pin)! r i7 J, B- p7 |! _/ P
- {# k2 G: A( v/ e! e# O* H# a
- _DataPin = pin;
- |( N7 R8 C9 C/ ?8 H - pinMode(_DataPin, OUTPUT);
8 y$ w7 C4 X, o" b - digitalWrite(_DataPin, HIGH);
3 Y; `* E& K" M! l" d- _) q) O
) i e8 e$ z G9 d2 I* s2 I- #ifdef ME_PORT_DEFINED
4 W/ }7 l# e, r" J1 X - s2 = pin;
; N a: q; M: [+ l* v: W9 O. f+ F, D - #endif a" U6 j8 t3 _
- }
; B$ e6 G8 C& b6 }) B8 {$ ]7 c
. ~! P2 ?! c: k! n! T6 x- uint8_t MeLineFollowerArray::getValue()" X/ Z! ?" k# ]- l* C% g
- {, W4 Q* i9 B: `# A5 X! N$ u
- uint32_t LOW_level_read_time;7 W8 r- }7 v: s2 m8 v2 V# s1 W
- uint32_t HIGH_level_read_time;1 C" G- Z* e L) A
- uint32_t time_out_flag;
# t& r2 Q/ q9 \! y# u - uint8_t Sensor_Data[3];) s& k; U/ S( B7 T, a e/ J
- static uint8_t old_data = 0xff;
0 S. A( H, k" U. ~1 a
4 [7 ?9 }! A/ U0 O- c/ y0 Y- pinMode(_DataPin, OUTPUT);* M6 l2 h( Y. Z8 w$ D& B% i; L
- digitalWrite(_DataPin, LOW);, l, Z% }) a! U$ d- w
- delayMicroseconds(980);; O% @! x6 J t$ C" q0 q3 f" V
- digitalWrite(_DataPin, HIGH);
% Y/ c& |" z# a
" C: P: L) p. @+ R) g1 ^( E1 E2 ?- pinMode(_DataPin, INPUT_PULLUP);
, i5 H; {2 [" W' C - delayMicroseconds(10);
' v6 ?2 u7 `. l( J# n
8 l9 j$ J; g5 B7 P% p7 u* ^3 D1 I- time_out_flag = millis();! _2 r+ x. \8 r% {
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );# y. O+ k" {" C- i* O
1 ?% Z$ x. j J B; }; ^- LOW_level_read_time = micros();; ^, ~ Z+ o& [7 p3 E/ \% D
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out6 m& v1 o4 v8 |4 @) g, I' ?1 [3 G
- {8 C8 m; {) Z3 J# l( j: s( ?
- return 0xff;
# s$ f* P; h& J( ?( { - }9 z6 P/ I+ C& s9 E V3 @, Y0 A
- X9 H/ L h8 a$ p
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
$ q2 H; F- E( ]7 u) |9 E" k) n9 @ - , @7 \& V9 t) M& F
- HIGH_level_read_time = micros();
, X# ~$ A+ W0 J' l5 W/ h - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level
( v9 S6 J" `' A6 b i9 x$ W - 1 a1 y: U2 N% A+ p. w2 E
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out9 T/ v) e# p; y' \5 U0 |! j* X
- {# }- V) {* w) H
- return 0xff;
( {3 p; p: _, }/ E* S7 T6 b - }
/ G, m. m0 M+ D' N+ |- p$ U - $ F2 i3 a6 z& z/ o
- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))
) N( `2 ]3 N8 H - {
- Y* s2 b. n( w# q" U - return 0xff;2 M7 `; f& g/ m# S8 T" W
- }# W# ~- n& }* q7 i
- ~% S4 ?7 W5 _# j
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );- Q. z: X) |; ]# e1 c
- LOW_level_read_time = micros();+ X$ D- G* Y" m, H- O
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level
4 s, ^1 [& u& y9 G$ a3 B$ B. F - ' l5 q1 V- H9 [9 C3 z+ o
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
# {6 v0 q' j( P' S2 e5 ~ - {: y8 K& {+ d- s& j
- return 0xff;3 ?; o! f: B" r4 ?
- }! P, I- E! _+ q* m5 v6 k- W6 D
h g$ J# d# E3 e3 P Z7 c- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))( ~# W t1 T' a3 a+ w5 s; U+ N
- {9 K* Q9 M6 |1 i/ L# ?5 S- s4 i
- return 0xff;: n1 v9 l- }! q% S4 s4 A/ L
- }6 q% y/ _3 l- M( p4 r
1 E8 y% E7 U( j( v" W: `- for(uint8_t k=0; k<3; k++)
% F3 z* b, j, m0 _4 k7 i; R - {
. j8 M, \% [. i" ^* S - Sensor_Data[k] = 0x00;6 M5 J/ w1 }0 [" X
- + m/ i1 }; V t3 P9 ?
- for(uint8_t i=0;i<8;i++)
5 `: L6 S' y& G [ - {
# V+ d2 T9 i1 E1 O* I% Q - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level. m: V9 a# T7 T0 o: }
- HIGH_level_read_time = micros();
1 L! H2 X- G' a7 c! @$ Q - LOW_level_read_time = micros() - LOW_level_read_time;
( Q% P5 s% y+ `# s
7 P4 t1 T' e! t& j- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )$ ~, v, j/ b3 M W% x) I2 o$ n
- {
. O. h8 \1 G# B3 D - return 0xff;1 h! X8 a3 w. G7 \! c/ G
- }
: D& O( C2 T0 }7 I - 2 P _8 |3 c* v$ U' r, H% o
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );+ W- O3 j2 W( E3 r
- LOW_level_read_time = micros();# s+ p7 E; P8 {2 u' f% \* A; z0 w
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level) n0 A% y O# Y
3 ]! R: g+ D, K- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1! q1 h' r+ Q, O3 l- }
- {
4 K$ Y6 _" Y& A( c, v1 _% K - Sensor_Data[k] |= (0x80 >> i);
# u2 e8 I. r# I! J; X6 m- H - }
4 K! G# w. M8 x; y - else if(HIGH_level_read_time >= 100)
, b; m. T6 z: A T2 C7 D - {3 f. d; c) H$ B' y% v4 h2 s
- return 0xff;
; [" @( ~% k* G8 o& ? - }( w. L! l. C c
- 6 s" |; \( s0 Q* g/ Z
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)( {8 w$ Y7 ^8 y- n d, _
- {+ i4 c+ I" V* Y0 d( {3 U5 @
- return 0xff;% C; A1 \& r1 E4 m" L( Y' K
- }
- l% d+ w, s @6 t2 G: k7 [$ c - }
& Y* Z& s' V, f( P3 K2 h. y7 \ - }" b+ W0 X+ E8 ]4 W8 R
- ~( c: t: T& {7 [1 T
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
; m4 F. _7 T; \) v9 W - HIGH_level_read_time = micros();
3 c5 H, T6 s4 X) k - LOW_level_read_time = micros() - LOW_level_read_time;
. |3 Y- ?1 A. v - $ h* O1 z Y9 l# h7 h1 l& q
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): A2 S6 F7 a7 ~% Q
- { O/ D! |8 v$ K) ?
- return 0xff;2 K5 ~& B) ]( a6 _) y- N0 x
- }
0 O8 L- x; u1 @ - 2 g# |- R7 k, {7 l
- pinMode(_DataPin, OUTPUT);$ w6 Z+ V0 U; s5 F0 s" ^
- digitalWrite(_DataPin, HIGH);
( G/ `+ C) o1 k: O$ q: ]& _" d
A4 z8 `/ X1 b- p6 @/ c% Y- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
@0 c$ m9 N) K8 u: S; e" ~ - {
6 S' p i+ o( k$ V& }' T - old_data = Sensor_Data[0];$ t0 z' E9 `% N9 L
- return Sensor_Data[0];
; o# w6 _) e) n - }
9 @0 L1 C: l/ ]1 V: b& D - else1 G: M; g( O- X; R4 }
- {9 U9 [7 W# \7 o( K: d: G6 S
- return old_data;4 X% M: ]$ o" ?, z; O# ]9 j
- }% H& z, B: e8 R
- }+ Q4 I+ A! F' g7 x! j( w$ S1 I
複製代碼 & y' H( V9 `5 E/ e& z2 a: g
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
' c/ e3 p0 c6 S% e( x) A8 }
( Y* m. N$ H' S1 @6 f |
|