|
|
- #include <Arduino.h>
8 v' T( L* }' b* T: O$ o# r- v& S4 K - #include <Wire.h>3 u9 x5 ^7 ]: l/ p' F! j
- #include <Servo.h>
6 f' w" X; S1 {5 T+ O( \$ |# ]
1 h( e" m2 L7 j% k6 N/ q- #include "MePort.h" v2 R& P& N2 z' t/ c* e) i
- #include "MeUltrasonic.h"
+ G% D" S# E9 w% w N* b - #include "MeDCMotor.h"/ a& {( \: H& g6 ~7 ]+ x+ ^
8 P! Y6 [; S: M5 P0 m! U% R, K- //double Input, Output ;, P9 O& J( N* ^; C; ?8 R
- float MaxSpeed = 255;
0 g- ?1 O. s, U; s- |) C) k8 x - float MaxPower = 180;8 K% i5 y2 l' U
- float MinPower = 120;
) P& t6 `* N2 q3 F K# R) ` - float Error,ErrorAcc,ErrorDec;
! b* k% `1 K3 F' }$ t
9 n9 t6 }* k6 b+ n( a- float Kp=0.14;
3 U5 d5 y- F0 R) G - float Kd=0.00020;//23;
) _# d2 p7 V' d - float Ki=0.000201;! x: ^) P# w9 b' R9 g
7 j5 D5 u3 G( N, r- float nPower;
* i4 E7 k6 K% N8 o% y* x5 h - MePort lightsensor_6(6);9 q' R+ H# E$ J% \9 A( X
- MePort lightsensor_8(8);
: Y' U7 A, i8 x - MeDCMotor motor_9(9);4 h+ ?4 K P) P& l7 \
- MeDCMotor motor_10(10);
2 w. N+ q: Z+ t+ ]# P2 ~ - unsigned long previousMillis = 0;
7 r% E% S$ ]- O+ h3 {6 u8 d - const long interval = 1;
- `+ m' m' j, S2 K% S, B - 2 w2 j) j( O4 [6 s( a! M8 [
- void setup(){
5 O$ v6 w! B3 Z - lightsensor_6.dWrite1(1);6 u! n1 C; Y r' d
- nPower = 160;
' Y; _6 i: J) A. I - Error=0;
/ w& E4 ^6 K4 e; S4 g7 y/ i0 j - ErrorAcc=0;$ C9 L6 T7 |. |6 t b
- }
( C" @% f& ^. {5 q+ p - : o% \2 T) o. `$ T: l
- void loop(){, a- ?/ ]- b. C
- unsigned long StartTime = millis();
) P) ^2 P7 L3 b" X' y- m0 f - if(ErrorAcc < 18000 && ErrorAcc > -18000){+ M" r- u; Z2 z: T, K
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
( e7 X1 P9 u3 ?" i; y - ErrorAcc += nError*Kd ;9 j" O1 Z& I* g. m! T# Z/ V
- ErrorDec -= nError*Ki ;4 P U7 O. b7 A, s% W9 [% {1 h& ?0 U, B
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
9 }$ I' O' b" ]; H+ s - if(nError < 80 && nError > -80){* k. o( i. G v4 I7 h$ z% L. O; t
- if(nPower < MaxPower){
( a& B9 ~8 _3 l6 g$ G' Q+ T - nPower += 3; Z3 l- X# Y0 d* B( w
- }
! @! `# @3 [. x) H - } else{
- Q! r" H; A2 X, M2 D& ]$ G; h - if(nPower > MinPower){' ]: ?" V+ z4 Z$ r/ o. L% k
- nPower -= 2;
9 j, x- A8 A$ h! t4 F. C+ e8 m9 V - }6 q3 P& f: a& B; f) L3 e
- }
4 |2 V( f/ }5 m5 J4 X2 Y7 G, _, } - MotoL(nPower-Error);
1 ~; N; p1 b1 h' x/ q- B - MotoR(nPower+Error); ; D' ?; L P/ v) m7 B. o- J' e6 P
- }else{
' S( F7 B; h1 I+ v - motor_9.run(0); `2 T- ^! ^ N6 H" U; v
- motor_10.run(0);
8 @' L4 [; h6 _1 u8 `. G& g7 O7 x - }' p+ ] `/ @2 T; K- f
- do{}while(millis() - StartTime < interval);
5 Q* M: c* ^! B7 X* m9 I - }
# v# V" C2 e7 Y1 L- R: u. |
4 c1 m+ s' ^+ O/ |1 r- void MotoL(int Power){0 b3 @. V7 Q& S3 T( e6 f, t
- if (Power > MaxSpeed){
, |: e4 h7 M( p4 r% f, j/ J3 J' t0 O" s - Power = MaxSpeed;3 o! L7 N0 Y9 S
- } 6 B" q, ?8 g4 x9 I( [/ L
- if (Power < -MaxSpeed){$ r3 Q' c W! F: d" N
- Power = -MaxSpeed;
! X4 G8 A3 p; f, h* E - }
G! U' x8 O& ?" o3 @ - motor_9.run(Power);: }! M- l9 P; x# q7 S
- }
$ @, e$ |" H. T3 y' `* c - , i; T- s; m6 K& P. Y
- void MotoR(int Power){
7 U; L! b5 w7 o% C - if (Power > MaxSpeed){& Y! j- |. E7 J6 ?
- Power = MaxSpeed;
! [. v( G0 c0 h) N - }
* V6 o. y/ W+ m8 `! Y - if (Power < -MaxSpeed){
! V1 n; p" G- `. r5 M Y, V8 h5 ~ - Power = -MaxSpeed;* Y! L2 u& ^- C1 ]0 f9 G s0 |
- }
( f( c0 C- W* o" x9 ~/ @+ [ - motor_10.run(Power);
9 Q& B O: c6 R6 H( x! o. w - }
複製代碼 4 h( ]" l7 m9 o3 u% f2 G/ {
- L& K+ ]5 z; `- Q( C |
|