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