|
|
- #include <Arduino.h>
' p" Q4 K& _. o) T - #include <Wire.h>
& }) w" q# ~. N1 C2 I4 Z - #include <Servo.h>+ e' e/ F: U* i# g1 y
' B. E! ~& U6 e \/ U- #include "MePort.h"4 ]# o( @2 v) c( @3 \
- #include "MeUltrasonic.h"
4 w8 T6 l: i1 k' H - #include "MeDCMotor.h"+ i6 V; n0 |7 M, P& F4 @" M
- - I, h8 J: D3 q# \6 H
- //double Input, Output ;
5 J6 A2 k9 p+ A. R - float MaxSpeed = 255;' U9 J8 P* x$ g, _
- float MaxPower = 180;
! j9 v- X9 o' G# } F9 M - float MinPower = 120;
" ~; w0 h0 R& F9 G - float Error,ErrorAcc,ErrorDec;
( M- I/ y# j7 r+ ~ - 5 |# ^! d$ W% j- N- X
- float Kp=0.14;+ K6 c* W7 X; d4 K' q
- float Kd=0.00020;//23;
% n9 V; v9 l6 H6 A! g. B% z$ Y - float Ki=0.000201;
3 p0 ^: R4 U6 t5 _/ ~ `
, x5 j* \; O$ Z- float nPower;$ o$ ^% \5 y/ X4 ]$ x) ?
- MePort lightsensor_6(6);2 c. o1 |& f/ o7 v g
- MePort lightsensor_8(8);; |) X3 l7 N! K* j5 d
- MeDCMotor motor_9(9);' O+ P$ {4 w, | \; X: ] y: q7 \
- MeDCMotor motor_10(10);4 m N; r/ b1 R
- unsigned long previousMillis = 0;
2 {- I/ D6 y- ]" u - const long interval = 1;2 K4 G$ B6 w! Q* \9 J
- . p- w* {/ g' {) k, ^* A& n
- void setup(){* {' O, ^' R( r/ @
- lightsensor_6.dWrite1(1);
9 L& j5 Q, ~# h5 w' t7 E: q - nPower = 160;
4 t2 S6 }2 R3 L1 T - Error=0;
: F7 X9 e. D8 G7 v2 i - ErrorAcc=0;# \: a" t* H+ X5 P2 }+ J; R! U. {5 R
- }
5 ?0 z7 j, X2 ~9 n5 [& w - * w) Z. e6 T' ^. h5 k
- void loop(){! b. T: O- G6 T# _; O" k- t+ c; q
- unsigned long StartTime = millis();
" s. c% F3 O2 R! c9 c/ P - if(ErrorAcc < 18000 && ErrorAcc > -18000){
9 z+ s$ z! D, \8 A, | I - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();2 G; u1 S, S6 j c) j
- ErrorAcc += nError*Kd ;( M- X6 q! n5 W' W/ z
- ErrorDec -= nError*Ki ;
8 M0 r+ j6 G! I G. y, W) ] - Error = int(nError*Kp+ErrorAcc+ErrorDec);
/ ?% L v0 @! @$ F- \ - if(nError < 80 && nError > -80){+ l- F e3 Z. G$ _7 F
- if(nPower < MaxPower){- T( x- S: u/ u* Z: v' f
- nPower += 3;: I! U) ]- o# s" O) T: j; t, B* K
- }2 d- r8 Q. ]7 X/ A3 V
- } else{6 b' ?; v' R8 C1 @; Z/ x% c# b
- if(nPower > MinPower){
3 T$ d y8 S6 U1 W+ G - nPower -= 2;
- Q6 n% f% Z: e5 V, V7 V - }
( |+ y2 P0 l1 p! N - } 9 U8 n3 Y- Y' L" Z& }1 w; W7 f6 W
- MotoL(nPower-Error);
! m0 Z& O6 l, R) R% I5 M - MotoR(nPower+Error); ; I' t. t C9 C3 `; J; k
- }else{! Z1 y! |4 a, \- m5 C
- motor_9.run(0);( G* |! ~, d7 v' [1 |% v6 W
- motor_10.run(0); V$ E0 R# ^5 E6 K$ O$ q: `
- }! o$ N) W$ O' C: t9 m( y" D6 M) w
- do{}while(millis() - StartTime < interval);
' Y, P$ p. S. E) g" M% Y3 n0 B - }7 W9 c/ |/ k/ C" m1 V! l5 A' J
- " `5 _* U: b. a/ \
- void MotoL(int Power){
- _9 l, {5 z1 \# f4 k& l# L* @ Q - if (Power > MaxSpeed){! f: r% _: B: s
- Power = MaxSpeed;0 [+ @1 c$ j) {
- } - q7 J3 f, |( g: C
- if (Power < -MaxSpeed){
* A6 o( J3 ?6 @$ k, { O% a% r - Power = -MaxSpeed;; j5 j- @: Z% r% M2 d6 @) K
- } , c1 N+ j/ d2 h: L& U" e. X
- motor_9.run(Power);6 p+ ], C, O0 r( I8 c8 y$ S
- } " G0 ]9 \' N3 R+ P9 E, }1 _ x
/ d5 i5 S7 `3 E7 E) ~2 L9 o- void MotoR(int Power){
4 s9 I( v8 n! R$ J! @6 o& T - if (Power > MaxSpeed){) F S- q7 G) k
- Power = MaxSpeed;
- F: O& d0 x3 l - }
* U+ T9 ~0 [" E3 ^6 p - if (Power < -MaxSpeed){ D$ P/ I8 H, L3 I
- Power = -MaxSpeed;
2 K4 P( e$ I M0 l. [$ m5 D+ |, u2 O, ] - } ; y2 u+ Q4 n4 e+ q
- motor_10.run(Power);3 `( Y a9 t! ]* G
- }
複製代碼
8 i# K1 J7 B* G8 W- ~8 V# k! m7 K& y7 s4 t" U' O7 V
|
|