|
|
- #include <Arduino.h>
Y2 R: g7 s; G - #include <Wire.h>
- H: l/ e- c- P e - #include <Servo.h>7 P# W- A7 K$ U6 `+ X& Z, `2 S+ ~$ U
- 6 r J! n+ K) I" L7 J; S1 \
- #include "MePort.h"9 [% `) C$ [; E9 ]; b( }" Y, {
- #include "MeUltrasonic.h"3 J3 A9 D. T' w; A/ ~
- #include "MeDCMotor.h"
4 i& k( W( N1 S2 b' Z1 O% }9 ^
8 F# p( j; x* E1 Z6 M- //double Input, Output ;
) ]; i. g( a* [+ D* v - float MaxSpeed = 255;
5 Z9 k2 L4 r) T! i% P - float MaxPower = 180;
, e& O" S( _, |% l' N' o - float MinPower = 120;/ ^+ L: y1 `9 R' `" K, ?
- float Error,ErrorAcc,ErrorDec;, C8 i7 K G+ e0 E9 u- w
- & e* ^' z1 i8 i* M. ]
- float Kp=0.14;( f+ `' o) b3 i6 J
- float Kd=0.00020;//23;5 o: B: A: Q+ H$ \
- float Ki=0.000201;
5 C! o8 E* y, A+ G - 6 Y# ^ s2 c- y* W$ e" X$ g+ `* a- [
- float nPower;7 ~3 x7 s) z6 Q. Z/ t9 K
- MePort lightsensor_6(6);
8 `. X, I9 [; e - MePort lightsensor_8(8); |' j* A1 ~3 h; {6 E: w, m2 }2 I8 U
- MeDCMotor motor_9(9);
, f) W+ s+ ~/ A" m- Z - MeDCMotor motor_10(10);* {) `0 E4 H& K
- unsigned long previousMillis = 0;3 J9 V. U. q7 O
- const long interval = 1;
2 L I- Q+ x4 g9 x0 C7 g
$ e V; \ T9 O& d x$ W- void setup(){
/ g8 v6 K# R4 \2 E7 m0 q - lightsensor_6.dWrite1(1);
- @8 K; U3 P# ~ - nPower = 160;
, T: h0 x2 |" _4 l {( j - Error=0;
2 P: l. k' R( ^; Q- x! K4 { - ErrorAcc=0;% |: P; R+ e" { H9 L
- }; y: R( f4 F5 j! ~
- $ p8 N( b( A K! m, z
- void loop(){
6 y0 T( a9 |0 c1 D n - unsigned long StartTime = millis();) }2 n) x% ~7 ]' X$ o6 r9 [- j
- if(ErrorAcc < 18000 && ErrorAcc > -18000){
9 R) E- }0 j( Y# o( b3 q+ D - float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();8 A8 B; m& Y$ D4 ~, y. V I0 `. A
- ErrorAcc += nError*Kd ;
* E' [5 q! U( S' m" `7 a% o. [ - ErrorDec -= nError*Ki ;& }/ I7 k* T/ C6 c
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
9 o2 z" d7 n$ x7 e1 y+ A: Q - if(nError < 80 && nError > -80){3 O4 c+ R# C& i& d; l
- if(nPower < MaxPower){8 s0 F4 B, z, C' J9 g G
- nPower += 3;
- k3 G% @0 w) }+ i* L _ - }
7 D) c) K" r! z$ u' B9 H: e - } else{
4 I, f# d3 f* D. r4 `) S6 w5 A - if(nPower > MinPower){1 v5 F3 h) t1 M0 e6 P( I ^
- nPower -= 2;$ p3 \7 H! m6 M! d( a
- }- b* ] ], f, m: ]) O
- } 7 m" C# }' K% p$ F
- MotoL(nPower-Error);
+ S# e/ n7 t2 E" U. H - MotoR(nPower+Error); 7 p) O( L, }& X2 {& t
- }else{
1 F7 P% p# t+ q- @6 K) T+ l% L - motor_9.run(0);, f4 M5 E" K4 u/ C# o A O% g% ?5 b
- motor_10.run(0); N. x# O& c* }, ]
- }
4 v: f! v1 E- x s# E) U, `" S- A - do{}while(millis() - StartTime < interval);( m7 y' X8 i- O, {
- }- _- V5 [8 _$ `$ ]
- ' Z5 ?! ?3 a5 W2 r
- void MotoL(int Power){
2 [/ H# i4 B+ ~: l- f' d) t; g - if (Power > MaxSpeed){
" ]) m7 [. J5 f0 W5 i% h - Power = MaxSpeed;
. D) ]+ N: _( | - } 4 A/ f- n# q9 E: r* w
- if (Power < -MaxSpeed){
; h- i* N" Y+ M: n - Power = -MaxSpeed;) U) _! _' [7 R1 H
- } * D v% e# |! u, c/ i8 T+ ^7 O
- motor_9.run(Power);% d* ?1 s6 k% @+ R( x- g
- }
8 s1 o" C' p4 P6 \* i) C2 F% v - + ]4 Q1 y2 X, O0 u
- void MotoR(int Power){6 B. n+ n) y Y+ m! d0 W) ?
- if (Power > MaxSpeed){
^8 A5 A6 D5 _ |" K - Power = MaxSpeed;
9 Z7 K* b" B& O* O. k - }
$ P; ~% l3 Y; b! W# X0 m" c+ S& u- l - if (Power < -MaxSpeed){+ q/ v i9 S( w
- Power = -MaxSpeed;
! J0 D! i: f9 ^4 }" G& @3 O - } 1 l, w6 S4 O/ x h
- motor_10.run(Power);4 } w, }) N( b, l# x# H! U: [
- }
複製代碼 9 H% P# S9 X" V
: N/ z9 q& H3 @3 u; ?" I
|
|