|
|
# X1 Y F- R& B! M- #include <Arduino.h>
/ T0 N* x- M& | - #include <MeAuriga.h>. d0 m6 Z3 l- i2 }; R
- #include "MeLineFollowerArray.h"
6 f8 g9 `8 Y' Y - 0 v Y- B/ R9 ? ?/ ~, a
- MeEncoderOnBoard Encoder_1(SLOT1);6 o. i- F5 ]+ @' o
- MeEncoderOnBoard Encoder_2(SLOT2);
& R4 ~/ L& l$ d0 g) k: Z - MeLightSensor lightsensor_1(12);
T; S2 H4 B+ M* I2 F! L' k" M - MeLightSensor lightsensor_2(11);
1 P1 B- u8 l7 I% R" f( P( L - MeBuzzer buzzer;
9 e. Z* f" r' u' [7 l+ j' _ - MeLineFollowerArray linefollower(PORT_6);
, v; i$ ~4 Z: D8 @7 ] - 7 `; t+ s" E1 ~; u
- #define Error1 1) g8 v" F, Y5 o5 Y7 M3 w: {+ d% v( L
- #define Error2 2% q" K9 r% n- o1 N
- #define Error3 3/ x) i5 l1 i; r6 N" Q
+ c9 q m( R4 Z- #define Kp 15
* v. q M% A7 e: k: b7 t - #define Ki 0.154 [ I5 u4 d; \& {; F; [+ C, o
- #define Kd 0.03
4 _( \2 d: t0 k
3 T# P% Y9 Y, c- uint8_t sData;
6 {+ ]) I% e! e8 A- { - uint8_t D1;
1 q/ V6 g6 L- ]3 ^' h. Z - uint8_t D2;' Q4 k" U! u# P
- uint8_t D3;6 G& x2 `3 n' t" K5 U c6 @2 d9 P1 u
- uint8_t D4;% [1 h2 h$ ^& Q( h3 d7 }
- uint8_t D5;- s8 `9 \0 Y6 ]
- uint8_t D6;
' F! |3 C" \8 j8 N6 i& ]
9 D/ Y! {( g( U7 f- s* W- float previous_error = 0;5 ~- ^0 Z2 d7 H5 Z! O" l
- float integral = 0;
_9 j9 i1 z, ?! ~9 X! | - float derivative = 0;
# F0 D% ^) @) A6 J, `1 z - int Speed = 160;
, W4 h# ]% M6 }$ ` - float output;5 U9 h" h0 B- p* @2 h% K( c
+ u/ }. j3 R/ v- byte Left;
& G0 E( o( H, Y9 e" G- o' ~
& ^! D! o' ~8 T; y- V1 X* K- void setup() 7 g! i. X6 K# v
- {
O; z9 Q& L3 i) | - //Set PWM 8KHz6 C3 V! u" w3 A/ [# ~8 M) s( E& h
- TCCR1A = _BV(WGM10);
4 p. O7 x/ Z% f% {% O - TCCR1B = _BV(CS11) | _BV(WGM12);* V8 C' a2 b/ t W: v; t8 y
- TCCR2A = _BV(WGM21) | _BV(WGM20);
B% j' d6 O) v% I" C$ {) h6 ? - TCCR2B = _BV(CS21);
, w% {" e2 A; @6 `# p - Serial.begin(9600);" X+ U# g- u+ K# m8 }( I/ U& Q& O
- buzzer.setpin(45);
* m/ Q* i' g9 d1 H6 S2 ~' E - }
7 X w; T- A' |& I. L h
/ W) \2 K5 d: a- void loop()
9 x3 Y$ J1 q4 p+ O$ R - { S U. F! b- X* ^$ I
- while(!((lightsensor_1.read()) < (10)));
. V: j) o2 b# A - buzzer.tone(1047, 500);3 f. H+ N% v ^+ N
- delay(500);. C9 O2 [% A- \0 k' k' B7 N
- do; h8 ]% L3 N) U4 z/ _
- {
1 A: T" @/ w9 g0 N9 H2 N6 z - int Err = getErr();& ?9 l& w. D t: |
- if(D1 == 1)
f' o: [( W F* j9 N2 f$ ^, W) Q - {
! o" H( }& _2 ?3 ?0 A; Q - Left = 1;
4 `+ n9 C# c/ |5 X; o - }
k7 P1 K9 _2 S3 e: d& L - if(D6 == 1)
" S8 {% A0 B9 n) s% ` - {
5 z, p; e" B r+ m- \ - Left = 0;
; Y5 y4 P' v$ b* \ - }
) M# r- B! e+ J1 p0 K$ b! s - if(Err == 99)5 D f) F8 I6 [2 k5 X6 e" Q1 Z4 z4 [
- {2 Q' Y0 _/ K( n
- if(Left == 1)
+ k% ` y+ b) h- B. O! u4 G - {! c/ Y: P* g* J& O8 t% a; v' |
- Speed -= 5;
$ @. {0 X2 ?- ]" T6 `1 W" e - moto(0,Speed);
% x5 G3 W: @7 w! F - do' [+ U% X5 k: v/ ]8 |- q. [
- {
3 V+ x R1 d) I1 `6 I' E- R! U - Err = getErr();
6 S0 H3 h, U& L/ b: k: Z* F - }while((D1+D6) == 0);
+ L6 K& R2 U V - }
: C( g0 y1 {- e+ a+ ?. w/ Y2 O - else
8 n* z$ R; w( Q - {
! d9 N) O' b0 g" k! } - Speed -= 5;
& ?5 b% q) Y" ?" x' h# K. }9 u - moto(Speed,0);
/ l) |, r+ Y9 C* u* q+ _4 B9 N - do/ d0 D3 [ Y% _7 q. M/ f: J
- {
. [, q3 E; L! A/ a8 X5 I - Err = getErr();6 X6 q3 R( w: U- I
- }while((D1+D6) == 0);
3 A! T: _. U5 R* t. ?/ B* m - }
r0 H, P1 G9 N, V, G: L2 m' v - }0 {* E; E2 f& F- l
- else. R% d& \ L+ {7 f- A/ H3 v6 E" g) N
- {& @! a- H2 O) c/ H
- if((Speed < 160) & (Err < 2)) Speed+=1;
9 W+ P: b! k5 f) k1 ~1 P" H, ~( a+ K - if((Speed > 100) & (Err > 2)) Speed-=2;
C0 ]" A# {$ W( a - integral = integral + Err;
) z- p n, p" H - derivative = Err - previous_error;. n/ }4 l2 w$ W2 B# x- _
- output = Kp*Err + Ki*integral + Kd*derivative;
9 o; r5 E$ @4 l: K$ B7 h/ {& H' J - moto(int(Speed-output),int(Speed+output));
' M- z+ g' b5 y - previous_error = Err;
/ W' s- n* n V$ S% l, @, ?- B* v5 ^& j - }
2 d- L9 i1 O# `, k - }while(!((lightsensor_2.read()) < (10)));
( ^ D; p% S c5 [; ]7 M! h - moto(0,0);
0 U2 z( l1 e4 @6 @/ e - delay(500);
# z: E9 D" S: a - buzzer.tone(262, 500);
* ~7 s7 U8 U% ~ u - }
& b2 y$ D: J, X% f4 n6 ~ - , _. [4 p' F' f- U; a
- int getErr()
( K! x. I7 W; C) J - { / r% { x% N2 N2 N# @" b8 {
- sData = linefollower.getValue();
# v3 G7 T5 u3 J/ Z; N4 w& C - D1 = ~(sData>>0)&1;
; S- G$ S% w9 w0 D - D2 = ~(sData>>1)&1;
/ S( d7 u3 N! U+ T' W8 M1 M3 V - D3 = ~(sData>>2)&1;; e, o2 J% G" _7 l: u, O: M
- D4 = ~(sData>>3)&1;
/ g1 x3 w4 \+ X5 ?3 Q8 G - D5 = ~(sData>>4)&1;
5 Y7 h3 ? S0 T1 p) f6 K& {5 w - D6 = ~(sData>>5)&1;
# W- o/ o, N$ E9 J3 [ - int downD = D1+D2+D3+D4+D5+D6;3 X5 g2 y1 @0 V
- int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
$ K: q) }* T$ f8 d+ }) S - if(downD == 0)3 g, N$ K" ^% p9 `& n- C
- {
" v, o# `1 O( e! J$ { - return 99;
9 R" J: F1 l0 ?% F) `$ Y, G8 m* Z; { - }# ]" t; w7 U3 ^& b
- else
3 v; m* _/ v% R! ^2 k f5 q/ `5 d - {$ P: t( \% ]% E* G1 Y$ a5 f( b! z
- return upD/downD;
7 g3 f, `1 W0 `, T j$ m9 ? - }* z' f. R# e/ Y4 \) |0 U/ D
- }% b% ~, z7 T0 p5 m+ P* }# s. ~! E% _
- 6 K( i6 D( V( A8 L1 y
- void moto(int Speed_L,int Speed_R)
' g& C+ U9 j- ` \4 I8 o - {
; l( {: b2 G1 C4 h$ \& k' E2 `6 | - Encoder_2.setMotorPwm(Speed_L);
d! M: I2 _6 l6 z& O. ` - Encoder_1.setMotorPwm(-Speed_R);$ _/ U$ ^ N6 _1 v8 m1 y* W
- }
複製代碼 8 A( p; k5 x; B4 q) p! B
MeLineFollowerArray.cpp
: |! l8 e- V6 |- #include "MeLineFollowerArray.h"6 e% v6 F1 a4 S) R; r: q
% p' W4 p$ a3 B, C# q( a- #ifdef ME_PORT_DEFINED
" f" j1 a; y) c - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
+ W1 p. K9 v1 D0 a% v2 [& ?$ o - {9 x7 \: p' C" h! W6 o
- " }* C5 Y" M+ y- J {0 e
- }% ?: n- C/ [( N+ m4 h5 F
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
\" n" u7 K5 \% y' i - {
7 Q9 q2 y$ @4 t9 I# a0 ^6 f" X - _DataPin = mePort[port].s2;
1 p! `( D7 W4 x+ d. y3 s - pinMode(_DataPin, OUTPUT);
z, w# R: @9 \ t: l4 Q% F - digitalWrite(_DataPin, HIGH);9 g5 f4 H1 }, P, e [5 D7 P
- }
5 `% B! R, @$ ]. n5 l0 u+ H0 l. v - #else // ME_PORT_DEFINED
3 E8 t+ }9 X6 M, `# K - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)
( S- F% H/ z+ W - {
# ^2 F) k9 c3 m% b% E4 ?1 h2 A - _DataPin = pin;
& B. E2 b: h3 J8 K# k7 l - pinMode(_DataPin, OUTPUT); ; _: a2 |- v8 V& u* L5 X' r
- digitalWrite(_DataPin, HIGH);* `4 i; u1 M6 t# L+ ~, R* ^6 y
- }9 n$ e% a0 }3 N+ J8 U' v* @
- #endif // ME_PORT_DEFINED# r2 o# [* J' P; U2 A
7 _; b+ @$ r! I( |* C; F/ j- , C) y; y) X: ?1 l& ?8 V
- void MeLineFollowerArray::setpin(uint8_t pin)* m. L% @1 @8 F) W! |: P' V2 x) {
- {9 h. S6 v1 |4 @1 C
- _DataPin = pin;
$ I z0 f2 Z0 |0 ]+ Y7 ~ - pinMode(_DataPin, OUTPUT);
0 {. C# K: }+ l/ S) W - digitalWrite(_DataPin, HIGH);! @" `4 Q# r9 A- b! W: m6 E
, Q [+ J. H# l: q8 ?# l& h- #ifdef ME_PORT_DEFINED# e* D9 a( \ ~
- s2 = pin; e9 ?: @0 G; W0 o. {8 H
- #endif
[' {/ t9 T5 W: K Q; y1 D5 g4 V - }1 S/ z) }' J( s: E4 h; H
: ?6 m1 Q/ p/ x: p- uint8_t MeLineFollowerArray::getValue()3 D% |3 r2 f7 k5 c8 l
- {
3 ?7 } L, A$ |2 j1 K9 T7 E - uint32_t LOW_level_read_time;# I0 P& J0 W8 z6 g" |
- uint32_t HIGH_level_read_time;. I/ i# Q: j# x: U6 s% r/ n
- uint32_t time_out_flag;
: |8 @6 |- h r0 s5 r$ s9 U - uint8_t Sensor_Data[3];
& [8 y! r9 Q! w- S+ x) H - static uint8_t old_data = 0xff;
. [" B8 H9 M% o: j9 d3 O5 t
( b: O" s+ T1 y" J- pinMode(_DataPin, OUTPUT);
( S* I0 }. l+ t2 C - digitalWrite(_DataPin, LOW);# ^6 d6 j) a2 B% E3 G( V! R
- delayMicroseconds(980);& T" G& B3 l, M$ Z/ O
- digitalWrite(_DataPin, HIGH);3 W* Q; m# |& f1 S0 S
- " P7 ]( N6 y% }& N C0 s
- pinMode(_DataPin, INPUT_PULLUP);& {6 s9 w8 R; E0 n' x' G a8 k
- delayMicroseconds(10);) X4 F3 z j4 [# l( \# L, h+ P
- * X' i1 t. O( S5 k+ e2 n2 V% Y1 Y: |
- time_out_flag = millis();3 q- C0 h$ Q w7 U* t
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
. [# v( S. O, x - / a/ ]* L+ P; h; o
- LOW_level_read_time = micros();/ J7 y) M- O4 K# @
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
" Q/ E- m2 n% d - {
. U; |1 i; r" C - return 0xff;
1 [- [" S( i" w( W% p - }; d0 w" x4 f4 i
- 7 J( P3 t# C2 y! H" q
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );& t! N1 @7 Y3 q- L, x
5 y: I U0 S% R- i- HIGH_level_read_time = micros();
( N2 H8 a( E0 Z( I3 k1 N - LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level; \& @" |2 }! ?: o9 U1 {% Z
- # G) r7 _2 C: x
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out5 w! ^1 X& i& S$ l" H `
- {! N: q7 L0 a4 O( O7 e! ?: y
- return 0xff;
- ]7 d1 P: R$ M2 \6 a - }
6 d! d: H1 t* Y" g
- m: I& r1 \1 r2 T- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))* D3 r; i9 x( R3 \ w
- {
; N+ }& ]: G/ z$ ] Q - return 0xff;8 a, E4 X* G8 Z" o/ I
- }
) u/ `8 \( L/ l6 h5 ]. x- ]+ _( U
+ n, R( H3 m4 k- o- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );
- w( j b3 n; H a E& B - LOW_level_read_time = micros();4 [5 y+ I' [) ?. p9 E% T/ q/ [* }
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level8 L2 ^; y0 }) O5 A0 d5 u( u
: G4 o9 N1 S$ w! y2 y- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out" Q- C4 @/ p$ @$ ~
- {) l. Y* K" c- |0 z" h
- return 0xff;* E5 L$ `: V$ _+ Q* N' p& ]
- }5 V3 O+ ^' C0 V: B, w
- 8 d; M. b6 C9 u' d3 p j0 u
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))
- P9 Z$ M8 R& O1 s! B5 H9 A - {
7 h1 a, i# j7 a' P6 x - return 0xff;" f/ M4 w* r, @9 m
- }
7 i% u8 W% Z7 \ - & s6 z; J" [) B
- for(uint8_t k=0; k<3; k++)
+ t( ^4 @, w) [: F3 y) I - {
7 ^* Y$ G' ~. D - Sensor_Data[k] = 0x00;
6 q3 a" D5 k! ?# N( j! Y( v
3 ]. L6 \8 \( M1 C# g( X& ^* ^- for(uint8_t i=0;i<8;i++), l: a" d7 Z, t$ Z+ h9 b, t" g
- {2 r6 X% Q6 \8 u; N- j
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level. X9 U5 Z8 x/ l; F6 N4 }
- HIGH_level_read_time = micros();- o$ \: a( W. R- A( V- I% _) L7 L
- LOW_level_read_time = micros() - LOW_level_read_time;
% a* h4 z+ i4 f: h l
% c& W9 x9 E% b! C! c4 S. A5 m- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
! k% L2 G8 j( z4 j - {
+ t1 h; c6 ?2 z- h+ n; L - return 0xff;
5 h: `+ f& V! i$ r6 `' \9 a - }
1 e# w6 J/ v: v8 `, R - ' E! h" ~1 |4 ~; S& X; h
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );8 X" U2 F5 s: m7 O8 \
- LOW_level_read_time = micros();$ j! Y2 b( R, r" D7 `
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level/ J; L0 z. ?% h, i3 @4 u4 i: T
- $ K- E7 J5 d# v) \+ \/ ~
- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 1- R2 f6 c1 ? X5 s2 c5 u) ]: ~! R
- {0 R) s$ y* N0 H# j! X3 R
- Sensor_Data[k] |= (0x80 >> i);4 }) F& x' h; }( k6 ]9 E, F% t
- }& X1 H$ k, C' I0 R1 K
- else if(HIGH_level_read_time >= 100)
; O3 o( v6 l3 b! M; Y/ H - {
, X; W. Z8 m6 @* v# R$ r+ ` - return 0xff;
! t6 N# D2 U4 p0 Y' m9 X o - }/ z- k% p1 Z* a6 L7 T- @
- J; l0 M! N; p: i: U- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out): z' A! g+ H) o# N. y7 s
- {
$ D) ^3 N1 W( n, K - return 0xff;; Z0 I4 J D! \8 s0 d
- }# M' d' G; k2 X/ N& h5 j# L8 b
- }
0 M7 v8 E( `$ r" Q7 q! q2 j4 \/ g - }
$ {' @# |1 O0 n+ h/ [: b; t - 8 a* A3 i7 b3 z7 O8 I- c
- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level
7 I* [ [5 \( P0 [ - HIGH_level_read_time = micros();
- c) e" }) F) `* n$ h - LOW_level_read_time = micros() - LOW_level_read_time;
5 d1 j$ v+ W$ r+ f5 }/ V - 1 j# ^5 R. o% e+ v* Q# u6 @
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )
7 w+ \ C0 _, [ - {
' p3 O, j. K7 \6 ^ X- g - return 0xff;
4 ]8 G$ Y' q8 I$ G+ L$ \ - }* ?( ^' J( J: V e1 e2 A6 }* x
! m" ~) w# {* R- pinMode(_DataPin, OUTPUT);# I: E9 o- X7 {; Y
- digitalWrite(_DataPin, HIGH);9 {4 \# t9 e q5 v( }
6 W4 A& e- I6 M3 ~' k4 q$ H- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0])))
$ d5 Q4 ^* e& V - {
) c% Z# B0 K7 b - old_data = Sensor_Data[0];
4 d! i6 ~' \. c: a; K - return Sensor_Data[0];4 ]7 y M/ S$ h
- }
! d9 L) g$ V3 l* ?$ h" J, M - else
$ ?( w6 e1 J; W! @0 _. ^1 G - {. ~: Z& R6 X+ {8 k
- return old_data;
6 b& ^+ [2 b/ M2 r, x- l - }) B2 B! c. ^9 b& i, @
- }8 s1 l8 |5 ]/ P; h6 Z5 h
複製代碼
* b, E4 G U7 X( K$ TMeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
) x1 i( Y# }/ \' y1 ]
0 [1 t9 w2 Q( L( V" b& K
|
|