|
|
- #include <Arduino.h>, d8 p2 A5 w1 p" \' r: ?
- #include <Wire.h>8 i& i- ?5 f Z6 C( Z
- #include <Servo.h>
}' n3 h2 L+ E/ U
% ]2 b2 c( `$ W7 M- #include "MePort.h"$ J# y+ t" j! f+ R0 N( m
- #include "MeUltrasonic.h"
3 F* T/ n. P8 i) J/ j - #include "MeDCMotor.h"
/ `( M/ V6 X! q8 p1 ] - \0 _! ^) m+ m& G- z
- //double Input, Output ;- F9 w0 N) n# |7 X; E
- float MaxSpeed = 255;
* J: O: k" I3 Q8 r/ b - float MaxPower = 180;
# D1 U5 l7 n' `2 d1 f - float MinPower = 120;
/ i3 j' z& X5 o( _- C# o - float Error,ErrorAcc,ErrorDec;% m; M8 u1 H2 r
- 1 }+ h$ {! \1 J ]& n9 s9 ~( }
- float Kp=0.14;8 Z" y0 {& Z; l% D: }0 l
- float Kd=0.00020;//23;
, R" L7 o) G5 I* J2 h - float Ki=0.000201;) ~: A6 H1 s, a$ h
4 U/ V% W( v z5 @2 O: I1 j; x- float nPower;
$ p4 y4 T4 h2 S ]8 V# E, @% t - MePort lightsensor_6(6);
+ N% G- {9 J# K& x. a3 a - MePort lightsensor_8(8);. w `: Q" W6 L; u7 H: N; P
- MeDCMotor motor_9(9);, ~# \' k1 z6 ~) C7 r5 H# O
- MeDCMotor motor_10(10);7 X2 J5 w" |3 d& g
- unsigned long previousMillis = 0;. L% i* K# I3 w! |, ]
- const long interval = 1;
. l h, T+ b) D" ]" f
1 J' j$ s5 t: @1 H5 D% i- void setup(){: R- y0 V9 ~- X0 A0 g
- lightsensor_6.dWrite1(1);
# } [; Z0 M2 P3 H+ a - nPower = 160;) x) E# e9 s; [4 ]
- Error=0;
. i2 c$ `8 ~' o s5 m - ErrorAcc=0;
6 } U/ u+ T j - }9 j' N$ v& w$ v
6 H7 l/ _$ n0 Z1 U% s) q T- void loop(){
$ J: b! t6 ]5 F! b - unsigned long StartTime = millis();
0 ^' R. M7 _) i( y2 N) \% @8 C- S% ` - if(ErrorAcc < 18000 && ErrorAcc > -18000){8 D: }8 W1 m9 ]7 T. j
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
/ U1 k0 B+ W6 ?0 K* S/ A2 ? - ErrorAcc += nError*Kd ;0 c6 u. V0 u! _: U& l
- ErrorDec -= nError*Ki ;, D4 m2 P+ g7 |, l8 ~+ T
- Error = int(nError*Kp+ErrorAcc+ErrorDec);2 ~6 K0 p0 r, j
- if(nError < 80 && nError > -80){
4 a* [$ K, _8 G. O+ \ - if(nPower < MaxPower){- b4 H' r8 w% \- G* p9 k9 e
- nPower += 3;
& E9 o. \7 }$ v0 Q" ` - }
# m; Q6 L) Q6 m3 Z M - } else{
' B% k& m0 n5 k5 j - if(nPower > MinPower){
! N' K/ r" t. B" L8 n - nPower -= 2;. p6 I1 k) @0 S0 u2 b# k
- }
- r/ w" x5 I3 C6 n - }
- L+ r& ^* x' ^; c9 i& c( B - MotoL(nPower-Error);# @+ i3 [0 K2 J' _; F
- MotoR(nPower+Error);
; t+ R/ K& m5 [- @5 M( g" K - }else{' Z7 f2 s: U! m' N
- motor_9.run(0);# J" ?+ u% Y( I5 h
- motor_10.run(0);
6 G5 d( R) t! r: E2 D, f8 _9 c - }
; D# G. m! Q. ]1 { - do{}while(millis() - StartTime < interval);. p: `- }& g- d4 {
- }, Q3 B( {" I( x5 q4 y
4 K2 o7 C* \8 j1 U8 n- void MotoL(int Power){: M7 t: g; X: A8 G& V# |- r1 }
- if (Power > MaxSpeed){* o. w9 X9 z. i' l, _, m
- Power = MaxSpeed;: \, t! m5 e4 [0 _9 j
- } : S! h; E, ~, o/ e$ `
- if (Power < -MaxSpeed){
q( }* O. y' X" {9 P" `8 Y, C - Power = -MaxSpeed;$ [' [2 L& v ~+ \/ `9 I8 L+ M9 {
- } 0 O7 e! L8 e6 T! |: H
- motor_9.run(Power);4 o6 ~. ]5 G' k$ D
- }
- w8 r& S! n3 i - 3 z/ V9 F! |6 v( W x
- void MotoR(int Power){
. A: b O: p6 W& T3 E7 g - if (Power > MaxSpeed){
# t6 b7 R% I* g& A4 w F - Power = MaxSpeed;: E [1 U9 }% `9 d: s' v o$ N/ G, P
- } 3 y+ M$ V8 v4 ?+ @$ e
- if (Power < -MaxSpeed){
8 u* o; c( K: L' _2 J+ X7 C - Power = -MaxSpeed;) k: H; Y# w9 m5 s: r7 r
- }
& _% e4 u% x# Q - motor_10.run(Power);( s9 ~9 i2 R, v
- }
複製代碼 9 p1 |* q- Y6 d- p H
" f* G# e$ |' J) S E$ U: ~4 S' j1 E |
|