public void Tick()
{
// track angle
int a = this.Motor.Angle - _zero;
if (a - this.Angle < -Circle.Limit / 2)
{
a += Circle.Limit;
}
if (a - this.Angle > Circle.Limit / 2)
{
a -= Circle.Limit;
}
this.Angle = a;
_angleHistory.Add(this.Angle);
if (!_on)
{
this.Motor.Torque = 0;
return;
}
var f = _maxTorque;
var m = _m;
// target time and angle
int now = Environment.TickCount;
var t2 = _targetTime - now;
var a2 = Circle.Distance2(_targetAngle, this.Angle);
// calc actual current velocity
var ago = _angleHistory.Get(2);
var v = (float)(this.Angle - ago.Angle) / (now - ago.TickCount);
// calc minimum and maximum acceptable velocity
// that will allow us to finish the trajectory
// maximum acceptable time until breaking
var maxT1 = Quadratic.Solve(f / m * 3 / 2, -2 * t2 * f / m, t2 * t2 * f / m / 2);
var t1 = Math.Max(maxT1[0], maxT1[1]);
// mininum acceptable velocity until breaking
var minV = (t2 - t1) * f / m;
// maximum acceptable velocity
var maxV = t2 * f / m;
// optimal velocity
var optV = (minV + maxV) / 2;
}
public void Tick()
{
// track angle
int a = this.Motor.Angle - _zero;
if (a - this.Angle < -Circle.Limit / 2)
{
a += Circle.Limit;
}
if (a - this.Angle > Circle.Limit / 2)
{
a -= Circle.Limit;
}
this.Angle = a;
//
_recentBuffer.Add(this.Angle);
if (!_on)
{
this.Motor.Torque = 0;
return;
}
int now = Environment.TickCount;
float fraction = (float)(now - _startTime) / (_targetTime - _startTime);
if (fraction < 0)
{
fraction = 0;
}
if (fraction > 1)
{
fraction = 1;
}
var desiredAngle = Circle.Fraction(_startAngle, _targetAngle /*, _direction*/, fraction);
var error = Circle.Distance2(this.Angle, desiredAngle);
//error = (int)_ema.Calc(error);
// proportional
var proportionalTorque = error * _power;
// integral
var maxError = (int)((_maxTorque - proportionalTorque) / _integral);
_integralTorque += (/*Sqrt(error)*/ error * _integral);
if (_integralTorque + proportionalTorque > _maxTorque)
{
_integralTorque = _maxTorque - proportionalTorque;
}
if (_integralTorque + proportionalTorque < -_maxTorque)
{
_integralTorque = -_maxTorque + proportionalTorque;
}
// derivative
var ago = _recentBuffer.Get(3);
var d = (float)(this.Angle - ago.Angle) / (now - ago.TickCount);
var derivativeTorque = d * _derivative;
_integralTorque += derivativeTorque;
//
if (_debug)
{
Debug.WriteLine("a={0}, da={1} | P={2}, I={3}, D={4} | err={5}, int-err={6}", this.Angle, desiredAngle, proportionalTorque, _integralTorque, derivativeTorque, error, _integralTorque);
}
var torque = (int)(proportionalTorque + _integralTorque);
if (torque < -_maxTorque)
{
torque = -_maxTorque;
}
if (torque > _maxTorque)
{
torque = _maxTorque;
}
this.Motor.Torque = torque;
}