void DampSuspHinge(IMyMotorAdvancedStator h, double r, double p) { var T = 30000 + h.Angle / Math.Abs(h.LowerLimitRad) * 30000 + (float)(2 * p / Math.PI) * Math.Sign(Vector3D.Dot(h.GetPosition() - HingeNeck.GetPosition(), HingeNeck.WorldMatrix.Up)) * 20000 + (float)(2 * r / Math.PI) * Math.Sign(Vector3D.Dot(h.GetPosition() - HingeNeck.GetPosition(), HingeNeck.WorldMatrix.Forward)) * 20000 ; byte upLegs = (byte)Convert.ToSingle(Controller.MoveIndicator.Y < 0 && ((Vector3D.Dot(h.GetPosition() - HingeNeck.GetPosition(), HingeNeck.WorldMatrix.Up) < 0 && Controller.MoveIndicator.Z <= 0) || (Vector3D.Dot(h.GetPosition() - HingeNeck.GetPosition(), HingeNeck.WorldMatrix.Up) > 0 && Controller.MoveIndicator.Z > 0))); h.Torque = Convert.ToSingle(T > 0) * T + upLegs * 100000; h.TargetVelocityRPM = upLegs * 80 - 40; }
public void Main(string argument, UpdateType updateSource) { if (diagDisplay == null) { initDiagPanel(); } if (pivot == null) { initPivot(); } if (leftWheel == null || rightWheel == null) { initWheels(); } if (lastReportedPosition == null) { lastReportedPosition = new Dictionary <IMyEntity, Vector3D>(); } StringBuilder output = new StringBuilder(); output.Append(pivot.CustomName + "\n"); output.Append(pivot.Angle + "\n\n"); // output.Append("Mean wheel position\n"); Vector3D meanWheelPosition = getMeanPosition(leftWheel, rightWheel); // output.Append(meanWheelPosition.ToString().Replace(' ', '\n') + "\n\n"); output.Append("Distance to pivot\n"); output.Append(Vector3D.Distance(pivot.GetPosition(), meanWheelPosition) + "\n\n"); float meanHeight = (leftWheel.Height + rightWheel.Height) / 2; output.Append("Height: " + meanHeight + "\n\n"); if (pivot.Angle < 0.3 || pivot.Angle > 6) { if (pivot.Angle > 0 && pivot.Angle < 3.14) { leftWheel.Height -= .01f; rightWheel.Height -= .01f; } else { leftWheel.Height += .01f; rightWheel.Height += .01f; } } // output.Append(leftWheel.CustomName + "\n"); // output.Append(leftWheel.GetPosition().ToString().Replace(' ', '\n') + "\n\n"); // output.Append(rightWheel.CustomName + "\n"); // output.Append(rightWheel.GetPosition().ToString().Replace(' ', '\n') + "\n\n"); outputDiagnostics(output.ToString()); }