|
|
- #include <Arduino.h>
) V( X2 N$ F8 I( `- O/ D. J8 E - #include <Wire.h>
1 D$ ]2 P9 v/ e& B - #include <Servo.h>
9 J y- o; N1 U4 Q. N3 j2 D/ T3 h/ r - 2 A7 n! G6 q: @3 @
- #include "MePort.h"
2 N" C# n+ E# G; E- V, ]; Y - #include "MeUltrasonic.h"
/ M* a6 e4 g; i" ]+ ?7 @/ S- t - #include "MeDCMotor.h"0 H9 @" n1 R; W" Z% X9 i
- : l0 ^9 R2 W5 ?& i9 j- Q/ p
- //double Input, Output ;2 x, j& q) q' m, I- W2 {' H
- float MaxSpeed = 255;
7 ~9 O7 u5 \+ w: ] - float MaxPower = 180;
# H% H% ^1 ^# {9 e- V - float MinPower = 120;
0 Z+ n1 ]8 p. [8 `! B& v; U - float Error,ErrorAcc,ErrorDec;* S3 N! p, w4 F n& ?
' A1 }" b$ i) i+ z$ i3 ~- float Kp=0.14;; ?1 Y: e" ~6 B6 y* a, f
- float Kd=0.00020;//23;4 a, Z8 S9 s( Q* r' `! \
- float Ki=0.000201;
/ z; q. m% K( P* x+ }: x
3 q( y1 i3 r- U4 V. p/ Y- float nPower;
0 C& Q: g# r9 ~1 U - MePort lightsensor_6(6);
; Y! u' X. I4 `/ Y6 s, J! d - MePort lightsensor_8(8);7 ?7 w0 J. v+ ` ~: V
- MeDCMotor motor_9(9);' N" E: {6 h. b. f! `' Z
- MeDCMotor motor_10(10);. B* P; h9 l! c. R# A) ]
- unsigned long previousMillis = 0;" Z" }, h. x+ y$ h F s0 `
- const long interval = 1;
$ f! @3 U- K/ l; S8 \
& a, d, }$ k5 P. \" s- z- void setup(){
) b7 i; R9 _1 ~5 a: B - lightsensor_6.dWrite1(1);. y& z; S, C& j, g5 K( t3 ?- R
- nPower = 160;
9 f! g' N/ P1 j! C* V+ u - Error=0;- s; J9 b" N0 O' N% U
- ErrorAcc=0;
) H5 m8 x; w9 R* g" g" o - }
. O% f$ r' i4 D# A7 R/ T$ k5 y: |7 Z
; [, K5 M% q1 h2 H9 R3 h, t. h- void loop(){
W- A: \" a8 e: q/ y2 L - unsigned long StartTime = millis();
5 U! d0 b/ S2 F2 V% O: | - if(ErrorAcc < 18000 && ErrorAcc > -18000){
& V7 b- o" Z- f8 Z7 g! w" W - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();0 a/ H( s2 Z7 `
- ErrorAcc += nError*Kd ;
2 J: c2 F4 o7 ?# t5 }' A, r - ErrorDec -= nError*Ki ;+ ]+ t6 N7 A) K( _, \& d
- Error = int(nError*Kp+ErrorAcc+ErrorDec);# }1 p0 c, k" g" [1 V
- if(nError < 80 && nError > -80){' P6 C5 m- V$ U1 f( x6 `" U
- if(nPower < MaxPower){
; @+ t0 U: Q L% U& d' Y* ^! O - nPower += 3;8 I6 @! G* h4 ^* T4 [
- }% Q+ m' P. p. \8 f
- } else{) v4 G1 G. U' ~; R) B
- if(nPower > MinPower){
3 K- G. E0 i: m7 a - nPower -= 2;, o' P. e( z/ A. x5 p+ f
- }9 t& w" T( e$ F/ u! s9 v0 D
- } * h$ ^2 G4 L' }6 E9 @
- MotoL(nPower-Error);2 G* p, O! M. e# r+ _/ j" x+ O, ^
- MotoR(nPower+Error); 4 r* Z# H- J$ y( F, M/ g4 e( n
- }else{% L8 M/ c: T! a: T
- motor_9.run(0);+ t* z- y# a& |' `2 k* {( ]
- motor_10.run(0);
1 S* I2 {' L* t, G3 Z3 e - }3 o' M4 T/ ?, _ N
- do{}while(millis() - StartTime < interval);) Z3 o1 B; x# \5 n9 \
- }; z& ]/ s2 W2 I* I/ R5 Y( p8 k* Y6 p
( R' ]7 q5 p8 I- void MotoL(int Power){# n9 V, o; Q; K* v9 U8 e1 E$ |
- if (Power > MaxSpeed){
% E0 G w! Q- m$ C1 ~& _4 T# i - Power = MaxSpeed;1 E' @# T: K& R7 r# r' Y9 z! ~5 G
- }
V6 W+ B. L u4 z& v1 F4 t* p5 C4 P - if (Power < -MaxSpeed){0 a, k ` ]0 ~/ G
- Power = -MaxSpeed;: a! P! {6 L6 s: g( {# @4 q
- } ) Z' Q; |- l/ y- K) O4 b
- motor_9.run(Power);
+ ]5 Z4 g- V! K' j( [6 G - } o8 i+ J, q1 W6 o3 D2 r% n- b
. | l5 C; D& o& ~& ^4 b- void MotoR(int Power){
* z5 @& g! `9 c; @ - if (Power > MaxSpeed){
+ a. ]+ t3 H, v& V* b - Power = MaxSpeed;0 _2 M2 W4 H; i; h+ v, M( q8 }) X* p
- }
) L. |, V4 h- v6 i$ @ - if (Power < -MaxSpeed){( H8 N4 L% X5 f7 p
- Power = -MaxSpeed;0 t R/ i# l0 ?
- } 4 p% [# k0 ]4 _
- motor_10.run(Power);
: C$ i0 `2 R' H5 ?, { - }
複製代碼
, {& i0 e" P5 m9 {* ?- c
) W% [- y( k, L6 R |
|