|
|
- #include <Arduino.h>% o' v0 P( {8 ~ f3 K& A
- #include <Wire.h>% \! r i4 c: m1 v% P& D
- #include <Servo.h>
4 @1 p' a9 u; r2 H J4 p: P6 p
* U; M, C8 M; D0 y7 P- #include "MePort.h"
# I- d$ s2 i. ~) B - #include "MeUltrasonic.h"1 b% U2 V3 ]+ e
- #include "MeDCMotor.h"
8 p9 c- N0 w |' C1 d3 ]% }) m! l& L - 1 W0 D9 ]& C1 a: x
- //double Input, Output ;
8 ]9 u2 P0 B4 |; m4 X - float MaxSpeed = 255;7 a# s- }: l1 e. s! b
- float MaxPower = 180;- d/ R) H% S& C: G3 J' V
- float MinPower = 120;
+ q2 T2 w$ j7 [. I - float Error,ErrorAcc,ErrorDec;
& [; C9 \) i0 b' b) ?: V$ k - 4 v; ?# a# d" t& W' X) o, Z p
- float Kp=0.14;
/ \1 [* S% J$ s! y- v - float Kd=0.00020;//23;
3 H5 G; b& y J - float Ki=0.000201;
* k3 U( Z0 ]2 y! a$ h7 k4 b
9 ?) L% D/ Y% \0 Q- float nPower;/ w9 S g2 R- U& i) p
- MePort lightsensor_6(6);
4 v3 y$ c& a& Q6 l: ?- O4 \- c4 _ - MePort lightsensor_8(8);
3 x9 ~) I7 J, J! P - MeDCMotor motor_9(9);. M. A/ E& p) y; [8 a" T
- MeDCMotor motor_10(10);4 D p' B) c: [& f9 t X! d' h
- unsigned long previousMillis = 0;& z) O3 Y9 U/ F: J$ f
- const long interval = 1;
# ^1 E% ]: z) U( `* G% \% L - 9 C$ p0 T/ O- ]* h0 h
- void setup(){
* v9 |, f6 R0 g* s% m - lightsensor_6.dWrite1(1);6 m" Y2 A5 U8 `% e; D
- nPower = 160;
( j2 q0 q6 Z: i- B/ Z, O) Z A - Error=0;2 l% \% l. M6 s$ k' @% B; J* l1 `
- ErrorAcc=0;5 q3 Q7 z4 F" s5 e
- }. }4 Q N9 n, L" s5 T0 K6 ^* Q
' E: L" W% I0 i1 W- void loop(){) a' d# r; n. |$ o0 o& S
- unsigned long StartTime = millis();+ a2 Y& o/ n+ \( E" a: d
- if(ErrorAcc < 18000 && ErrorAcc > -18000){, a3 U6 F7 d' F% j4 r2 M
- float nError = lightsensor_6.aRead2() - lightsensor_8.aRead2();
+ A5 x3 @- P1 _. Z7 D - ErrorAcc += nError*Kd ;
' ?/ z8 G4 ?( L* E$ O# y - ErrorDec -= nError*Ki ;1 A( N5 b. \% V( v, ~) `" g3 A
- Error = int(nError*Kp+ErrorAcc+ErrorDec);
2 j" T( z, e/ q& @6 K1 ^ - if(nError < 80 && nError > -80){& x! r* q9 q1 q' I) e3 \
- if(nPower < MaxPower){
) V( Q# V% p$ }0 j/ v: B6 h& y! M - nPower += 3;+ [8 x0 f% f& @" w1 b' }
- }9 k" w- }0 t5 K0 e& M6 T( P
- } else{# _/ k F4 N7 W3 v8 c' ?* v
- if(nPower > MinPower){# R5 o( l+ t1 C, {* w1 h" O, A8 l
- nPower -= 2;+ {' J) o8 J) L9 N6 q5 q; i
- }& M6 d* F5 E/ E5 B
- } , F7 ?5 S# w9 y- q1 Z' E3 V
- MotoL(nPower-Error);; A! x$ z6 k5 J" ~- V) [
- MotoR(nPower+Error);
: X6 u: G- V* \+ }2 m - }else{
* i& w- o6 H. c) C- `/ O" s0 y - motor_9.run(0);
( F6 b ]) K9 [ - motor_10.run(0);; {! J* p7 C" m1 |- a
- }
3 h( p+ m4 e. R* @( X - do{}while(millis() - StartTime < interval);: x1 E" F, v% M0 D% D( k2 B& ~5 d
- }
2 r1 q- V* d' u$ K- Y - A% ]; l8 j+ Z2 b6 ~
- void MotoL(int Power){
4 o C0 \) s+ u8 n0 v - if (Power > MaxSpeed){
1 x! u) Q) H, R2 R- A+ J$ o - Power = MaxSpeed;8 |6 A8 e1 E0 R4 ]9 Z
- }
" [' g6 A' ?9 R q; G' T - if (Power < -MaxSpeed){! A3 C' r. [7 m% P0 k6 R
- Power = -MaxSpeed;
$ H/ r+ Z* J$ J% d: ?8 O' X$ p - } ! j M0 x W. u* P* ~
- motor_9.run(Power);
; {6 |+ s3 e8 f, P4 Z1 }4 O - } + N$ T4 i# h$ `6 A% q" H2 p h2 \6 I) h
1 w T* d* l1 K5 x# R& @7 ^6 f- void MotoR(int Power){
. m1 H1 D5 h {0 Z+ o - if (Power > MaxSpeed){
% P. S* w3 y' x$ b, ~4 i h. [ - Power = MaxSpeed;
5 i5 `$ U& N$ \( d1 n' ^' H - } ) t. e7 y- d* ?7 r" m. u' Y" q. p
- if (Power < -MaxSpeed){9 I$ I/ Q1 R8 Q0 ?; ^0 ?
- Power = -MaxSpeed;
4 }, @( ^1 I$ J" Y. i - }
7 _8 c" H7 n2 ]# O. K+ ]0 a" `1 t+ @( S - motor_10.run(Power);4 ~7 s! S, }8 ^! _! U
- }
複製代碼 $ o w, n% J! `0 p5 m7 H& E, j
9 f( R7 U! E5 E4 l
|
|