|
) F! |" e6 P4 N% M
- #include <Arduino.h>
- t3 L$ _0 w- \9 A" ~ - #include <MeAuriga.h>4 d" u9 t- J+ p" Y7 G
- #include "MeLineFollowerArray.h"6 c+ ]( a) _$ F4 Y
1 m L/ ~# o* ^. H) \( i- MeEncoderOnBoard Encoder_1(SLOT1);
8 l e; w5 Y7 P) K) P, H - MeEncoderOnBoard Encoder_2(SLOT2);
9 y! \$ U; y, |( ?, A$ I( ]& k, _ - MeLightSensor lightsensor_1(12);
2 Z( ]6 {, x8 n) I3 k# u - MeLightSensor lightsensor_2(11);
8 M( ^5 b/ F' a3 z6 \ - MeBuzzer buzzer;: F7 s: [3 n9 q# d
- MeLineFollowerArray linefollower(PORT_6);
( y+ {. [) I" s4 W1 c - : m: _7 l7 G% q) t
- #define Error1 1
4 P# R. t9 o7 m; j4 p7 ?' n - #define Error2 2
: K n7 O1 A3 g! |+ g - #define Error3 38 }+ ~5 Q& ?, |2 m( g* H
- , I' [" Y1 Q1 G$ A/ v v# @* P3 K! |
- #define Kp 15
4 V7 i9 f0 }. Y% [# z+ L# s9 E5 A& T - #define Ki 0.15$ z* Q4 q; _2 s2 q9 e/ E
- #define Kd 0.03
. J6 x+ V' `2 g6 n* i0 @- }* j - " G# o; n! h: H( F g
- uint8_t sData;# j M- N, m7 h! l! ?* Y
- uint8_t D1;0 I9 t& g5 k9 `8 A5 v" W4 T* w, I
- uint8_t D2;
! ?% W1 y& M; z9 P' d - uint8_t D3;
% ^$ m2 i( L: _: c2 n0 M1 Z6 g - uint8_t D4;6 ]8 y+ E5 h# d( n' ~+ u; t
- uint8_t D5;; ?( ^6 |& m9 U3 m0 k5 g1 A$ Q, I
- uint8_t D6;9 K* \( \3 F4 C+ ?' J
- 3 |- B- Q$ k3 n; r% a
- float previous_error = 0;
7 h, B* l$ x6 ^; d - float integral = 0;
- U8 m' a' J( O3 H - float derivative = 0;
- [: k0 ]5 n# _/ {3 V8 w% [" s - int Speed = 160;
9 Q. _0 i& _9 J7 }" z" s8 ~ - float output;3 @7 a2 z- n+ }& T+ a! ]) I4 U
- 6 k& Y( E2 V) h {4 X
- byte Left;' S" m% }. r( A; v, b1 S
# F/ B6 C! f3 x# Z4 r- V- void setup() * D$ z' r7 R% q% Z# @
- {
: `+ ?7 D5 R) w - //Set PWM 8KHz
9 o* ?1 s, f( }3 b6 D0 o! d# D - TCCR1A = _BV(WGM10);
5 e6 M% h, k& v$ l6 Z - TCCR1B = _BV(CS11) | _BV(WGM12);$ \' f9 U8 I/ G* z
- TCCR2A = _BV(WGM21) | _BV(WGM20);6 Q3 e9 k W; ]% \9 D% O
- TCCR2B = _BV(CS21);' k- t: ]$ X/ Y4 p: f' @
- Serial.begin(9600);
9 f% u0 l+ H$ S, l - buzzer.setpin(45);: K/ y1 k+ m% z6 `3 {; a3 C) |
- }
$ w f( W+ Z* }, Y5 _ - & R* F7 K7 o& l
- void loop()
- c- D) c' ^8 k- v - {
! R+ Y0 y' O9 y - while(!((lightsensor_1.read()) < (10)));# A- k% j5 V( O$ U3 K# u
- buzzer.tone(1047, 500);
8 I7 u3 F) S9 M0 `' ] - delay(500);
2 `" I" A+ ^6 Q: R+ X - do
; E/ m- }6 o% ~5 O" k - {8 O- [2 |; V* X
- int Err = getErr();
$ C3 C, H$ A: i9 x8 g6 s - if(D1 == 1): Z" P( Z; `+ y* P( z) Y. J, i) {
- {6 E' y5 O; v6 e$ u4 R# f
- Left = 1;/ C+ B: [, J6 A, L0 j2 M& _4 N+ B
- }
' J3 z; F3 ]* i" A: Z - if(D6 == 1)/ G1 |1 [) E0 m, R! z
- {
8 m3 u' `4 N; ~& r' k) R0 S* f - Left = 0;3 }. Q# K. A% N7 P5 c4 f- V
- }
$ _6 G" ]. X2 X" L8 I - if(Err == 99)
+ V: t3 \. n- [0 { - {
6 T% J/ J( z- d - if(Left == 1)/ B B: X+ \# k# q, b
- {- q' ^8 v/ U/ v" C3 ]3 k
- Speed -= 5;" h8 G) A7 n7 }& ] D
- moto(0,Speed);
5 |/ h7 I9 y3 o: Q; |, W* i) V - do
; k; k8 R# ~9 t; y" ]8 d' H$ V - {
2 p6 K4 k/ l* G& n" j - Err = getErr();& l4 V& Y8 ~" {7 |" `
- }while((D1+D6) == 0);
' b$ g7 w% w# B' z4 N$ @! X - }
) C( G4 ^: K9 {3 G - else! O3 ^# B4 L) x" i4 i7 b+ s! F
- {
7 Y0 a+ m& a/ T. Z2 d - Speed -= 5;6 T1 c& }0 c1 h/ G
- moto(Speed,0); 9 q) F( p$ M: _7 {. C, _
- do2 \* y7 y9 y! a6 _5 s: ~1 h- f1 A
- {: p7 O( n2 J$ f
- Err = getErr();
2 b7 d( ` o5 `8 z1 Z( a' U8 P+ f - }while((D1+D6) == 0);! e& `2 `: n5 p( S
- }
- k4 w \ Y( E( d- R - }/ [0 N# p3 B% k! S' q& ?
- else
1 H/ u1 B% a# \ - {
+ f# S8 n+ @* O5 v - if((Speed < 160) & (Err < 2)) Speed+=1;
, |& f1 y, N. r( b9 t0 L - if((Speed > 100) & (Err > 2)) Speed-=2;
. U0 R* D2 E3 p- I2 P3 ^& S6 f' L- c - integral = integral + Err; R4 j1 a5 n- n3 t! u
- derivative = Err - previous_error;; I y5 e$ H _. ` w
- output = Kp*Err + Ki*integral + Kd*derivative;: U0 v" ~6 P! L
- moto(int(Speed-output),int(Speed+output));# L/ U' ?; n' {0 D0 b
- previous_error = Err;& V5 x" c8 A7 t l- r+ o0 Z/ N
- }
6 s# s6 |8 Y8 A* L# P; k - }while(!((lightsensor_2.read()) < (10)));/ M* K' A" G2 f6 p1 x8 ^: [
- moto(0,0);
7 v9 _5 v. y0 z# K% C; B$ z( _ - delay(500);4 w8 \9 X! u0 a; @
- buzzer.tone(262, 500);* ~0 o6 D @- \0 r5 J! {& O9 c1 G
- }
, {: i! b* L% H" d# I
/ u2 M* n7 }0 `: i+ k; ?- int getErr(). Q" f9 M: u4 \( g- ~( s: u
- { " V9 b' f- ~+ x7 z: b
- sData = linefollower.getValue();
/ X4 s6 b0 v* g5 S - D1 = ~(sData>>0)&1;' ^+ H* W# h# C6 a8 v
- D2 = ~(sData>>1)&1;+ D. c4 x% k' S: \! c# O
- D3 = ~(sData>>2)&1;3 Q0 B: s. d8 f' A9 `
- D4 = ~(sData>>3)&1;8 }5 F) o$ J5 P, b6 V
- D5 = ~(sData>>4)&1;
4 o; s3 W3 x2 \! n - D6 = ~(sData>>5)&1;
6 t4 d. T, A' \. U9 B7 h7 M- { - int downD = D1+D2+D3+D4+D5+D6;
0 e2 R/ \, p0 U" A9 }+ V2 B8 n! D- H - int upD = D1*(Error3)+D2*(Error2)+D3*(Error1)+D4*(-Error1)+D5*(-Error2)+D6*(-Error3);
3 }" A' L3 E! F) w6 y - if(downD == 0), v. e+ Q% N* _! I" S
- {
, u# ]# W0 v8 ?/ R* j - return 99;
& U# D2 X6 c* O/ \ I C - }6 S3 D4 C( i& z$ }6 T
- else
! }; |) x2 q5 R; \ - {
) u: i3 Y- @7 V - return upD/downD;
+ ]7 L6 y7 c$ k& x - }' U8 Z: {3 \3 `$ R# s
- }
( D- A ~6 `3 @8 N3 ]8 a) p8 o1 m
" i* K' c/ E- u8 j- void moto(int Speed_L,int Speed_R)
9 S" p% W4 A O `+ n- k& i; F+ g - {% [$ O" t' d1 C# i
- Encoder_2.setMotorPwm(Speed_L);' p0 I, q- B: H
- Encoder_1.setMotorPwm(-Speed_R);
/ {5 I8 }: I5 ~ - }
複製代碼
$ A2 R3 f3 g8 ?MeLineFollowerArray.cpp$ _# ]% N. O4 R4 n9 I# a/ V& N
- #include "MeLineFollowerArray.h"
* C3 b5 G, }" w5 ~! i' h - 7 l3 v" D; ^0 U: {& A: @
- #ifdef ME_PORT_DEFINED
/ X% x& x1 ?: p& Q: Z, a - MeLineFollowerArray::MeLineFollowerArray(void) : MePort(0)
: o$ {) ^6 L8 j4 e d - {; d: |" \+ [* V, k8 T. |. i7 F% c) g
6 a. K9 `. y i+ g0 A- }$ V, H+ S2 B. I' L$ g; [+ g M# W
- MeLineFollowerArray::MeLineFollowerArray(uint8_t port) : MePort(port)
2 @6 |$ @, ]9 \ - {- M, B- `0 V4 m1 p- p6 k5 t% p- q
- _DataPin = mePort[port].s2;
. @( p! U) Z5 w5 k/ { - pinMode(_DataPin, OUTPUT);
; v8 G) e0 D; t6 M - digitalWrite(_DataPin, HIGH);
% _% Z6 I ~! z1 u - }, R( C [6 M( n! g( s+ N& U
- #else // ME_PORT_DEFINED
1 f5 O: W4 n5 c - MeLineFollowerArray::MeLineFollowerArray(uint8_t pin)1 t- p+ t) |' {3 y
- {
; ^+ ?6 Q; z" s y# @0 r4 V - _DataPin = pin;
6 ]& g$ k# N6 @$ R - pinMode(_DataPin, OUTPUT);
' _( L4 s. Q7 m - digitalWrite(_DataPin, HIGH);
* {) B z/ D: h - }/ `9 W; Q) F; J9 k
- #endif // ME_PORT_DEFINED
) f/ |& g! C) A9 B - % O. k. V/ W7 c: Q, k& J- {
- 7 f7 l. H& F- f* T* J7 ~4 r3 k# L
- void MeLineFollowerArray::setpin(uint8_t pin)
8 V5 s; `8 w% { g - {7 M/ R4 G2 V# I7 S
- _DataPin = pin;. X$ G7 R0 w% r7 y
- pinMode(_DataPin, OUTPUT);
8 Q r) u; h& r3 Y7 K - digitalWrite(_DataPin, HIGH);! S/ h# }( m; A/ G9 }
5 \* \ E" x. H' m) D- #ifdef ME_PORT_DEFINED) ~$ c8 b: T) c& T0 K7 K# q! i
- s2 = pin;
/ E1 O! ]4 `) ? X7 k: W. V8 u - #endif4 [" K1 `3 Q) y8 }( f% w) v
- }
4 B6 A6 ?8 z7 H& N/ p( q4 Z* c, G
" k+ S- N, M0 B8 @9 a- uint8_t MeLineFollowerArray::getValue()% Z( O$ U: g) P
- {: u( P8 G1 H7 l
- uint32_t LOW_level_read_time;
7 C6 k; X, z9 X% x/ A - uint32_t HIGH_level_read_time;
% |' L9 q& ]1 B" q7 E - uint32_t time_out_flag;
( I- G" C1 p* ^+ E - uint8_t Sensor_Data[3];6 {, X4 G# _3 C
- static uint8_t old_data = 0xff;
9 K6 P: V* B7 O5 G1 T
4 ?- h g+ W1 Q2 D& v- pinMode(_DataPin, OUTPUT);3 O+ c+ Z) K/ s' x5 ]8 {9 F* g
- digitalWrite(_DataPin, LOW);
; |2 ^( L( a. a - delayMicroseconds(980);
+ F/ |: T2 d) b" Q - digitalWrite(_DataPin, HIGH);# R, l+ g) ]. j5 _) h
! t& ~9 O/ y- H) e- pinMode(_DataPin, INPUT_PULLUP);
+ a* D# C2 C# ~4 g( A - delayMicroseconds(10);6 ?. T- j: G Q& F( w) s
- & s* ~' t$ }$ u/ T
- time_out_flag = millis();& f4 ?4 r$ Q U# Z. f4 R; i
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );. Q0 ]- |* n, l: H6 p' u) \
- {; _ E/ `8 Y! V) b- LOW_level_read_time = micros();8 u2 B% N% \: I# I0 r8 L3 J9 ?( g
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
( C, d' [6 \) F, O - {7 q; o! A- V& Q- ^
- return 0xff;
2 w0 L) S4 G5 X6 r - }
2 }! F0 b. i9 s( j9 K) r' |3 j4 N
% `& i% d0 t9 H& O% x, J- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );4 M' S+ [, ?7 N4 [: U n+ u9 R
+ b+ [( C; C- P2 x* [5 i- HIGH_level_read_time = micros();) \7 ^! H8 n. x" ^6 p2 Y
- LOW_level_read_time = micros() - LOW_level_read_time; //read 100us LOW level9 L. _( U0 c9 D6 f* Y: k) @( W
- . n# |0 A5 S# a* i; D# d
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
7 W2 q; ?/ N6 e* _9 m - {" ^5 @6 A) J3 K4 f" p9 P
- return 0xff;
* ]- L! N f9 g, t8 i - }2 @- [7 g) u G0 a( U
/ `3 a- h4 I3 E- if( (LOW_level_read_time < 90) && (LOW_level_read_time > 110))7 L$ y- f8 N2 v" c) E; ^. _
- {
- c! m! Q w5 A: D+ ` - return 0xff;8 y+ s h- U: ], x4 K: m* X2 E# B5 D
- }
0 m x& I1 r* d; p8 N$ e& I9 M* o
% e6 f/ e6 H& f. I8 P- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) );0 Z; y- g, o T1 Y3 ~) h
- LOW_level_read_time = micros();. P" I( C* @% ?7 m7 Q+ M( Y
- HIGH_level_read_time = micros() - HIGH_level_read_time; //read 50us HIGH level/ {- M' ~/ i5 `+ Q
& M' v4 m3 ?. f" R- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out) //time out
/ g" l' p; j% I2 y6 Q - {1 o5 h8 v! Z2 }% @4 t
- return 0xff;
- X7 r# s" W" ~7 F# K! ?/ m4 y - }$ H, }3 [4 c" l' `+ n/ W+ `
- A9 k2 j l C2 C, U' n
- if( (HIGH_level_read_time < 5) && (HIGH_level_read_time > 55))3 ?) K" V Z8 c% _& Z
- {) i9 Z2 \1 R: Q' c! B" t! |1 Y
- return 0xff;
& }$ c! I+ l0 `- P9 |# _2 u - }6 o% L' e- W, F/ E
- . L3 v' G3 t7 [# ~9 i$ H) N
- for(uint8_t k=0; k<3; k++)% V( M& K. d, B n
- {$ e2 T+ }% f+ ]1 ~
- Sensor_Data[k] = 0x00;! h; i0 R& p! T0 Y& e
$ D3 v, m! U; o; m- for(uint8_t i=0;i<8;i++)
1 t6 U. a, x( `4 y - {
- R6 q. v1 e( m/ T0 e - while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level6 i1 [9 {3 a- K2 H, j- m6 J
- HIGH_level_read_time = micros();# |0 k/ ]( ]$ A% U4 N
- LOW_level_read_time = micros() - LOW_level_read_time;
( [. R; o* W& ~- w2 f# T. p - / o% @( D* U) k6 A& U/ \
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) )7 }' y7 M. e! i8 I! i- u5 v# p' E, \
- {
; I9 g1 y- I2 t" I% |# ^! ` - return 0xff;1 A* P9 n* K& g- V
- }0 P! n/ V% U* w: _! v! J& m. x1 h
- 5 z9 C1 w0 D( H8 }/ y+ T
- while(digitalRead(_DataPin) == 1 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); t# u/ x9 J5 K+ r" y5 [( s
- LOW_level_read_time = micros();
! X _7 i$ @4 z9 ?2 F - HIGH_level_read_time = micros() - HIGH_level_read_time; //read HIGH level/ D" y6 f/ i: n! s1 r. E8 |
) w' d, c+ d4 F1 e1 B% c3 s `2 t+ R- if(HIGH_level_read_time > 50 && HIGH_level_read_time < 100) //bit 13 S3 O! | o' |/ j4 L6 v7 @1 W
- {
0 C8 e2 T& \& I% w' \% p% ~ Q# ^1 c1 X - Sensor_Data[k] |= (0x80 >> i);
6 u p5 i L+ o. I: b - }
/ ~3 u l1 J7 \9 B, v' G$ h( t2 x - else if(HIGH_level_read_time >= 100)
% t/ r0 [$ Q) ^3 M/ X7 D! Q7 \ - {1 Q' z6 K: y# t( R: j
- return 0xff;
$ k" t6 y" A% Z4 U J, K5 Y - }
# e, j7 C7 ]9 W/ [- w - % d7 g1 w9 c9 X! z! F
- if((millis() - time_out_flag) >= MeLineFollowerArray_Communication_Time_Out)
9 b8 g" k3 g+ v8 `; x - {
7 f5 h7 R. Y' x+ ~9 Z3 d2 C - return 0xff;( R* r3 G' n' Q7 J" w6 j( Z8 \3 ?1 y
- }
5 g) w: S, y& a1 _ - }$ E! G0 S. r& |, h( P) D
- }0 R6 J) g* M4 l3 r5 h8 I
% w2 ^6 K; c2 \8 |& |5 E- while(digitalRead(_DataPin) == 0 && ((millis() - time_out_flag) < MeLineFollowerArray_Communication_Time_Out) ); //read 50us LOW level3 I# d* g; @" `6 d8 G u
- HIGH_level_read_time = micros();
/ K1 D8 Z! L- r - LOW_level_read_time = micros() - LOW_level_read_time;3 v. Q$ j6 \. F: C/ L
- 2 J1 l1 H* {; p$ J& Q9 s" b
- if( (LOW_level_read_time < 5) && (LOW_level_read_time > 55) ): e) o$ E5 \, l6 ]) _
- {
$ t/ w( W) B8 I, Z: n - return 0xff;
+ {3 \. j" j" N3 _$ i - }7 W7 L7 L$ g+ S* D) p. m2 ]
; w* }* o! B" @$ E" s& q- pinMode(_DataPin, OUTPUT);' W. C7 |/ y5 u6 y- R0 t
- digitalWrite(_DataPin, HIGH);
0 x7 ?( A$ `- T1 @* L5 z6 a9 q - " {5 N5 f; K3 j! D" E
- if( (Sensor_Data[2] == (uint8_t)(Sensor_Data[1] - Sensor_Data[0])) && (Sensor_Data[1] == (uint8_t)(~(uint8_t)Sensor_Data[0]))) p5 b1 T3 _. w
- {
' Y+ V! H2 j% a: s5 \ - old_data = Sensor_Data[0];
5 z& n' j s, [( e - return Sensor_Data[0];
3 e5 a/ M' B! r - }
6 L, s& h6 n/ j1 p& v - else
- P* ]8 Y! A9 s# W! x8 S - {% i6 |# X6 F g, G0 Z4 y" R
- return old_data;
7 n0 q. T- H% g, Q- ]/ U `' z - }
: s1 e" g" ]+ {+ O - }# g# ~5 ~7 K% t5 M9 }
複製代碼 / x5 F8 Z3 J7 b. w6 T* F0 `
MeLineFollowerArray.h
MeLineFollowerArray.h
(731 Bytes, 下載次數: 10, 售價: 100 個銅錢)
$ B0 x, P* a- D7 S/ \$ R( A! W- F# }3 A
|
|