|
|
- #include <Arduino.h>* t5 C. o" q0 j0 X
- #include <Wire.h>
6 K' \9 ]" ?" | - #include <Servo.h>
7 C: {7 b- q1 h" [4 X - & S4 z5 F8 I* k4 @/ C2 _1 T& t
- #include "MePort.h"7 V5 \' z6 c6 B1 l4 M! p
- #include "MeUltrasonic.h"
$ V( }8 B, Q" i5 f, H - #include "MeDCMotor.h" j) p% A; e& u1 R0 n: w: j. q
8 E& `/ |3 }: _- //double Input, Output ;, c, Y7 @( L9 A. l4 B$ g
- float MaxSpeed = 255;
0 @8 C9 `' L: M - float MaxPower = 180;
6 ?& d9 Y- _! L: Y4 O, `( V - float MinPower = 120;
: e4 N( O' d- q5 X9 H - float Error,ErrorAcc,ErrorDec;; r; M) H. u7 Y. b+ ?
' p9 E: x0 l; }- float Kp=0.14;- |8 C% q/ c0 X, v& ]2 [
- float Kd=0.00020;//23;; ]0 ]+ k2 A- _7 l6 I* F
- float Ki=0.000201;# ~4 B0 c7 \8 t0 N/ g5 o* }( ~$ ], k
- @! C% [( _) b( n- float nPower;0 R' b/ G: o/ e8 Q! }* W- @3 q2 d
- MePort lightsensor_6(6);# s$ D+ P7 Z6 m* @8 H8 W
- MePort lightsensor_8(8); j0 E n& T& K/ H3 J- g5 ~
- MeDCMotor motor_9(9);' e2 \" P( o2 b' d
- MeDCMotor motor_10(10);, Y6 s( ^; ~# z, o/ \0 i1 H
- unsigned long previousMillis = 0;
! Q U! Y9 p4 |( o4 _2 q+ T - const long interval = 1;$ w7 k. ], o3 V% C7 ~4 w4 |. q
( k* G: G8 I. I8 T( P2 ~; v3 o- void setup(){
% B; G% S; U. l& I; B* v+ k! O - lightsensor_6.dWrite1(1);2 Q5 n) X2 g; `; [7 A1 z$ u" K
- nPower = 160;$ M& ^4 s/ \* u
- Error=0;: L; [' A+ _. h$ R
- ErrorAcc=0;1 E3 K, }! _+ H( V4 u$ R, {. }
- }
5 A4 x$ n8 K4 G8 W# _ - * M& G0 G% w9 \5 ]9 `
- void loop(){
8 M5 ?0 y4 O) _" b8 |/ w ]$ c - unsigned long StartTime = millis();/ z) Z+ e% M) z1 s
- if(ErrorAcc < 18000 && ErrorAcc > -18000){/ A+ b7 x' |+ p8 l2 }) _; P
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
8 j/ e( T9 h/ b0 J! P# H0 G2 \# V9 w - ErrorAcc += nError*Kd ;
% A( O1 X5 S6 V6 }) Y - ErrorDec -= nError*Ki ;" I, |& @7 c5 h" E3 I E3 O
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
* `: y3 t+ E' { - if(nError < 80 && nError > -80){1 v+ k# d! R& A H
- if(nPower < MaxPower){+ p% d0 M. c& w' y' N
- nPower += 3;. }( K, j: C8 Y7 d, z
- }
$ ]" q, j4 I3 H5 L6 Z - } else{8 `" o& @$ g0 l- U4 g: t% z5 Y
- if(nPower > MinPower){
* p& t, N$ f, B) ` H - nPower -= 2;
- Y2 u0 Q1 H: ?8 [% \+ C - }
) |, Q: |/ b1 `! Z5 K - } 1 Q* F' \: `6 q* g4 q
- MotoL(nPower-Error);
( I) {) V, i2 P5 s; m. f - MotoR(nPower+Error); 9 T! J3 H9 z S1 e: t j; P
- }else{
5 d+ {8 w& ?5 l: m6 `: W; ]/ ` - motor_9.run(0);: j/ v+ r" X2 g" a' _
- motor_10.run(0);
& x+ h- L* y9 A/ y) o S! E# l - }
- r" z% N5 M4 m" @5 g - do{}while(millis() - StartTime < interval);6 J8 F% `7 g* i a- Z# `
- }
3 a$ ^" [5 Y# H# d4 M
; k3 O% }. O# P- void MotoL(int Power){
, F2 m" m+ D% n" x/ u6 E - if (Power > MaxSpeed){
v# d0 p$ ^1 p( T! R& | - Power = MaxSpeed;# _( T5 M( t. ?
- } 7 X8 U7 K5 H2 f. i
- if (Power < -MaxSpeed){7 }0 O8 ^; u! H! k
- Power = -MaxSpeed;
6 c: l, c: E- L5 L- Y" m - }
5 [+ y7 m* T: O - motor_9.run(Power);- m( f) Q: }% o
- }
8 q1 o& D# k2 {2 ~, B* G' n" M
. I* |! s4 o$ D- void MotoR(int Power){
! p6 `+ X3 p% W! q+ b - if (Power > MaxSpeed){
" l& h: l: M& J: ?5 \' z3 P/ x - Power = MaxSpeed;
7 a1 J1 \' s% m( @: o5 x) U0 j( b - } : o3 ~) G; e7 _6 H- z
- if (Power < -MaxSpeed){
- L5 s' y8 I9 Y& p# ^) j - Power = -MaxSpeed;
X( V9 `* j/ o6 C4 n0 o4 k - }
|) S n$ P& J: \- }+ c( T - motor_10.run(Power);3 ]; D5 F/ T! t( Q
- }
複製代碼
# r7 q i. T6 P+ [" | w7 n2 C( q' i/ i/ V
|
|