private Vector3D GetReferenceVector(ShipControlCommons shipControl,
Base6Directions.Direction direction)
{
var offset = shipControl.Reference.Position + Base6Directions.GetIntVector(direction);
return(Vector3D.Normalize(shipControl.Reference.CubeGrid.GridIntegerToWorld(offset) - shipControl.ReferencePoint));
}
private Vector3D GetTarget(ShipControlCommons shipControl,
Vector3D direction)
{
// Vector from start to current
var startVector = shipControl.ReferencePoint - StartPoint;
// Determine projection on start direction vector
var startDot = startVector.Dot(direction);
Vector3D targetVector;
if (startDot >= 0.0)
{
// Set targetVector to that projection
targetVector = startDot * direction;
}
else
{
// Or set it to the start (0,0,0) if the ship is
// behind the start position
targetVector = new Vector3D(0, 0, 0);
}
// Offset forward by some amount
targetVector += LOS_OFFSET * direction;
// Offset by difference between start and current positions
targetVector += StartPoint - shipControl.ReferencePoint;
return(targetVector);
}
private void Maneuver(ShipControlCommons shipControl, EventDriver eventDriver)
{
velocimeter.TakeSample(shipControl.ReferencePoint, eventDriver.TimeSinceStart);
// Determine velocity
var velocity = velocimeter.GetAverageVelocity();
if (velocity != null)
{
// Only absolute velocity
var speed = ((Vector3D)velocity).Length();
var error = ManeuveringSpeed - speed;
var force = thrustPID.Compute(error);
var thrustControl = shipControl.ThrustControl;
if (force > 0.0)
{
thrustControl.SetOverride(Base6Directions.Direction.Forward, force);
}
else
{
thrustControl.SetOverride(Base6Directions.Direction.Forward, false);
}
}
}
public void Init(ShipControlCommons shipControl,
Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
{
ShipForward = shipForward;
ShipUp = shipUp;
ShipLeft = Base6Directions.GetLeft(ShipUp, ShipForward);
var small = shipControl.Reference.CubeGrid.GridSize == 0.5f;
yawPID.Kp = small ? SmallGyroKp : LargeGyroKp;
yawPID.Ti = small ? SmallGyroTi : LargeGyroTi;
yawPID.Td = small ? SmallGyroTd : LargeGyroTd;
pitchPID.Kp = small ? SmallGyroKp : LargeGyroKp;
pitchPID.Ti = small ? SmallGyroTi : LargeGyroTi;
pitchPID.Td = small ? SmallGyroTd : LargeGyroTd;
rollPID.Kp = small ? SmallGyroKp : LargeGyroKp;
rollPID.Ti = small ? SmallGyroTi : LargeGyroTi;
rollPID.Td = small ? SmallGyroTd : LargeGyroTd;
yawPID.Reset();
pitchPID.Reset();
rollPID.Reset();
}
public void Init(ShipControlCommons shipControl,
Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
{
ShipForward = shipForward;
ShipUp = shipUp;
ShipLeft = Base6Directions.GetLeft(ShipUp, ShipForward);
var small = shipControl.Reference.CubeGrid.GridSize == 0.5f;
yawPID.Kp = small ? SmallGyroKp : LargeGyroKp;
yawPID.Ti = small ? SmallGyroTi : LargeGyroTi;
yawPID.Td = small ? SmallGyroTd : LargeGyroTd;
pitchPID.Kp = small ? SmallGyroKp : LargeGyroKp;
pitchPID.Ti = small ? SmallGyroTi : LargeGyroTi;
pitchPID.Td = small ? SmallGyroTd : LargeGyroTd;
rollPID.Kp = small ? SmallGyroKp : LargeGyroKp;
rollPID.Ti = small ? SmallGyroTi : LargeGyroTi;
rollPID.Td = small ? SmallGyroTd : LargeGyroTd;
yawPID.Reset();
pitchPID.Reset();
rollPID.Reset();
}
// Yaw/pitch/roll
public GyroControl Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D targetUp,
out double yawPitchError, out double rollError)
{
return(_Seek(shipControl, targetVector, targetUp,
out yawPitchError, out rollError));
}
public void Init(ShipControlCommons shipControl,
Base6Directions.Direction localForward = Base6Directions.Direction.Forward)
{
LocalForward = localForward;
LocalBackward = Base6Directions.GetFlippedDirection(LocalForward);
thrustPID.Reset();
}
private void Reset(ShipControlCommons shipControl)
{
shipControl.Reset(gyroOverride: false, thrusterEnable: true,
thrusterCondition: ThrusterCondition);
Mode = Modes.Idle;
SaveLastCommand(shipControl, null);
}
// Yaw/pitch/roll
public GyroControl Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D targetUp,
out double yawError, out double pitchError,
out double rollError)
{
return _Seek(shipControl, targetVector, targetUp,
out yawError, out pitchError, out rollError);
}
public void Init(ShipControlCommons shipControl,
Base6Directions.Direction localForward = Base6Directions.Direction.Forward)
{
LocalForward = localForward;
LocalBackward = Base6Directions.GetFlippedDirection(LocalForward);
thrustPID.Reset();
}
private bool HaveWorkingThrusters(ShipControlCommons shipControl,
Base6Directions.Direction direction)
{
// First pass: Look for a working, non-overridden thruster
var found = false;
var overridden = new List <IMyThrust>();
var thrusters = shipControl.ThrustControl.GetThrusters(direction);
thrusters.ForEach(thruster =>
{
if (thruster.IsWorking)
{
if (thruster.GetValue <float>("Override") > 0.0f)
{
// Thruster is overridden. Keep track of it.
overridden.Add(thruster);
}
else
{
// Found a good thruster
found = true;
}
}
});
// Depending on outcome, disable or zero-out overridden thrusters
overridden.ForEach(thruster =>
{
if (found)
{
// Disable and let good thrusters take care of it
thruster.SetValue <bool>("OnOff", false);
}
else
{
// No good thrusters. Zero-out override.
thruster.SetValue <float>("Override", 0.0f);
found = true;
// Note this means we will zero-out at most 1 thruster.
// For now, this is the desired effect.
}
});
if (!found)
{
// Final desperation move. Enable and zero-out overrides for
// all thrusters on this side.
thrusters.ForEach(thruster =>
{
thruster.SetValue <bool>("OnOff", true);
thruster.SetValue <float>("Override", 0.0f);
});
// Still return false, but we'll check again after a few ticks
}
return(found);
}
// Yaw/pitch only
public GyroControl Seek(ShipControlCommons shipControl,
Vector3D targetVector,
out double yawError, out double pitchError)
{
double rollError;
return(_Seek(shipControl, targetVector, null,
out yawError, out pitchError, out rollError));
}
private void ResetOrbit(ShipControlCommons shipControl)
{
var gyroControl = shipControl.GyroControl;
gyroControl.Reset();
gyroControl.EnableOverride(false);
Mode = Modes.Idle;
SaveLastCommand(shipControl, null);
}
private void OrbitInit(ShipControlCommons shipControl)
{
// Don't touch thrusters at all
shipControl.GyroControl.Reset();
shipControl.GyroControl.EnableOverride(true);
var forward = shipControl.ShipBlockOrientation.TransformDirection(VTVLHELPER_ORBIT_DIRECTION);
seeker.Init(shipControl,
shipUp: Base6Directions.GetPerpendicular(forward),
shipForward: forward);
}
private void SetTarget(ShipControlCommons shipControl)
{
// Only set if neither mining nor reversing
if (Mode == IDLE)
{
StartPoint = shipControl.ReferencePoint;
StartDirection = shipControl.ReferenceForward;
StartUp = shipControl.ReferenceUp;
StartLeft = shipControl.ReferenceLeft;
}
}
private double Perturb(ShipControlCommons shipControl, TimeSpan timeSinceStart, out Vector3D targetVector)
{
targetVector = Target - shipControl.ReferencePoint;
var distance = targetVector.Normalize(); // Original distance
var amp = ScaleAmplitude(distance);
var newTarget = Target;
newTarget += shipControl.ReferenceUp * amp * Math.Cos(PerturbTimeScale * timeSinceStart.TotalSeconds + RandomOffset);
newTarget += shipControl.ReferenceLeft * amp * Math.Sin(PerturbTimeScale * timeSinceStart.TotalSeconds + RandomOffset);
targetVector = Vector3D.Normalize(newTarget - shipControl.ReferencePoint);
return(distance);
}
public void Init(ShipControlCommons shipControl,
Base6Directions.Direction shipUp = Base6Directions.Direction.Up,
Base6Directions.Direction shipForward = Base6Directions.Direction.Forward)
{
ShipForward = shipForward;
ShipUp = shipUp;
ShipLeft = Base6Directions.GetLeft(ShipUp, ShipForward);
double maxVel = Math.PI / 4.0;
yawPID.Kp = AngleKp;
yawPID.Ti = AngleTi;
yawPID.Td = AngleTd;
yawPID.min = -maxVel;
yawPID.max = maxVel;
pitchPID.Kp = AngleKp;
pitchPID.Ti = AngleTi;
pitchPID.Td = AngleTd;
pitchPID.min = -maxVel;
pitchPID.max = maxVel;
rollPID.Kp = AngleKp / 2.0; // Don't ask
rollPID.Ti = AngleTi;
rollPID.Td = AngleTd;
rollPID.min = -maxVel;
rollPID.max = maxVel;
yawVPID.Kp = VelKp;
yawVPID.Ti = VelTi;
yawVPID.Td = VelTd;
yawVPID.min = -Math.PI;
yawVPID.max = Math.PI;
pitchVPID.Kp = VelKp;
pitchVPID.Ti = VelTi;
pitchVPID.Td = VelTd;
pitchVPID.min = -Math.PI;
pitchVPID.max = Math.PI;
rollVPID.Kp = VelKp / 2.0; // Don't ask
rollVPID.Ti = VelTi;
rollVPID.Td = VelTd;
rollVPID.min = -Math.PI;
rollVPID.max = Math.PI;
yawPID.Reset();
pitchPID.Reset();
rollPID.Reset();
yawVPID.Reset();
pitchVPID.Reset();
rollVPID.Reset();
}
// Use ship controller velocity
public bool Cruise(ShipControlCommons shipControl, EventDriver eventDriver,
double targetSpeed,
Func<IMyThrust, bool> condition = null,
bool enableForward = true,
bool enableBackward = true)
{
var velocity = shipControl.LinearVelocity;
if (velocity != null)
{
Cruise(shipControl, targetSpeed, (Vector3D)velocity, condition,
enableForward, enableBackward);
}
return velocity != null;
}
private void AlignmentThrust(ShipControlCommons shipControl, Vector3D offset, Cruiser cruiser)
{
var velocity = shipControl.LinearVelocity;
if (velocity != null)
{
// Project offset against reference direction
var referenceDirection = GetReferenceVector(shipControl, cruiser.LocalForward);
var referenceDistance = Vector3D.Dot(offset, referenceDirection);
var targetSpeed = Math.Min(Math.Abs(referenceDistance) * VTVLHELPER_APPROACH_GAIN, VTVLHELPER_MAXIMUM_SPEED);
targetSpeed *= Math.Sign(referenceDistance);
cruiser.Cruise(shipControl, targetSpeed, (Vector3D)velocity);
}
}
// Use ship controller velocity
public bool Cruise(ShipControlCommons shipControl,
double targetSpeed,
Func <IMyThrust, bool> condition = null,
bool enableForward = true,
bool enableBackward = true)
{
var velocity = shipControl.LinearVelocity;
if (velocity != null)
{
Cruise(shipControl, targetSpeed, (Vector3D)velocity, condition,
enableForward, enableBackward);
}
return(velocity != null);
}
private IMyShipController GetShipController(ShipControlCommons shipControl)
{
if (shipControl.ShipController == null)
{
// No more controllers? Just abort
if (Mode == Modes.Orbiting)
{
ResetOrbit(shipControl);
}
else
{
Reset(shipControl);
}
}
return(shipControl.ShipController);
}
// Use externally-measured velocity
public void Cruise(ShipControlCommons shipControl, double targetSpeed,
Vector3D velocity,
Func <IMyThrust, bool> condition = null,
bool enableForward = true,
bool enableBackward = true)
{
// Determine forward unit vector
var forward3I = shipControl.Me.Position + Base6Directions.GetIntVector(shipControl.ShipBlockOrientation.TransformDirection(LocalForward));
var referenceForward = Vector3D.Normalize(shipControl.Me.CubeGrid.GridIntegerToWorld(forward3I) - shipControl.Me.GetPosition());
// Take dot product with forward unit vector
var speed = Vector3D.Dot(velocity, referenceForward);
var error = targetSpeed - speed;
//shipControl.Echo(string.Format("Set Speed: {0:F1} m/s", targetSpeed));
//shipControl.Echo(string.Format("Actual Speed: {0:F1} m/s", speed));
//shipControl.Echo(string.Format("Error: {0:F1} m/s", error));
var force = thrustPID.Compute(error);
var thrustControl = shipControl.ThrustControl;
if (Math.Abs(error) < ThrustDeadZone * targetSpeed)
{
// Close enough, just disable both sets of thrusters
thrustControl.Enable(LocalForward, false, condition);
thrustControl.Enable(LocalBackward, false, condition);
}
else if (force > 0.0)
{
// Thrust forward
thrustControl.Enable(LocalForward, enableForward, condition);
if (enableForward)
{
thrustControl.SetOverride(LocalForward, force, condition);
}
thrustControl.Enable(LocalBackward, false, condition);
}
else
{
thrustControl.Enable(LocalForward, false, condition);
thrustControl.Enable(LocalBackward, enableBackward, condition);
if (enableBackward)
{
thrustControl.SetOverride(LocalBackward, -force, condition);
}
}
}
private void AlignmentThrust(ShipControlCommons shipControl, Vector3D offset, Cruiser cruiser)
{
var velocity = shipControl.LinearVelocity;
if (velocity != null)
{
// Project offset against reference direction
var referenceDirection = GetReferenceVector(shipControl, cruiser.LocalForward);
var referenceDistance = Vector3D.Dot(offset, referenceDirection);
var targetSpeed = Math.Min(Math.Abs(referenceDistance) * VTVLHELPER_APPROACH_GAIN, VTVLHELPER_MAXIMUM_SPEED);
targetSpeed *= Math.Sign(referenceDistance);
Func <IMyThrust, bool> AlignThrusterCondition = VTVLHELPER_USE_BRAKING_THRUSTER_SPEC_FOR_ALIGN ? ThrusterCondition : null;
cruiser.Cruise(shipControl, targetSpeed, (Vector3D)velocity,
condition: AlignThrusterCondition);
}
}
// Last-ditch check before inducing spin
private bool HaveWorkingThrusters2(ShipControlCommons shipControl,
Base6Directions.Direction direction)
{
var found = false;
var thrusters = shipControl.ThrustControl.GetThrusters(direction);
thrusters.ForEach(thruster =>
{
if (thruster.IsWorking)
{
// Really make sure it isn't overridden
thruster.SetValue <float>("Override", 0.0f);
found = true;
}
});
return(found);
}
private void StartReverse(ShipControlCommons shipControl, EventDriver eventDriver)
{
shipControl.Reset(gyroOverride: true);
var shipBackward = Base6Directions.GetFlippedDirection(shipControl.ShipForward);
seeker.Init(shipControl,
shipUp: shipControl.ShipUp,
shipForward: shipBackward);
cruiser.Init(shipControl,
localForward: Base6Directions.Direction.Backward);
if (Mode != REVERSING)
{
Mode = REVERSING;
eventDriver.Schedule(0, Reverse);
}
}
private void Thrust(ShipControlCommons shipControl, double distance,
Vector3D velocity, Cruiser cruiser)
{
if (Math.Abs(distance) < 1.0)
{
// Close enough
var thrustControl = shipControl.ThrustControl;
thrustControl.Enable(cruiser.LocalForward, true);
thrustControl.Enable(cruiser.LocalBackward, true);
}
else
{
var targetSpeed = Math.Min(Math.Abs(distance) / AUTOPILOT_TTT_BUFFER,
AutopilotSpeed);
targetSpeed = Math.Max(targetSpeed, AUTOPILOT_MIN_SPEED); // Avoid Zeno's paradox...
targetSpeed *= Math.Sign(distance);
cruiser.Cruise(shipControl, targetSpeed, velocity);
}
}
private void Thrust(ShipControlCommons shipControl, double distance,
Vector3D velocity, Cruiser cruiser)
{
if (Math.Abs(distance) < 1.0)
{
// Close enough
var thrustControl = shipControl.ThrustControl;
thrustControl.Enable(cruiser.LocalForward, true);
thrustControl.Enable(cruiser.LocalBackward, true);
}
else
{
var targetSpeed = Math.Min(Math.Abs(distance) / AUTOPILOT_TTT_BUFFER,
AutopilotSpeed);
targetSpeed = Math.Max(targetSpeed, AUTOPILOT_MIN_SPEED); // Avoid Zeno's paradox...
targetSpeed *= Math.Sign(distance);
cruiser.Cruise(shipControl, targetSpeed, velocity);
}
}
private void Start(ShipControlCommons shipControl, EventDriver eventDriver)
{
shipControl.Reset(gyroOverride: true);
if (MINING_ROLL_RPM > 0.0f)
{
shipControl.GyroControl.SetAxisVelocityRPM(GyroControl.Roll, MINING_ROLL_RPM);
}
seeker.Init(shipControl,
shipUp: shipControl.ShipUp,
shipForward: shipControl.ShipForward);
cruiser.Init(shipControl,
localForward: Base6Directions.Direction.Forward);
if (Mode != MINING)
{
Mode = MINING;
eventDriver.Schedule(0, Mine);
}
}
// Use externally-measured velocity
public void Cruise(ShipControlCommons shipControl, double targetSpeed,
Vector3D velocity,
Func<IMyThrust, bool> condition = null,
bool enableForward = true,
bool enableBackward = true)
{
// Determine forward unit vector
var forward3I = shipControl.Reference.Position + Base6Directions.GetIntVector(shipControl.ShipBlockOrientation.TransformDirection(LocalForward));
var referenceForward = Vector3D.Normalize(shipControl.Reference.CubeGrid.GridIntegerToWorld(forward3I) - shipControl.ReferencePoint);
// Take dot product with forward unit vector
var speed = Vector3D.Dot(velocity, referenceForward);
var error = targetSpeed - speed;
//shipControl.Echo(string.Format("Set Speed: {0:F1} m/s", targetSpeed));
//shipControl.Echo(string.Format("Actual Speed: {0:F1} m/s", speed));
//shipControl.Echo(string.Format("Error: {0:F1} m/s", error));
var force = thrustPID.Compute(error);
var thrustControl = shipControl.ThrustControl;
if (Math.Abs(error) < ThrustDeadZone * targetSpeed)
{
// Close enough, just disable both sets of thrusters
thrustControl.Enable(LocalForward, false, condition);
thrustControl.Enable(LocalBackward, false, condition);
}
else if (force > 0.0)
{
// Thrust forward
thrustControl.Enable(LocalForward, enableForward, condition);
if (enableForward) thrustControl.SetOverride(LocalForward, force, condition);
thrustControl.Enable(LocalBackward, false, condition);
}
else
{
thrustControl.Enable(LocalForward, false, condition);
thrustControl.Enable(LocalBackward, enableBackward, condition);
if (enableBackward) thrustControl.SetOverride(LocalBackward, -force, condition);
}
}
private void Alignment(ShipControlCommons shipControl, IMyShipController controller)
{
Vector3D center;
if (DropTarget == null || !controller.TryGetPlanetPosition(out center))
{
return;
}
// Project the target position to our sphere
var targetRayDirection = Vector3D.Normalize((Vector3D)DropTarget - center);
var myRayLength = (shipControl.ReferencePoint - center).Length();
var targetPosition = center + targetRayDirection * myRayLength;
// Now get offset to target point on our sphere
// (not all that accurate over large distances, but eh)
var targetOffset = targetPosition - shipControl.ReferencePoint;
// Project targetOffset along each reference vector,
// set cruiser speed appropriately
AlignmentThrust(shipControl, targetOffset, LongCruiser);
AlignmentThrust(shipControl, targetOffset, LatCruiser);
}
private IMyShipController GetShipController(ShipControlCommons shipControl)
{
if (shipControl.ShipController == null)
{
// No more controllers? Just abort
if (Mode == ORBITING)
{
ResetOrbit(shipControl);
}
else
{
Reset(shipControl);
}
}
return shipControl.ShipController;
}
private void ResetOrbit(ShipControlCommons shipControl)
{
var gyroControl = shipControl.GyroControl;
gyroControl.Reset();
gyroControl.EnableOverride(false);
Mode = IDLE;
SaveLastCommand(shipControl, null);
}
private void Reset(ShipControlCommons shipControl)
{
shipControl.Reset(gyroOverride: false, thrusterEnable: true,
thrusterCondition: ThrusterCondition);
Mode = IDLE;
SaveLastCommand(shipControl, null);
}
private void OrbitInit(ShipControlCommons shipControl)
{
// Don't touch thrusters at all
shipControl.GyroControl.Reset();
shipControl.GyroControl.EnableOverride(true);
var forward = shipControl.ShipBlockOrientation.TransformDirection(VTVLHELPER_ORBIT_DIRECTION);
seeker.Init(shipControl,
shipUp: Base6Directions.GetPerpendicular(forward),
shipForward: forward);
}
private Vector3D GetReferenceVector(ShipControlCommons shipControl, Base6Directions.Direction direction)
{
var offset = shipControl.Me.Position + Base6Directions.GetIntVector(shipControl.ShipBlockOrientation.TransformDirection(direction));
return(Vector3D.Normalize(shipControl.Me.CubeGrid.GridIntegerToWorld(offset) - shipControl.Me.GetPosition()));
}
private Vector3D GetReferenceVector(ShipControlCommons shipControl,
Base6Directions.Direction direction)
{
var offset = shipControl.Reference.Position + Base6Directions.GetIntVector(direction);
return Vector3D.Normalize(shipControl.Reference.CubeGrid.GridIntegerToWorld(offset) - shipControl.ReferencePoint);
}
private void Maneuver(ShipControlCommons shipControl, EventDriver eventDriver)
{
velocimeter.TakeSample(shipControl.ReferencePoint, eventDriver.TimeSinceStart);
// Determine velocity
var velocity = velocimeter.GetAverageVelocity();
if (velocity != null)
{
// Only absolute velocity
var speed = ((Vector3D)velocity).Length();
var error = ManeuveringSpeed - speed;
var force = thrustPID.Compute(error);
var thrustControl = shipControl.ThrustControl;
if (force > 0.0)
{
thrustControl.SetOverride(Base6Directions.Direction.Forward, force);
}
else
{
thrustControl.SetOverride(Base6Directions.Direction.Forward, false);
}
}
}
private GyroControl _Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D?targetUp,
out double yawError, out double pitchError,
out double rollError)
{
Vector3D referenceForward;
Vector3D referenceLeft;
Vector3D referenceUp;
GyroControl gyroControl;
// See if local orientation is the same as the ship
if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward)
{
// Use same reference vectors and GyroControl
referenceForward = shipControl.ReferenceForward;
referenceLeft = shipControl.ReferenceLeft;
referenceUp = shipControl.ReferenceUp;
gyroControl = shipControl.GyroControl;
}
else
{
referenceForward = GetReferenceVector(shipControl, ShipForward);
referenceLeft = GetReferenceVector(shipControl, ShipLeft);
referenceUp = GetReferenceVector(shipControl, ShipUp);
// Need our own GyroControl instance in this case
gyroControl = new GyroControl();
gyroControl.Init(shipControl.Blocks,
shipUp: ShipUp,
shipForward: ShipForward);
}
// Determine projection of targetVector onto our reference unit vectors
var dotZ = targetVector.Dot(referenceForward);
var dotX = targetVector.Dot(referenceLeft);
var dotY = targetVector.Dot(referenceUp);
var projZ = dotZ * referenceForward;
var projX = dotX * referenceLeft;
var projY = dotY * referenceUp;
// Determine yaw/pitch error by calculating angle between our forward
// vector and targetVector
var z = projZ.Length() * Math.Sign(dotZ);
var x = projX.Length() * Math.Sign(dotX);
var y = projY.Length() * Math.Sign(-dotY); // NB inverted
yawError = Math.Atan2(x, z);
pitchError = Math.Atan2(y, z);
var gyroYaw = yawPID.Compute(yawError);
var gyroPitch = pitchPID.Compute(pitchError);
gyroControl.SetAxisVelocityFraction(GyroControl.Yaw, (float)gyroYaw);
gyroControl.SetAxisVelocityFraction(GyroControl.Pitch, (float)gyroPitch);
if (targetUp != null)
{
// Also adjust roll
dotX = ((Vector3D)targetUp).Dot(referenceLeft);
dotY = ((Vector3D)targetUp).Dot(referenceUp);
projX = dotX * referenceLeft;
projY = dotY * referenceUp;
x = projX.Length() * Math.Sign(-dotX);
y = projY.Length() * Math.Sign(dotY);
rollError = Math.Atan2(x, y);
var gyroRoll = rollPID.Compute(rollError);
gyroControl.SetAxisVelocityFraction(GyroControl.Roll, (float)gyroRoll);
}
else
{
rollError = default(double);
}
return(gyroControl);
}
private Vector3D GetTarget(ShipControlCommons shipControl,
Vector3D direction)
{
// Vector from start to current
var startVector = shipControl.ReferencePoint - StartPoint;
// Determine projection on start direction vector
var startDot = startVector.Dot(direction);
Vector3D targetVector;
if (startDot >= 0.0)
{
// Set targetVector to that projection
targetVector = startDot * direction;
}
else
{
// Or set it to the start (0,0,0) if the ship is
// behind the start position
targetVector = new Vector3D(0, 0, 0);
}
// Offset forward by some amount
targetVector += LOS_OFFSET * direction;
// Offset by difference between start and current positions
targetVector += StartPoint - shipControl.ReferencePoint;
return targetVector;
}
private void Start(ShipControlCommons shipControl, EventDriver eventDriver)
{
shipControl.Reset(gyroOverride: true);
if (MINING_ROLL_RPM > 0.0f) shipControl.GyroControl.SetAxisVelocityRPM(GyroControl.Roll, MINING_ROLL_RPM);
seeker.Init(shipControl,
shipUp: shipControl.ShipUp,
shipForward: shipControl.ShipForward);
cruiser.Init(shipControl,
localForward: Base6Directions.Direction.Forward);
if (Mode != MINING)
{
Mode = MINING;
eventDriver.Schedule(0, Mine);
}
}
private GyroControl _Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D?targetUp,
out double yawPitchError, out double rollError)
{
var angularVelocity = shipControl.AngularVelocity;
if (angularVelocity == null)
{
// No ship controller, no action
yawPitchError = rollError = Math.PI;
return(shipControl.GyroControl);
}
Vector3D referenceForward;
Vector3D referenceLeft;
Vector3D referenceUp;
GyroControl gyroControl;
// See if local orientation is the same as the ship
if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward)
{
// Use same reference vectors and GyroControl
referenceForward = shipControl.ReferenceForward;
referenceLeft = shipControl.ReferenceLeft;
referenceUp = shipControl.ReferenceUp;
gyroControl = shipControl.GyroControl;
}
else
{
referenceForward = GetReferenceVector(shipControl, ShipForward);
referenceLeft = GetReferenceVector(shipControl, ShipLeft);
referenceUp = GetReferenceVector(shipControl, ShipUp);
// Need our own GyroControl instance in this case
gyroControl = new GyroControl();
gyroControl.Init(shipControl.Blocks,
shipUp: ShipUp,
shipForward: ShipForward);
}
targetVector = Vector3D.Normalize(targetVector);
// Invert our world matrix
var toLocal = MatrixD.Transpose(MatrixD.CreateWorld(Vector3D.Zero, referenceForward, referenceUp));
// And bring targetVector & angular velocity into local space
var localTarget = Vector3D.TransformNormal(-targetVector, toLocal);
var localVel = Vector3D.TransformNormal((Vector3D)angularVelocity, toLocal);
// Use simple trig to get the error angles
var yawError = Math.Atan2(localTarget.X, localTarget.Z);
var pitchError = Math.Atan2(localTarget.Y, localTarget.Z);
// Set desired angular velocity
var desiredYawVel = yawPID.Compute(yawError);
var desiredPitchVel = pitchPID.Compute(pitchError);
//shipControl.Echo(string.Format("desiredVel = {0:F3}, {1:F3}", desiredYawVel, desiredPitchVel));
// Translate to gyro outputs
double gyroYaw = 0.0;
if (Math.Abs(desiredYawVel) >= ControlThreshold)
{
gyroYaw = yawVPID.Compute(desiredYawVel - localVel.X);
}
double gyroPitch = 0.0;
if (Math.Abs(desiredPitchVel) >= ControlThreshold)
{
gyroPitch = pitchVPID.Compute(desiredPitchVel - localVel.Y);
}
//shipControl.Echo(string.Format("yaw, pitch = {0:F3}, {1:F3}", gyroYaw, gyroPitch));
gyroControl.SetAxisVelocity(GyroControl.Yaw, (float)gyroYaw);
gyroControl.SetAxisVelocity(GyroControl.Pitch, (float)gyroPitch);
// Determine total yaw/pitch error
yawPitchError = Math.Acos(MathHelperD.Clamp(Vector3D.Dot(targetVector, referenceForward), -1.0, 1.0));
if (targetUp != null)
{
// Also adjust roll by rotating targetUp vector
localTarget = Vector3D.TransformNormal((Vector3D)targetUp, toLocal);
rollError = Math.Atan2(localTarget.X, localTarget.Y);
var desiredRollVel = rollPID.Compute(rollError);
//shipControl.Echo(string.Format("desiredRollVel = {0:F3}", desiredRollVel));
double gyroRoll = 0.0;
if (Math.Abs(desiredRollVel) >= ControlThreshold)
{
gyroRoll = rollVPID.Compute(desiredRollVel - localVel.Z);
}
//shipControl.Echo(string.Format("roll = {0:F3}", gyroRoll));
gyroControl.SetAxisVelocity(GyroControl.Roll, (float)gyroRoll);
// Only care about absolute error
rollError = Math.Abs(rollError);
}
else
{
rollError = 0.0;
}
return(gyroControl);
}
private double Perturb(ShipControlCommons shipControl, TimeSpan timeSinceStart, out Vector3D targetVector)
{
targetVector = Target - shipControl.ReferencePoint;
var distance = targetVector.Normalize(); // Original distance
var amp = ScaleAmplitude(distance);
var newTarget = Target;
newTarget += shipControl.ReferenceUp * amp * Math.Cos(PerturbTimeScale * timeSinceStart.TotalSeconds + RandomOffset);
newTarget += shipControl.ReferenceLeft * amp * Math.Sin(PerturbTimeScale * timeSinceStart.TotalSeconds + RandomOffset);
targetVector = Vector3D.Normalize(newTarget - shipControl.ReferencePoint);
return distance;
}
private void Alignment(ShipControlCommons shipControl, IMyShipController controller)
{
Vector3D center;
if (DropTarget == null || !controller.TryGetPlanetPosition(out center)) return;
// Project the target position to our sphere
var targetRayDirection = Vector3D.Normalize((Vector3D)DropTarget - center);
var myRayLength = (shipControl.ReferencePoint - center).Length();
var targetPosition = center + targetRayDirection * myRayLength;
// Now get offset to target point on our sphere
// (not all that accurate over large distances, but eh)
var targetOffset = targetPosition - shipControl.ReferencePoint;
// Project targetOffset along each reference vector,
// set cruiser speed appropriately
AlignmentThrust(shipControl, targetOffset, LongCruiser);
AlignmentThrust(shipControl, targetOffset, LatCruiser);
}
private void AlignmentThrust(ShipControlCommons shipControl, Vector3D offset, Cruiser cruiser)
{
var velocity = shipControl.LinearVelocity;
if (velocity != null)
{
// Project offset against reference direction
var referenceDirection = GetReferenceVector(shipControl, cruiser.LocalForward);
var referenceDistance = Vector3D.Dot(offset, referenceDirection);
var targetSpeed = Math.Min(Math.Abs(referenceDistance) * VTVLHELPER_APPROACH_GAIN, VTVLHELPER_MAXIMUM_SPEED);
targetSpeed *= Math.Sign(referenceDistance);
cruiser.Cruise(shipControl, targetSpeed, (Vector3D)velocity);
}
}
private void SetTarget(ShipControlCommons shipControl)
{
// Only set if neither mining nor reversing
if (Mode == IDLE)
{
StartPoint = shipControl.ReferencePoint;
StartDirection = shipControl.ReferenceForward;
StartUp = shipControl.ReferenceUp;
StartLeft = shipControl.ReferenceLeft;
}
}
private void StartReverse(ShipControlCommons shipControl, EventDriver eventDriver)
{
shipControl.Reset(gyroOverride: true);
var shipBackward = Base6Directions.GetFlippedDirection(shipControl.ShipForward);
seeker.Init(shipControl,
shipUp: shipControl.ShipUp,
shipForward: shipBackward);
cruiser.Init(shipControl,
localForward: Base6Directions.Direction.Backward);
if (Mode != REVERSING)
{
Mode = REVERSING;
eventDriver.Schedule(0, Reverse);
}
}
// Last-ditch check before inducing spin
private bool HaveWorkingThrusters2(ShipControlCommons shipControl,
Base6Directions.Direction direction)
{
var found = false;
var thrusters = shipControl.ThrustControl.GetThrusters(direction);
thrusters.ForEach(thruster =>
{
if (thruster.IsWorking)
{
// Really make sure it isn't overridden
thruster.SetValue<float>("Override", 0.0f);
found = true;
}
});
return found;
}
private GyroControl _Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D? targetUp,
out double yawError, out double pitchError,
out double rollError)
{
Vector3D referenceForward;
Vector3D referenceLeft;
Vector3D referenceUp;
GyroControl gyroControl;
// See if local orientation is the same as the ship
if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward)
{
// Use same reference vectors and GyroControl
referenceForward = shipControl.ReferenceForward;
referenceLeft = shipControl.ReferenceLeft;
referenceUp = shipControl.ReferenceUp;
gyroControl = shipControl.GyroControl;
}
else
{
referenceForward = GetReferenceVector(shipControl, ShipForward);
referenceLeft = GetReferenceVector(shipControl, ShipLeft);
referenceUp = GetReferenceVector(shipControl, ShipUp);
// Need our own GyroControl instance in this case
gyroControl = new GyroControl();
gyroControl.Init(shipControl.Blocks,
shipUp: ShipUp,
shipForward: ShipForward);
}
// Determine projection of targetVector onto our reference unit vectors
var dotZ = targetVector.Dot(referenceForward);
var dotX = targetVector.Dot(referenceLeft);
var dotY = targetVector.Dot(referenceUp);
var projZ = dotZ * referenceForward;
var projX = dotX * referenceLeft;
var projY = dotY * referenceUp;
// Determine yaw/pitch error by calculating angle between our forward
// vector and targetVector
var z = projZ.Length() * Math.Sign(dotZ);
var x = projX.Length() * Math.Sign(dotX);
var y = projY.Length() * Math.Sign(-dotY); // NB inverted
yawError = Math.Atan2(x, z);
pitchError = Math.Atan2(y, z);
var gyroYaw = yawPID.Compute(yawError);
var gyroPitch = pitchPID.Compute(pitchError);
gyroControl.SetAxisVelocityFraction(GyroControl.Yaw, (float)gyroYaw);
gyroControl.SetAxisVelocityFraction(GyroControl.Pitch, (float)gyroPitch);
if (targetUp != null)
{
// Also adjust roll
dotX = ((Vector3D)targetUp).Dot(referenceLeft);
dotY = ((Vector3D)targetUp).Dot(referenceUp);
projX = dotX * referenceLeft;
projY = dotY * referenceUp;
x = projX.Length() * Math.Sign(-dotX);
y = projY.Length() * Math.Sign(dotY);
rollError = Math.Atan2(x, y);
var gyroRoll = rollPID.Compute(rollError);
gyroControl.SetAxisVelocityFraction(GyroControl.Roll, (float)gyroRoll);
}
else
{
rollError = default(double);
}
return gyroControl;
}
private bool HaveWorkingThrusters(ShipControlCommons shipControl,
Base6Directions.Direction direction)
{
// First pass: Look for a working, non-overridden thruster
var found = false;
var overridden = new List<IMyThrust>();
var thrusters = shipControl.ThrustControl.GetThrusters(direction);
thrusters.ForEach(thruster =>
{
if (thruster.IsWorking)
{
if (thruster.GetValue<float>("Override") > 0.0f)
{
// Thruster is overridden. Keep track of it.
overridden.Add(thruster);
}
else
{
// Found a good thruster
found = true;
}
}
});
// Depending on outcome, disable or zero-out overridden thrusters
overridden.ForEach(thruster =>
{
if (found)
{
// Disable and let good thrusters take care of it
thruster.SetValue<bool>("OnOff", false);
}
else
{
// No good thrusters. Zero-out override.
thruster.SetValue<float>("Override", 0.0f);
found = true;
// Note this means we will zero-out at most 1 thruster.
// For now, this is the desired effect.
}
});
if (!found)
{
// Final desperation move. Enable and zero-out overrides for
// all thrusters on this side.
thrusters.ForEach(thruster =>
{
thruster.SetValue<bool>("OnOff", true);
thruster.SetValue<float>("Override", 0.0f);
});
// Still return false, but we'll check again after a few ticks
}
return found;
}
