/**
* A szabályozási folyamatért felelős fgv
* */
public override void Run(APresenter _in)
{
run = true;
getInput();
// state[0] - Angle
// state[1] - Position
double[] state = Process.get();
double epsilon = 0.001;
double[] u = new double[] { 0.0 };
while (run && !(state[1] > (reference - epsilon) && state[1] < (reference + epsilon)))
{
state = Process.get();
_in.updateDraw(state);
// Ha a referenciánál kisebb, akkor jobbra megy, ha nagyobb, akkor (lassabban) balra
if (state[1] <= (reference - epsilon))
{
u[0] = 1.0;
}
if (state[1] > (reference + epsilon))
{
u[0] = -0.3;
}
Process.set(u);
_in.updateLog(new string[] { DateTime.Now.ToString("HH:mm:ss.fff"), state[0].ToString("f5"), state[1].ToString("f5"), u[0].ToString("f5") });
Thread.Sleep(25);
state = Process.get();
}
Process.set(new double[] { 0.0 });
for (int i = 0; i < 5; i++)
{
state = Process.get();
Thread.Sleep(25);
}
}
/**
* A szabályozási folyamatért felelős fgv
* */
public override void Run(APresenter _in)
{
run = true;
getInput();
// state[0] - Angle
// state[1] - Position
double[] state = Process.get();
double epsilon = 0.001;
#region Init
oldTime = DateTime.Now.Millisecond;
state = Process.get();
oldError = reference - state[1];
#endregion
// Addig megy a szabályozási folyamat, amíg a referencia érték epsilon sugarú körébe nem kerül a pozíció
while (run && !(state[1] > (reference - epsilon) && state[1] < (reference + epsilon)))
{
state = Process.get();
_in.updateDraw(state);
newTime = DateTime.Now.Millisecond;
double[] u = new double[] { 0.0 };
//PID logika
double error = reference - state[1];
I = I + error;
double D = (error - oldError) / (newTime - oldTime);
u[0] = clap(clap(Kp * error) + clap(Ki * I) + clap(Kd * D));
if( Double.IsNaN(u[0]))
{
u[0] = 0.0;
}
Process.set(u);
_in.updateLog(new string[] { DateTime.Now.ToString("HH:mm:ss.fff"), state[0].ToString("f5"),state[1].ToString("f5"), u[0].ToString("f5") });
oldTime = newTime;
oldError = error;
Thread.Sleep(25);
state = Process.get();
}
double[] stop = new double[] { 0.0 };
Process.set(stop);
}
