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