private double AngleTowards(IMyMotorStator stator, IMyTerminalBlock pointer, Vector3D direction)
{
Vector3D restDir = Vector3D.Normalize(pointer.GetPosition() - stator.GetPosition());
return(Math.Atan2(stator.WorldMatrix.Right.Dot(direction), stator.WorldMatrix.Forward.Dot(direction)) -
Math.Atan2(stator.Top.WorldMatrix.Right.Dot(restDir), stator.Top.WorldMatrix.Forward.Dot(restDir)));
}
public Program()
{
Runtime.UpdateFrequency = UpdateFrequency.Update1;
rotorBase = GetBlock <IMyMotorStator>("RotorBase");
hingeBase = GetBlock <IMyMotorStator>("HingeBase");
rotorMiddle = GetBlock <IMyMotorStator>("RotorMiddle");
hingeEnd = GetBlock <IMyMotorStator>("HingeEnd");
IMyCockpit cockpit = GetBlock <IMyCockpit>("Controller");
controller = cockpit;
controllerPanel = cockpit.GetSurface(0);
debugPanel = GetBlock <IMyTextSurface>("DebugPanel");
targetPosition = hingeEnd.GetPosition() - hingeBase.GetPosition();
if (Math.Abs(Vector3D.Distance(hingeBase.GetPosition(), rotorMiddle.GetPosition()) -
Vector3D.Distance(rotorMiddle.GetPosition(), hingeEnd.GetPosition())) > 0.1)
{
throw new Exception("Arms have different lengths.");
}
armLength = Vector3D.Distance(hingeBase.GetPosition(), rotorMiddle.GetPosition());
hingeBaseSign = -Math.Sign(Vector3D.Dot(targetPosition, hingeBase.WorldMatrix.Forward));
rotorMiddleSign = -Math.Sign(Vector3D.Dot(hingeBase.WorldMatrix.Right, rotorMiddle.WorldMatrix.Up)) *
hingeBaseSign;
Vector3D rotorMiddleToBase = hingeBase.GetPosition() - rotorMiddle.GetPosition();
rotorMiddleOffset = AngleTowards(rotorMiddle, hingeEnd, Vector3D.Normalize(rotorMiddleToBase));
pidRotor.Init(1, 0.5, 0.01);
pidBase.Init(1, 0.5, 0.01);
pidMid.Init(1, 0.5, 0.01);
locked = true;
}
public void Main()
{
detected_entities.Clear();
sensor.DetectedEntities(detected_entities);
foreach(MyDetectedEntityInfo entity in detected_entities)
{
if (entity.Type == player_type)
{
p = entity.Position - azimuth_rotor.GetPosition();
break;
}
}
block_p = convert_to_block_vector(azimuth_rotor, p);
azimuth_atan2 = (float)Math.Atan2(-block_p.X, block_p.Z);
azimuth_rotor.TargetVelocityRad = get_azimuth(azimuth_rotor.Angle, azimuth_atan2) * azimuth_mult;
}
double GetClosestDistance(List <IMyWarhead> entities, IMyMotorStator reference)
{
IMyWarhead closestEntity = null;
double closestDistance = 100;
if (entities.Count() > 0)
{
foreach (var entity in entities)
{
var distance = Vector3D.Distance(reference.GetPosition(), entity.GetPosition());
if (distance < closestDistance)
{
closestDistance = distance;
closestEntity = entity;
}
}
}
return(closestDistance);
}
public void Main(string argument, UpdateType updateSource)
{
if ((updateSource & UpdateType.Trigger) != 0)
{
if (argument == "Lock")
{
if (locked)
{
locked = false;
Unlock();
}
else
{
locked = true;
Lock();
}
}
if (argument == "Reset")
{
targetPosition = hingeEnd.GetPosition() - hingeBase.GetPosition();
}
}
Vector3D baseX = rotorBase.WorldMatrix.Forward;
Vector3D baseY = rotorBase.WorldMatrix.Left;
Vector3D baseZ = rotorBase.WorldMatrix.Up;
Func <Vector3D, string> P = value =>
$"X={F(baseX.Dot(value))} Y={F(baseY.Dot(value))} Z={F(baseZ.Dot(value))}";
Vector3D curPos = hingeEnd.GetPosition() - hingeBase.GetPosition();
targetPosition = Vector3D.ClampToSphere(targetPosition, 1.95 * armLength);
// TODO enforce out of central cylinder
double distance = targetPosition.Length();
Vector3D direction = Vector3D.Normalize(targetPosition);
const double epsilon = Math.PI / 360;
double armsAngle = Clamp(2 * Math.Asin(distance / armLength / 2), epsilon, Math.PI - epsilon);
double pitch = (Math.PI - armsAngle) / 2 - Angle(direction, rotorBase.WorldMatrix.Up);
double rotorBaseAngle = AngleTowards(rotorBase, hingeEnd, direction);
double hingeBaseAngle = Clamp(hingeBaseSign * pitch, -Math.PI / 2 + epsilon, +Math.PI / 2 - epsilon);
double rotorMiddleAngle = rotorMiddleSign * armsAngle + rotorMiddleOffset;
if (!locked)
{
StatorControl(rotorBase, rotorBaseAngle, ref pidRotor);
StatorControl(hingeBase, hingeBaseAngle, ref pidBase);
StatorControl(rotorMiddle, rotorMiddleAngle, ref pidMid);
hingeEnd.TargetVelocityRad = HINGE_END_SENSITIVITY * controller.RotationIndicator.X;
}
Vector3D deltaPosition = targetPosition - curPos;
targetPosition = curPos + Vector3D.ClampToSphere(deltaPosition, 1);
targetPosition +=
Vector3D.TransformNormal(Vector3D.ClampToSphere(controller.MoveIndicator, 1), controller.WorldMatrix) *
TARGET_SPEED;
Echo($"e={Vector3D.Distance(curPos, targetPosition):F1}");
string information =
$@"
trg: {P(targetPosition)}
cur: {P(curPos)}
e={F(Vector3D.Distance(curPos, targetPosition))}
rot: trg={A(rotorBaseAngle)} cur={A(rotorBase.Angle)} trg-spd={F(rotorBase.TargetVelocityRad)} pid=[{pidRotor.information}]
bas: trg={A(hingeBaseAngle)} cur={A(hingeBase.Angle)} trg-spd={F(hingeBase.TargetVelocityRad)} pid=[{pidBase.information}]
mid: trg={A(rotorMiddleAngle)} cur={A(rotorMiddle.Angle)} trg-spd={F(rotorMiddle.TargetVelocityRad)} pid=[{pidMid.information}]
hbs={F(hingeBaseSign)} rms={F(rotorMiddleSign)} rmo={A(rotorMiddleOffset)}
lock={locked}
al={F(armLength)} aa={F(armsAngle)} pitch={F(pitch)}
rb-fwd : {P(rotorBase.WorldMatrix.Forward)}
rb-up : {P(rotorBase.WorldMatrix.Up)}
rbt-fwd: {P(rotorBase.Top.WorldMatrix.Forward)}
rbt-up : {P(rotorBase.Top.WorldMatrix.Up)}
";
debugPanel.WriteText(information);
controllerPanel.WriteText(information);
}
public Vector3D GetPosition()
{
return(Rotor1.GetPosition());
}
public void Main(string argument, UpdateType updateSource)
{
if (!setupError)
{
if ((updateSource & UpdateType.Update1) != 0)
{
switch (reloadState)
{
case ReloadStates.RELOAD_COMPLETE:
// All done!
break;
case ReloadStates.RETRACTING_PLACER:
if (PlacerPiston.CurrentPosition == PlacerPiston.MinLimit)
{
Echo("Moving clip.");
reloadState = ReloadStates.MOVING_CLIP;
GridTerminalSystem.GetBlocksOfType <IMyWarhead>(warheads);
}
else
{
PlacerPiston.Retract();
}
break;
case ReloadStates.MOVING_CLIP:
var closestWarheadDistance = GetClosestDistance(warheads, PlacerRotor);
if (closestWarheadDistance < 0.5)
{
ClipPiston.Enabled = false;
Echo("Loading warhead.");
reloadState = ReloadStates.LOADING_WARHEAD;
}
else if (warheads.Count() == 0 && ClipPiston.CurrentPosition == ClipPiston.MaxLimit)
{
Echo("Out of warheads.");
reloadState = ReloadStates.RELOAD_COMPLETE;
}
else
{
ClipPiston.Enabled = true;
ClipPiston.Extend();
}
break;
case ReloadStates.LOADING_WARHEAD:
if (PlacerRotor.IsAttached)
{
var rotors = new List <IMyMotorStator>();
GridTerminalSystem.GetBlocksOfType <IMyMotorStator>(rotors, (IMyMotorStator x) => x.CubeGrid == ClipPiston.TopGrid && x.TopGrid == PlacerRotor.TopGrid);
if (rotors.Count() > 0)
{
var otherRotor = rotors.First();
otherRotor.Detach();
}
Echo("Extending placer.");
reloadState = ReloadStates.EXTENDING_PLACER;
}
else
{
Echo("Attaching.");
PlacerRotor.Attach();
}
break;
case ReloadStates.EXTENDING_PLACER:
if (PlacerPiston.CurrentPosition == PlacerPiston.MaxLimit)
{
Echo("Reload complete.");
reloadState = ReloadStates.RELOAD_COMPLETE;
}
else
{
PlacerPiston.Extend();
}
break;
default:
Echo("Encountered unexpected state while reloading.");
break;
}
}
else if (argument.ToLower() == "reload")
{
Echo("Retracting placer piston.");
PlacerRotor.Detach();
reloadState = ReloadStates.RETRACTING_PLACER;
}
else if (argument.ToLower() == "fire")
{
if (PlacerRotor.IsAttached)
{
Echo("Deploying payload.");
var viableWarheads = new List <IMyWarhead>();
GridTerminalSystem.GetBlocksOfType <IMyWarhead>(viableWarheads, (IMyWarhead x) => x.CubeGrid == PlacerRotor.TopGrid);
foreach (var warhead in viableWarheads)
{
warhead.DetonationTime = DetonationTime;
warhead.StartCountdown();
}
if (viableWarheads.Count() > 0)
{
countdown = DetonationTime;
bombPosition = PlacerRotor.GetPosition();
}
PlacerRotor.Detach();
reloadState = ReloadStates.RETRACTING_PLACER;
}
}
else if (argument.ToLower().StartsWith("timer"))
{
int.TryParse(argument.ToLower().Substring("timer".Length), out DetonationTime);
}
}
}
