|
- #include <Arduino.h>
' X) q, U; u, e - #include <Wire.h>
C) O* C ` c. p' m4 o4 n" S - #include <Servo.h>- ^( d+ E! R8 i
4 C0 {* ]; u+ }0 A- n( F5 P) I4 r- #include "MePort.h"7 K# x) p" t1 A2 q
- #include "MeUltrasonic.h"; ]# u p O( s+ R
- #include "MeDCMotor.h"% m& K0 z3 D( x, `; C2 S( ]. g
- 3 C5 M, l% I; ^9 y; h) G" R
- //double Input, Output ;
/ H- Z% ]. B* a3 `& p7 [ - float MaxSpeed = 255;
/ M5 i/ K) O( t! k9 t0 Q& v - float MaxPower = 180;
( _+ Q, n1 K% B* `& P8 O, C7 f - float MinPower = 120;+ e; B7 j L0 d ]2 }1 X
- float Error,ErrorAcc,ErrorDec;
3 K. }" Y! e% d) E$ m - . M2 ]+ Z# R- c
- float Kp=0.14;9 u8 y- Q# k( x: J
- float Kd=0.00020;//23;
/ v1 n/ f4 H3 I! \ - float Ki=0.000201;3 p' R* f: F& F6 Q: `) Z8 v& R! Z+ v3 B7 Y
- . p3 j* L+ |% d w9 Y5 E
- float nPower;
% C. t" v- `6 E, G - MePort lightsensor_6(6);
0 }4 W0 D0 g$ p2 _1 D. B - MePort lightsensor_8(8);% Z1 C. k0 d( ^/ |# p& w6 v
- MeDCMotor motor_9(9);& K6 d1 b% u5 R; h8 ~* R$ C" E
- MeDCMotor motor_10(10);
: ~6 u q/ @* T3 K; J5 F - unsigned long previousMillis = 0;. |- c. M- V' X' v; f, z0 k0 I
- const long interval = 1;
# f% g" x9 e% }" j" f' C - $ `6 X2 S5 J7 H- l0 o7 n" Q y: K# |3 a
- void setup(){
A7 N, x) W, H3 x; h) N6 j% s - lightsensor_6.dWrite1(1);$ C* c' u V; G
- nPower = 160;
1 w5 P4 j0 u/ b5 C: X1 H! w* l1 o - Error=0;# N- ]6 P b5 O C$ o
- ErrorAcc=0;8 d4 Y$ v' m: {) [8 x1 Y
- }
# @, G, ?* \( j8 U2 J B$ j
; p% ?/ d) R) u7 E- void loop(){- w6 C; @6 v7 ]- y# d
- unsigned long StartTime = millis();
% Q) V9 X8 [" O% c& N8 Z - if(ErrorAcc < 18000 && ErrorAcc > -18000){5 S3 F; l2 a1 x* o6 B
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();3 m/ _: v* R7 g3 B6 ]! ^
- ErrorAcc += nError*Kd ;
. w) r' f% {' R }3 D - ErrorDec -= nError*Ki ;
5 c, {3 L& F8 g4 C0 I. C6 c: ^/ \ - Error = int(nError*Kp+ErrorAcc+ErrorDec);
- O- u; K2 o/ E% R4 C) b% I. t5 J# j - if(nError < 80 && nError > -80){
; s/ Q$ O/ |( K! K7 f7 H, j: i - if(nPower < MaxPower){2 y: ~1 R: g) h" M9 P2 ?* v. {
- nPower += 3;
& |& Y3 {6 u& z" D: G2 `& Q; R, H - }
6 W1 G- T/ F3 n - } else{3 i( {/ `" R! c/ {
- if(nPower > MinPower){
, _" t9 c3 j' @) k - nPower -= 2;: B9 ?% i/ O9 ~; ^/ Y
- }( T. `5 ]6 o0 T! ~& m+ S
- }
0 x) l1 S: Y M+ q" c3 R' y- E - MotoL(nPower-Error);
- e1 `6 a+ S' S - MotoR(nPower+Error);
) n6 P; b. ?6 C1 | c - }else{ D! s& U2 n- a. p6 o3 y2 M4 M& G# X& d
- motor_9.run(0);( X' {, W" A6 n, T8 `8 W- }) e
- motor_10.run(0);
* L+ m& ^. T! \+ ~( j- ]- M - }
6 B; V4 E' S- g' U* I - do{}while(millis() - StartTime < interval);
5 x) b' s7 m% F; S - }/ i7 V$ P$ r4 v- c4 n, t5 U' I
- , i: S- x- X9 o4 r& t+ Z
- void MotoL(int Power){8 I/ O; Q' D6 A" J* h7 y+ Y
- if (Power > MaxSpeed){9 o8 M9 l+ ~4 s9 H% b4 p
- Power = MaxSpeed;
2 U5 ?$ q! H0 u; W7 e - } 6 V0 ~* ^! A4 v2 \( h" }) V
- if (Power < -MaxSpeed){
* Q) ^/ v5 m* Z% |. s0 [ - Power = -MaxSpeed;
% ^* J5 E! x, c2 r - } c6 X& e" r. \" n. g$ N/ n
- motor_9.run(Power);
9 k: G, W, u! A5 M7 [1 K - }
) u6 p7 n# ~/ p5 F1 ^- ?
; j1 K: w3 t5 {3 {- void MotoR(int Power){
, ?# R+ i) I$ w) p8 A8 x: f7 Q - if (Power > MaxSpeed){% k- U% @5 E' `" w: O& P
- Power = MaxSpeed;
h6 R) E1 S" g" V2 a, C - }
. O6 _- ]" Q7 |* G/ c$ X - if (Power < -MaxSpeed){/ D; o/ w4 D, \2 \+ m# R' [
- Power = -MaxSpeed; S! b) ?+ E7 `8 U+ R
- } 3 Q" S" l: s% N$ O7 ]! O+ p. T
- motor_10.run(Power);* E% b. Q/ x) S# ^. F) Z
- }
複製代碼 ! w- D) z% f1 ]9 T
" M7 D% [/ w; }1 w
|
|