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 Run(ZACommons commons, EventDriver eventDriver)
{
var shipControl = (ShipControlCommons)commons;
ResetIfNotLive(commons, eventDriver);
if (!Active)
{
return;
}
// Determine velocity
var velocity = shipControl.LinearVelocity;
// If we have no velocity, we have no ship controllers.
// In theory, ResetIfNotLive should have caught it above
if (velocity != null)
{
var cruiseDirectionFlipped = Base6Directions.GetFlippedDirection(CruiseDirection);
// Determine forward vector
var forward3I = shipControl.Reference.Position + Base6Directions.GetIntVector(shipControl.ShipBlockOrientation.TransformDirection(CruiseDirection));
var forward = Vector3D.Normalize(shipControl.Reference.CubeGrid.GridIntegerToWorld(forward3I) - shipControl.ReferencePoint);
CurrentSpeed = Vector3D.Dot((Vector3D)velocity, forward);
var error = TargetSpeed - CurrentSpeed;
var force = thrustPID.Compute(error);
//commons.Echo("Force: " + force);
var thrustControl = shipControl.ThrustControl;
var collect = ParseCruiseFlags();
if (Math.Abs(error) < CRUISE_CONTROL_DEAD_ZONE * TargetSpeed)
{
// Close enough, just disable both sets of thrusters
thrustControl.Enable(CruiseDirection, false, collect);
thrustControl.Enable(cruiseDirectionFlipped, false, collect);
}
else if (force > 0.0)
{
// Thrust forward
thrustControl.Enable(CruiseDirection, true, collect);
thrustControl.SetOverride(CruiseDirection, force, collect);
thrustControl.Enable(cruiseDirectionFlipped, false, collect);
}
else
{
thrustControl.Enable(CruiseDirection, false, collect);
thrustControl.Enable(cruiseDirectionFlipped, true, collect);
thrustControl.SetOverride(cruiseDirectionFlipped, -force, collect);
}
}
eventDriver.Schedule(FramesPerRun, Run);
}
// 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);
}
}
}
public void Run(ZACommons commons, EventDriver eventDriver)
{
if (!Active)
{
return;
}
var rotor = GetRotor(commons);
//commons.Echo("Angle: " + rotor.Angle);
//commons.Echo("SetPoint: " + SetPoint);
var error = SetPoint - rotor.Angle;
if (error > Math.PI)
{
error -= Math.PI * 2.0;
}
if (error < -Math.PI)
{
error += Math.PI * 2.0;
}
//commons.Echo("Error: " + error);
if (Math.Abs(error) > StepAmount / 2.0)
{
var rotorVelocity = pid.Compute(error);
rotor.SetValue <float>("Velocity", (float)(rotor.GetMaximum <float>("Velocity") * rotorVelocity));
eventDriver.Schedule(TicksPerRun, Run);
}
else
{
rotor.SetValue <float>("Velocity", 0.0f);
Active = false;
// NB We don't re-schedule
}
}
public void Run(ZACommons commons, EventDriver eventDriver)
{
if (!Active)
{
return;
}
var piston = GetPiston(commons);
var error = SetPoint - piston.CurrentPosition;
if (Math.Abs(error) > PISTONSTEPPER_MIN_ERROR)
{
var pistonVelocity = pid.Compute(error);
piston.SetValue <float>("Velocity", (float)(piston.GetMaximum <float>("Velocity") * pistonVelocity));
eventDriver.Schedule(TicksPerRun, Run);
}
else
{
piston.SetValue <float>("Velocity", 0.0f);
Active = false;
// NB We don't re-schedule
}
}
float CorrectY()
{
return(targetY + pID.Compute(transform.position.y - targetY));
}
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 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 async void TimerTick(object state)
{
DateTime current = DateTime.Now;
double tempReading = _temperatureControlModule.Readings[activeSensorID];
if (tempReading != op.Params.LastValue)
{
op.Params.LastValue = tempReading;
_displayManager.WriteTemperatureDisplays(op.Params);
}
if (current.Subtract(_windowStart).TotalMilliseconds > op.Params.WindowSize)
{
_windowStart = current;
await this.Hub.Clients.All.SendAsync("pid_ping", op);
}
if (UseTuning)
{
int i = autoTune.Runtime();
if (i != 0)
{
UseTuning = false;
}
if (!UseTuning)
{
PIDParameter p = autoTune.GetTuningValues();
Console.WriteLine("Tuning Finished...");
Console.WriteLine("KP: " + p.Kp);
Console.WriteLine("KI: " + p.Ki);
Console.WriteLine("KD: " + p.Kd);
_hardwareIOModule.SetHeatingElement(false);
return;
}
}
else
{
pidController.Compute(tempReading);
}
double newOutput;
if (UseTuning)
{
newOutput = autoTune.GetOutput();
}
else
{
newOutput = pidController.PIDOutput;
}
if (!UseTuning)
{
if (pidController.PIDOutput != op.Params.LastOutput)
{
op.Params.LastOutput = newOutput;
await this.Hub.Clients.All.SendAsync("pid_ping", op);
// Console.WriteLine("New Output: " + newOutput);
}
}
else
{
if (autoTune.Output != op.Params.LastOutput)
{
op.Params.LastOutput = autoTune.Output;
Console.WriteLine("Using:");
PIDParameter p = autoTune.GetTuningValues();
Console.WriteLine("KP: " + p.Kp);
Console.WriteLine("KI: " + p.Ki);
Console.WriteLine("KD: " + p.Kd);
}
}
if ((newOutput > 100) && (newOutput > current.Subtract(_windowStart).TotalMilliseconds))
{
if (!prevElState)
{
prevElState = true;
op.ElementState = prevElState;
await this.Hub.Clients.All.SendAsync("pid_change", "CONTROL", "ON");
_hardwareIOModule.SetHeatingElement(prevElState);
}
}
else
{
if (prevElState)
{
prevElState = false;
op.ElementState = prevElState;
_hardwareIOModule.SetHeatingElement(prevElState);
await this.Hub.Clients.All.SendAsync("pid_change", "CONTROL", "OFF");
}
}
//if (now - windowStartTime > WindowSize)
//{ //time to shift the Relay Window
// windowStartTime += WindowSize;
//}
//if ((onTime > 100) && (onTime > (now - windowStartTime)))
//{
// digitalWrite(RelayPin, HIGH);
//}
//else
//{
// digitalWrite(RelayPin, LOW);
//}
// if (counterTick == 10)
//{
//counterTick = 0;
// await this.Hub.Clients.All.SendAsync("pid_ping", op);
//}
//wtf
// activeSensorID
// output will be a value between 0 and 5000
//if (current - _windowStart > TimeSpan.FromMilliseconds(op.Params.WindowSize))
//{
// _windowStart = current;
// await this.Hub.Clients.All.SendAsync("pid_change", "NEWWINDOW", op.Params.WindowSize);
// //Console.WriteLine("new window");
//}
// Console.WriteLine("total milis: " + (current - _windowStart).TotalMilliseconds);
//Console.WriteLine("pid output:" + pidController.PIDOutput);
//if (pidController.PIDOutput > (current - _windowStart).TotalMilliseconds)
//{
// Console.WriteLine("ON");
// //if (op.State == PIDState.Started)
// //{
// _hardwareIOModule.SetHeatingElement(true);
// //}
// await this.Hub.Clients.All.SendAsync("pid_change", "CONTROL", "ON");
//}
//else
//{
// Console.WriteLine("OFF");
// //if (op.State == PIDState.Started)
// //{
// _hardwareIOModule.SetHeatingElement(false);
// //}
// await this.Hub.Clients.All.SendAsync("pid_change", "CONTROL", "OFF");
//}
//void loop()
//{
// sensors.requestTemperatures();
// Input = sensors.getTempCByIndex(0);
// Serial.print("Temperature: ");
// Serial.println(Input);
// myPID.Compute();
// /************************************************
// * turn the output pin on/off based on pid output
// ************************************************/
// if (millis() - windowStartTime > WindowSize)
// {e
// //time to shift the Relay Window
// windowStartTime += WindowSize;
// }
// if (Output < millis() - windowStartTime)
// digitalWrite(RELAY_PIN, HIGH);
// else
// digitalWrite(RELAY_PIN, LOW);
//}
// if()
}
public void Update()
{
_gyros.FindBlocks(true, null, _config.AlignGyrosGroupName);
_sorters.FindBlocks(true, null, _config.DumpSortersGroupName);
_controllers.FindBlocks(true, null);
_batteries.FindBlocks(true, null);
_reactors.FindBlocks(true, null);
_hydrogenTanks.FindBlocks(true, tank => {
return(BlockHelper.IsHydrogenTank(tank));
});
CalculateCharge();
CalculateHydrogen();
CalculateUranium();
IMyShipController controller = null;
IMyShipController firstWorking = null;
foreach (IMyShipController _controller in _controllers.blocks)
{
if (!_controller.IsWorking)
{
continue;
}
if (firstWorking == null)
{
firstWorking = _controller;
}
if (controller == null && _controller.IsUnderControl && _controller.CanControlShip)
{
controller = _controller;
}
if (_controller.IsMainCockpit)
{
controller = _controller;
}
}
if (controller == null)
{
controller = firstWorking;
}
if (controller == null)
{
Status = "Missing controller";
return;
}
if (_gyros.Count() == 0)
{
Status = "Missing gyros";
return;
}
Status = "";
if (GravityAlign)
{
_GravityAlignActive = true;
DoGravityAlign(controller, _gyros.blocks, GravityAlignPitch);
}
if (!GravityAlign && _GravityAlignActive)
{
ReleaseGyros(_gyros.blocks);
}
if (CruiseEnabled)
{
double currentSpeed = controller.GetShipSpeed();
var error = CruiseTarget - currentSpeed;
var force = thrustPID.Compute(error);
_systemManager.CruiseThrusters.ForEach(thruster =>
{
if (Math.Abs(error) < 0.02f * CruiseTarget)
{
thruster.ThrustOverridePercentage = 0.0f;
}
else if (force > 0.0)
{
thruster.Enabled = true;
thruster.ThrustOverridePercentage = (float)force * 0.1f;
}
else
{
thruster.ThrustOverridePercentage = 0.0f;
thruster.Enabled = false;
}
});
_systemManager.CruiseReverseThrusters.ForEach(thruster =>
{
if (Math.Abs(error) < 0.02f * CruiseTarget)
{
thruster.ThrustOverridePercentage = 0.0f;
thruster.Enabled = false;
}
else if (force > 0.0)
{
thruster.ThrustOverridePercentage = 0.0f;
thruster.Enabled = false;
}
else
{
thruster.Enabled = true;
thruster.ThrustOverridePercentage = -(float)force * 0.1f;
}
});
}
}
