|
|
- #include <Arduino.h>6 Y8 k* h+ @, I! M! }& g1 [' z
- #include <Wire.h>
" m: }6 A0 N3 ]6 t9 M - #include <Servo.h>
+ W( l; i8 d* \ Q3 q
+ M' Z4 Q: q2 I* M Y0 p/ I- #include "MePort.h"% c$ Q) p2 W+ H
- #include "MeUltrasonic.h"
* x3 F4 `. k$ Y k9 K& t% J5 U - #include "MeDCMotor.h"
8 @# H& p: \- _) I! c/ s+ c - 9 p* Y& G! {* Z, ~
- //double Input, Output ;/ c1 B' x0 q1 E" D
- float MaxSpeed = 255;
8 {" E) ^: w* W9 _ - float MaxPower = 180;
3 ~) E5 z( n- z# P2 r- _5 Z8 z( @ - float MinPower = 120;; c5 o' D p; q' z8 e
- float Error,ErrorAcc,ErrorDec;2 t& v$ ~" n3 i1 u( K
- : M, s1 X, W- k
- float Kp=0.14;; Z1 Z; d! i' N$ t. {" X
- float Kd=0.00020;//23;
5 B, u; p- F5 c# P) y - float Ki=0.000201;
+ R) _& V% ^# n" J6 m - & t# @. u# m {. Z R4 I5 D
- float nPower;
4 o x5 r3 t* u; A/ Y( T - MePort lightsensor_6(6);7 X5 b8 {( b1 X$ }0 Q
- MePort lightsensor_8(8);& z7 m# I) U( G/ O1 c" i
- MeDCMotor motor_9(9);
8 I5 J. K) g0 q# @ - MeDCMotor motor_10(10);
/ X7 G. X8 V* I( @1 \8 h - unsigned long previousMillis = 0;
7 x' t* i8 [+ G) S - const long interval = 1;
( T' F2 t C, `+ {: P' i
/ i2 V2 d6 }2 x) j- void setup(){
1 v5 H/ a7 I9 T5 s, I - lightsensor_6.dWrite1(1);
+ |+ l1 m8 D# [0 ?1 r& w2 C8 s# F; N - nPower = 160;
0 e$ v. D6 u! H2 T) l - Error=0;/ x; l/ R+ L- n& e0 w5 J
- ErrorAcc=0;
- `: R4 `. J0 O5 U; t8 Q - }
^/ R9 ^+ _. M& j- G6 q
9 U" w! K7 [: p- void loop(){: a; a, W! X& i5 W& S# d# W
- unsigned long StartTime = millis();
0 ^1 t6 C! K0 V% w6 D) P! e1 A& a - if(ErrorAcc < 18000 && ErrorAcc > -18000){
u4 |. M# P" c% m - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();, Z% i/ I6 }6 O+ @- ?( ]' \: @
- ErrorAcc += nError*Kd ;
5 C$ X* F8 h* D$ L - ErrorDec -= nError*Ki ;
3 o3 _6 b$ m7 m& m% f - Error = int(nError*Kp+ErrorAcc+ErrorDec);
5 }) C+ q8 B4 G - if(nError < 80 && nError > -80){
- _3 p" Q# r! v2 r' ^* ]1 @ - if(nPower < MaxPower){3 ~4 E' E% h/ R$ |- I
- nPower += 3;3 U& L/ X* N8 e$ Q% b) v; U1 V
- }2 u0 J" R6 B- x& J) V
- } else{3 a% v& C; c7 j+ {4 V4 a5 T
- if(nPower > MinPower){' W; V! _" f" z5 a/ a/ b! D4 _2 q& s
- nPower -= 2;) Q( M' H4 h/ Q/ P0 q) r
- }
5 O# k, K. b( X7 {7 ^; M9 z" n - }
4 J( {: Y5 C- L* J0 A( s; } - MotoL(nPower-Error);- j- _+ [. ]6 @" k) M# c/ s
- MotoR(nPower+Error);
% A& {6 `6 n4 A7 ] B2 w - }else{
# [# K- G- M7 U8 h+ M - motor_9.run(0);& I; `$ ~& ]; X1 m; }4 m. O- ]
- motor_10.run(0);
F5 s7 |8 h# ?4 @ - }
; h, j3 v+ ?8 ~' a4 M - do{}while(millis() - StartTime < interval);9 `& T: }0 @3 J/ \9 g# U" M$ r, p
- }/ [/ n" }$ O. Q: a& w0 r: R; D
: r+ p9 V" `$ C) v& ?& }- void MotoL(int Power){ D2 H* y1 o/ x
- if (Power > MaxSpeed){
N3 Y6 h5 p4 U/ D - Power = MaxSpeed;6 |- `$ }7 ?3 ]
- }
; c# V% `& u8 F1 _3 ^ - if (Power < -MaxSpeed){
9 {* ?+ M: c( F1 l" @2 l8 Q - Power = -MaxSpeed;
3 _- e3 V- K( e3 y }3 u2 d- d/ V8 P - } $ g; o' o* F# z. l# w* p
- motor_9.run(Power);
; Z8 K- n9 ?& \2 e7 O- _+ f$ f - }
2 j/ u' h: Z$ h* w- \$ G& R2 @( B1 | - 5 G1 l" u3 J; Q1 Y. D. j
- void MotoR(int Power){. G- v D0 s, ^+ \6 e
- if (Power > MaxSpeed){
) |+ a! U3 Y6 u. g6 R" Y - Power = MaxSpeed;; f1 O# {: f* J
- } ( ^$ Q/ i$ b' o+ Z
- if (Power < -MaxSpeed){
7 s8 F! g5 l4 _+ \ \0 I( m6 O+ U - Power = -MaxSpeed;
. J( F, O0 f& ?, y4 i - }
3 {8 j2 X3 u! y$ Q3 T. {5 d - motor_10.run(Power);
5 x4 ?, B# A- M. V( N# D% `% P - }
複製代碼 % s# ~, U8 L5 U0 g" n+ {( D
2 k2 t; d6 `4 ? s |
|