public void SetThrust(Vector3D deltaV, bool align)
{
Base6Directions.Direction direction = Base6Directions.GetClosestDirection(deltaV);
Base6Directions.Direction oppositeDirection = Base6Directions.GetOppositeDirection(direction);
//Program.Echo($"\nDirection: {direction.ToString()}");
//Program.Echo($"\nOpposite: {oppositeDirection.ToString()}");
float maxThrust = MaxThrusters[direction];
//double decayFactor = 0.8;
//if (align && direction != Base6Directions.Direction.Forward || direction != Base6Directions.Direction.Backward)
// decayFactor = 0.25;
//double acceleration = MathHelper.Clamp(maxThrust / Remote.CalculateShipMass().TotalMass, 0.1, Math.Pow(deltaV.Length(), decayFactor));
//Drone.LogToLcd($"{direction.ToString()} {acceleration.ToString()}m/s/s");
double acceleration = 0;
var freq = this.Program.Runtime.TimeSinceLastRun.TotalMilliseconds / 1000;
if (direction == Base6Directions.Direction.Left || direction == Base6Directions.Direction.Right)
{
acceleration = XPID.CorrectError(deltaV.Length(), freq);
}
else if (direction == Base6Directions.Direction.Up || direction == Base6Directions.Direction.Down)
{
acceleration = YPID.CorrectError(deltaV.Length(), freq);
}
else if (direction == Base6Directions.Direction.Forward || direction == Base6Directions.Direction.Backward)
{
acceleration = ZPID.CorrectError(deltaV.Length(), freq);
}
double force = Remote.CalculateShipMass().TotalMass *acceleration;
foreach (IMyThrust thruster in Thrusters[direction])
{
thruster.Enabled = true;
thruster.ThrustOverride = (float)force;
}
foreach (IMyThrust thruster in Thrusters[oppositeDirection])
{
thruster.ThrustOverride = 0f;
thruster.Enabled = false;
}
}
public bool AlignBlockTo(Vector3D vPos, IMyTerminalBlock block = null)
{
if (block == null)
{
block = Remote;
}
Matrix m = block.WorldMatrix;
Vector3D forward = m.Forward;
Vector3D left = m.Left;
Vector3D up = m.Up;
Vector3D vTarget = (vPos - block.GetPosition());
vTarget.Normalize();
// Check angle.
var angleF = VectorHelper.AngleBetween(vTarget, m.Forward);
this.Program.Echo(string.Format("Angle: {0}", angleF));
if (angleF <= 0.01)
{
DisableGyroOverride();
return(true);
}
double yaw, pitch;
GetRotationAngles(vTarget, forward, left, up, out yaw, out pitch);
// Correct with PID.
var freq = this.Program.Runtime.TimeSinceLastRun.TotalMilliseconds / 1000;
pitch = PitchPID.CorrectError(pitch, freq) * 0.1; // * speedFactor; apply factor to reduce the speed if needed.
yaw = YawPID.CorrectError(yaw, freq) * 0.1; // * speedFactor; apply factor to reduce the speed if needed.
// Apply gyro overrides.
ApplyGyroOverride(pitch, yaw, 0, Gyros, m);
// Return not aligned for now. Will keep aligning :)
return(false);
}