public override void Update(Vector3D setpoint, Vector3D actual, float deltaTimeGain)
{
//Proportional Error
Vector3D.Subtract(setpoint, actual, _proportionalError);
Vector3D.Multiply(_proportionalError,Settings.ProportionalGain,_proportionalErrorOutput);
//Intergral Error
Vector3D.Multiply(_proportionalError, deltaTimeGain, _integralErrorStep);
Vector3D.Add(_integralErrorStep, _integralError, _integralError);
Vector3D.Constrain(_integralError, _negativeWindupLimit, Settings.WindupLimit);
Vector3D.Multiply(_integralError, Settings.IntegralGain, _integralErrorOutput);
//Derivative Error
Vector3D.Subtract(actual, _lastActual, _derivativeError);
Vector3D.Divide(_derivativeError, deltaTimeGain * 100, _derivativeError);
Vector3D.Multiply(_derivativeError, Settings.DerivativeGain, _derivativeErrorOutput);
//Store last actual
Vector3D.Copy(actual, _lastActual);
//Store Output
Vector3D.Copy(_proportionalErrorOutput, Output);
Vector3D.Add(Output,_integralErrorOutput, Output);
Vector3D.Add(Output, _derivativeErrorOutput, Output);
}
public override void Update(float throttle, Vector3D output)
{
_frontMotor.Update(throttle - output.X + output.Z);
_rearMotor.Update(throttle + output.X + output.Z);
_leftMotor.Update(throttle - output.Y - output.Z);
_rightMotor.Update(throttle + output.Y - output.Z);
Debug.Print(
"F: " + (throttle - output.X + output.Z) +
", Rr: " + (throttle + output.X + output.Z) +
", L: " + (throttle - output.Y - output.Z) +
", R: " + (throttle + output.Y - output.Z));
}
public Radio(float throttle, Vector3D angularVelocity, bool gear)
{
Throttle = throttle;
AngularVelocity = angularVelocity;
Gear = gear;
}
public Gyro(Vector3D angularVelocity)
{
AngularVelocity = angularVelocity;
}
public static void Subtract(Vector3D vector3D1, float scalar, Vector3D vector3Dout)
{
vector3Dout.X = vector3D1.X - scalar;
vector3Dout.Y = vector3D1.Y - scalar;
vector3Dout.Z = vector3D1.Z - scalar;
}
public static void Subtract(float scalar, Vector3D vector3D2, Vector3D vector3Dout)
{
vector3Dout.X = scalar - vector3D2.X;
vector3Dout.Y = scalar - vector3D2.Y;
vector3Dout.Z = scalar - vector3D2.Z;
}
public static void Subtract(Vector3D vector3D1, Vector3D vector3D2, Vector3D vector3Dout)
{
vector3Dout.X = vector3D1.X - vector3D2.X;
vector3Dout.Y = vector3D1.Y - vector3D2.Y;
vector3Dout.Z = vector3D1.Z - vector3D2.Z;
}
public static void Multiply(Vector3D vector3D1, float scalar, Vector3D vector3Dout)
{
vector3Dout.X = vector3D1.X * scalar;
vector3Dout.Y = vector3D1.Y * scalar;
vector3Dout.Z = vector3D1.Z * scalar;
}
public static void Multiply(Vector3D vector3D1, Vector3D vector3D2, Vector3D vector3Dout)
{
vector3Dout.X = vector3D1.X * vector3D2.X;
vector3Dout.Y = vector3D1.Y * vector3D2.Y;
vector3Dout.Z = vector3D1.Z * vector3D2.Z;
}
public static void Divide(Vector3D vector3D1, float scalar, Vector3D vector3Dout)
{
vector3Dout.X = vector3D1.X / scalar;
vector3Dout.Y = vector3D1.Y / scalar;
vector3Dout.Z = vector3D1.Z / scalar;
}
public static void Copy(Vector3D vector3D, Vector3D vector3Dout)
{
vector3Dout.X = vector3D.X;
vector3Dout.Y = vector3D.Y;
vector3Dout.Z = vector3D.Z;
}
public static void Constrain(Vector3D vector3D, Vector3D min, Vector3D max)
{
vector3D.X = Logic.Constrain(vector3D.X, min.X, max.X);
vector3D.Y = Logic.Constrain(vector3D.Y, min.Y, max.Y);
vector3D.Z = Logic.Constrain(vector3D.Z, min.Z, max.Z);
}
public static void Add(Vector3D vector3D1, Vector3D vector3D2, Vector3D vector3Dout)
{
vector3Dout.X = vector3D1.X + vector3D2.X;
vector3Dout.Y = vector3D1.Y + vector3D2.Y;
vector3Dout.Z = vector3D1.Z + vector3D2.Z;
}
public Accelerometer(Vector3D acceleration)
{
Acceleration = acceleration;
}
public abstract void Update(float throttle, Vector3D output);
public abstract void Update(Vector3D setPoint, Vector3D actualPosition, float deltaTimeGain);
