/** * 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); }