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);