|
|
- #include <Arduino.h>$ i/ ^# o! E! }3 J4 ]
- #include <Wire.h>
- ^6 ?$ P1 K1 {% t" @) s - #include <Servo.h>
& I$ |6 V% F% \ - % V7 @0 S8 i& ~5 z" z- X
- #include "MePort.h"
: t, b0 O: E+ l2 J) N( m8 f( _ - #include "MeUltrasonic.h"7 @; h ~& z& h ^6 v2 e
- #include "MeDCMotor.h"
- l$ p8 F, s2 s! v) G) T3 |+ ? - - ~8 F5 S) Y; m% N4 s
- //double Input, Output ;
! M( b3 K! m) J3 J. x1 r4 } - float MaxSpeed = 255;
1 r* O6 ^9 h7 w - float MaxPower = 180;. w/ }& i% t& ?! ^/ W
- float MinPower = 120;4 |0 h: q1 v5 s8 {& s' a+ H
- float Error,ErrorAcc,ErrorDec;
. g- i% K8 h5 w
c* }# b% c& `: B* i7 Y+ W- float Kp=0.14;* a8 O- I( `9 k, n; t
- float Kd=0.00020;//23;- q8 S" Y6 e* B( N! _
- float Ki=0.000201;
5 D* F' ?. U6 y) o' L
; W4 h7 u* a6 W0 Z3 e1 f& `- float nPower;) d) p! r) r, |% G( p: |
- MePort lightsensor_6(6);
# N/ W7 [! ?+ x* u: `' G2 ~ - MePort lightsensor_8(8);
@1 }3 }% `6 P/ v" @( ?- p - MeDCMotor motor_9(9);
8 [" b8 t* t/ b( g5 a- w7 V3 L - MeDCMotor motor_10(10);
" ] K' X0 G5 [+ m+ w - unsigned long previousMillis = 0;1 d, D/ ]" D4 B' ?* o
- const long interval = 1;8 h+ p$ i z' E! O
- + n' R: P$ Z* G9 j
- void setup(){
4 D7 }; b/ ]$ }- _9 _- m - lightsensor_6.dWrite1(1);
; M* T+ M8 k M r - nPower = 160;- Y1 ?) ?1 v4 n* G! W) U4 @( l
- Error=0;( R$ D a* b0 ?' x+ x" [
- ErrorAcc=0;
: K$ W& o! m ~ - }5 ^ S! P4 }! s9 V$ N
% o; j0 L3 n' O* ]- void loop(){! x) |! W0 \, {% r9 N+ ]( v! ?3 M
- unsigned long StartTime = millis();
0 B: [2 ]& E# W, ^& C# N - if(ErrorAcc < 18000 && ErrorAcc > -18000){
_. l, }3 L& x8 m - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
/ x7 F3 S$ B0 h, p9 w7 e - ErrorAcc += nError*Kd ;
1 i8 s# Y9 q1 r$ g - ErrorDec -= nError*Ki ;' v6 ~ f+ Z9 I
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
8 ^7 l' U% c8 ]+ K1 T - if(nError < 80 && nError > -80){
7 J4 T5 K. b3 V - if(nPower < MaxPower){
! J9 w5 t& {9 X7 J/ M9 y4 j - nPower += 3;
, `) Q; u/ l; u5 F8 J5 X6 w - }
- Q- ~7 @" _. Y! w; z" T - } else{* v, r h; }0 V' B! K# G
- if(nPower > MinPower){
9 M2 P H2 o' M; y) g2 [7 H; Y - nPower -= 2;; w( \" ]8 W+ k5 k. `7 f, p7 u
- }
0 N7 O+ ~" i. v" ^. T7 W - }
" w m. I k$ }/ V" R( O - MotoL(nPower-Error);
0 g) H7 t5 N; {: h& A: Q/ o - MotoR(nPower+Error);
' s! G3 j7 ~3 ~' k ~ - }else{ l% _8 R0 x( ?" ~# M8 f% l: \
- motor_9.run(0);
( \/ g( D! \1 s1 }4 X0 k2 w - motor_10.run(0);
& Q/ V' o8 I l8 a" J5 W( t - }
8 \* r5 i; I! B1 q3 ? - do{}while(millis() - StartTime < interval);( t, r! q: S0 s0 D& r% U
- }! N% Y1 u3 _2 I5 I
- - V( }; Y: }7 t; C
- void MotoL(int Power){3 @; f5 B9 [" _
- if (Power > MaxSpeed){
2 w4 }: [; h2 F# }5 _$ |* M5 n' D - Power = MaxSpeed;8 U' F8 H: _0 U# l
- }
) I2 I1 H8 s0 A' N - if (Power < -MaxSpeed){9 l, ^9 C3 Q3 L
- Power = -MaxSpeed; G1 l8 m6 I% S/ W( _
- } 4 \% F0 s9 h" n' B! @
- motor_9.run(Power);! O# B# d* @4 l% X2 C Z
- } - N" X! B/ B1 \4 [% K, X
- $ \6 H& R0 o6 z+ U6 W" Z* U, g2 ~
- void MotoR(int Power){" t; H: \( b! A$ s( b
- if (Power > MaxSpeed){
6 e* X2 g- K3 {# [ L( U# U - Power = MaxSpeed;0 ~. X! N) s3 a/ v, _
- }
' O7 ], F& o1 u. K - if (Power < -MaxSpeed){
" w& f! h- s$ u1 H. u - Power = -MaxSpeed;
9 A" e5 ^% x2 |; J - } : L2 G j' H' E; N5 w2 p
- motor_10.run(Power);
8 l+ }4 o2 E4 S8 A# @ - }
複製代碼 9 ~: i2 T/ J- d. j0 c* L
: [) J" o5 s/ k3 i
|
|