public TP(ushort id, SubP p) : base(id, p)
{
loopturn = false;
NormWhAng = 35; Tangle = 0;
switchingsusp = false;
SuspensionMode = SuspensionModes.sport; DriveMode = DriveModes.full;
FirstStr = ""; SecondStr = "";
Wheels = new List <IMyMotorSuspension>();
if (
(DriverLCD = OS.GTS.GetBlockWithName("Screen Driver") as IMyTextPanel) == null ||
(WheelLF = OS.GTS.GetBlockWithName("Wheel Suspension 3x3 LF") as IMyMotorSuspension) == null ||
(WheelRF = OS.GTS.GetBlockWithName("Wheel Suspension 3x3 RF") as IMyMotorSuspension) == null ||
(WheelLB = OS.GTS.GetBlockWithName("Wheel Suspension 3x3 LB") as IMyMotorSuspension) == null ||
(WheelRB = OS.GTS.GetBlockWithName("Wheel Suspension 3x3 RB") as IMyMotorSuspension) == null ||
(RemoteDriver = OS.GTS.GetBlockWithName("Control car") as IMyRemoteControl) == null ||
(RotorRull = OS.GTS.GetBlockWithName("Rotor rull") as IMyMotorStator) == null
)
{
Terminate("Kontur blocks not found.");
return;
}
Wheels = new List <IMyMotorSuspension>()
{
WheelLF, WheelRF, WheelLB, WheelRB
};
string saved = DriverLCD.GetText();
if (string.IsNullOrEmpty(saved))
{
if (saved.Contains("front"))
{
DriveMode = DriveModes.front;
}
else if (saved.Contains("rear"))
{
DriveMode = DriveModes.rear;
}
if (saved.Contains("off-road"))
{
SuspensionMode = SuspensionModes.offroad;
}
if (saved.Contains("looper"))
{
loopturn = true;
}
}
ChangeFirst();
AddAct(ref MA, Main, 1);
SetCmd(new Dictionary <string, Cmd>
{
{ "sdm", new Cmd(CmdSDM, "Switch drive mode Full/Front/Rear.") },
{ "ssm", new Cmd(CmdSSM, "Switch suspension mode Sport/Off road.") },
{ "sl", new Cmd(CmdSL, "Switch loopturn mode.") }
});
}
/// <summary>Creates a new power wheel</summary>
/// <param name="wheel">The actual wheel it wraps</param>
/// <param name="wb">The wheel base, to which the power wheel will be added</param>
/// <param name="tform">Coordinates transformer, should be one that makes the Z axis parallel the wheel direction and Y axis the up axis</param>
public PowerWheel(IMyMotorSuspension wheel, WheelBase wb, CoordinatesTransformer tform)
{
this.wheelBase = wb;
this.CubeSize = wheel.CubeGrid.GridSizeEnum;
this.Position = tform.Pos(wheel.GetPosition());
this.IsRight = RIGHT.Dot(tform.Dir(wheel.WorldMatrix.Up)) > 0;
this.reverseY = UP.Dot(tform.Dir(wheel.WorldMatrix.Backward)) > 0;
this.transformer = tform;
this.wheel = wheel;
wheel.Height = 10;
this.maxY = wheel.Height;
wheel.Height = -10;
this.minY = wheel.Height;
if (this.reverseY)
{
float tmp = this.minY;
this.minY = -this.maxY;
this.maxY = -tmp;
}
this.WheelSize = (wheel.Top.Max - wheel.Top.Min).X + 1;
this.Mass = this.CubeSize == MyCubeSize.Small
? this.WheelSize == 1 ? 105 : (this.WheelSize == 3 ? 205 : 310)
: this.WheelSize == 1 ? 420 : (this.WheelSize == 3 ? 590 : 760);
// real center of rotation of the wheel
this.Position += tform.Dir(wheel.WorldMatrix.Up) * wheel.CubeGrid.GridSize;
this.wheelBase.AddWheel(this);
}
float GetForce(IMyMotorSuspension wheel)
{
switch (wheel.BlockDefinition.SubtypeId)
{
case "Suspension1x1mirrored":
case "Suspension1x1":
return(20000);
case "Suspension3x3mirrored":
case "Suspension3x3":
return(60000);
case "Suspension5x5mirrored":
case "Suspension5x5":
return(100000);
case "SmallSuspension1x1mirrored":
case "SmallSuspension1x1":
return(120);
case "SmallSuspension3x3mirrored":
case "SmallSuspension3x3":
return(1920);
case "SmallSuspension5x5mirrored":
case "SmallSuspension5x5":
return(4800);
}
throw new Exception("Unknown wheel type.");
}
public SuspensionWrapper(IMyMotorSuspension newSuspension, ref IMyShipController controller)
{
Suspension = newSuspension;
Controller = controller;
ToGridCenterDistance = (float)(Controller.CubeGrid.GetPosition() - Suspension.GetPosition()).Length();
Suspension.MaxSteerAngle = (float)(Math.PI / 4.0);
}
public override void UpdateAfterSimulation100()
{
if (suspension == null)
{
suspension = wheel.Base as IMyMotorSuspension;
}
if (wheel == null || !wheel.IsFunctional || suspension == null)
{
return;
}
water = WaterMod.Static.GetClosestWater(wheel.GetPosition());
if (water != null)
{
MyTuple <float, float> WheelSettings;
Vector3D WheelPosition = wheel.PositionComp.GetPosition();
if (water.IsUnderwater(ref WheelPosition))
{
if (suspension.Friction > 20f || suspension.Power > 40f)
{
WaterMod.Static.WheelStorage[wheel.EntityId] = new MyTuple <float, float>(suspension.Friction, suspension.Power);
suspension.Friction = Math.Min(suspension.Friction, 20f);
suspension.Power = Math.Min(suspension.Power, 40f);
}
}
else if (WaterMod.Static.WheelStorage.TryGetValue(wheel.EntityId, out WheelSettings))
{
suspension.Friction = WheelSettings.Item1;
suspension.Power = WheelSettings.Item2;
WaterMod.Static.WheelStorage.Remove(wheel.EntityId);
}
}
}
public void Update(Vector3D target)
{
MatrixD transpose = MatrixD.Transpose(rc.WorldMatrix);
Vector3D meToTarget = rc.WorldMatrix.Translation - target;
Vector3D localError = Vector3D.TransformNormal(meToTarget, transpose);
localError.Y = 0;
if (localError.X > -0.5 && localError.X < 0.5)
{
localError.X = 0;
}
if (localError.Z > -0.5 && localError.Z < 0.5)
{
localError.Z = 0;
}
float correction = (float)forwardPID.Control(localError.Z);
float force = correction * rc.CalculateShipMass().TotalMass;
float rightLeft = (float)anglePID.Control(-localError.X);
Vector3D localVelocity = Vector3D.TransformNormal(rc.GetShipVelocities().LinearVelocity, transpose);
float angle = -rightLeft;
if (localVelocity.Z < 0)
{
angle *= -1;
}
foreach (Wheel w in wheels)
{
IMyMotorSuspension wheel = w.block;
if (first)
{
Vector3D center = Vector3D.TransformNormal(rc.CenterOfMass - rc.GetPosition(), transpose);
Vector3D local = Vector3D.TransformNormal(wheel.GetPosition() - rc.GetPosition(), transpose);
wheel.InvertSteer = (local.Z > center.Z);
wheel.InvertPropulsion = (wheel.Orientation.Left != rc.Orientation.Forward);
wheel.Brake = false;
}
if (wheel.Steering)
{
wheel.SetValueFloat("Steer override", angle);
}
if (wheel.Propulsion)
{
float maxForce = w.maxForce;
if (maxForce <= 0)
{
continue;
}
float percent = MathHelper.Clamp(force / maxForce, -1, 1);
force -= percent * maxForce;
wheel.SetValueFloat("Propulsion override", percent);
}
}
first = false;
}
public Wheel(IMyMotorSuspension wheel, GridNode gridNode)
{
this.wheel = wheel;
this.gridNode = gridNode;
relativePosition = gridNode.GetRelativePosition(wheel);
sideMult = relativePosition.X > 0 ? -1 : 1;
}
public override void Init(MyObjectBuilder_EntityBase objectBuilder)
{
if (MyAPIGateway.Session.IsServer)
{
wheel = Entity as IMyWheel;
NeedsUpdate = MyEntityUpdateEnum.EACH_100TH_FRAME;
suspension = wheel.Base as IMyMotorSuspension;
}
}
public void WheelSettings(float powerFactor = 0, float powerOffset = 0, float heightFactor = 0, float heightOffset = 0, float frictionFactor = 0, float frictionOffset = 0)
{
for (int i = 0; i < Wheels.Length; i++)
{
var wheel = Wheels[i];
IMyMotorSuspension suspension = wheel.Suspension;
suspension.Power = (suspension.Power * powerFactor) + powerOffset;
suspension.Height = (suspension.Height * heightFactor) + heightOffset;
suspension.Friction = (suspension.Friction * frictionFactor) + frictionOffset;
}
}
public void Reset()
{
foreach (Wheel w in wheels)
{
IMyMotorSuspension wheel = w.block;
wheel.SetValueFloat("Propulsion override", 0);
wheel.SetValueFloat("Steer override", 0);
wheel.InvertPropulsion = false;
wheel.InvertSteer = false;
wheel.Brake = true;
}
first = true;
}
public void AddWheel(IMyMotorSuspension wheel)
{
Base6Directions.Direction blockOrientation = Controller.Orientation.TransformDirectionInverse(wheel.Orientation.Up);
if (blockOrientation == Base6Directions.Direction.Left || blockOrientation == Base6Directions.Direction.Right)
{
double steeringAngle = Math.Abs(GetAngleFromCoM(wheel.GetPosition()));
if (steeringAngle > Math.PI / 2)
{
steeringAngle -= Math.PI / 2;
}
if (!IsFrontSide(wheel.GetPosition()))
{
steeringAngle = Math.PI / 2 - steeringAngle;
}
Wheels.Add(new VehicleWheel(new SuspensionWrapper(wheel, blockOrientation), IsFrontSide(wheel.GetPosition()), steeringAngle / 90));
}
}
bool FuncTest(IMyMotorSuspension block)
{
//Wheel Suspension 1x1
//Wheel Suspension 3x3
//Wheel Suspension 5x5
//Interface name: IMyMotorSuspension
//Parent: IMyMotorBase
//Parent: IMyFunctionalBlock
//Fields:
bool Steering = block.Steering;
bool Propulsion = block.Propulsion;
float Damping = block.Damping;
float Strength = block.Strength;
float Friction = block.Friction;
float Power = block.Power;
return(true);
}
private bool initWheels()
{
IMyTerminalBlock block = GridTerminalSystem.GetBlockWithName("Balancer Wheel Left");
IMyMotorSuspension leftWheel = block as IMyMotorSuspension;
block = GridTerminalSystem.GetBlockWithName("Balancer Wheel Right");
IMyMotorSuspension rightWheel = block as IMyMotorSuspension;
if (leftWheel == null || rightWheel == null)
{
return(false);
}
else
{
this.leftWheel = leftWheel;
this.rightWheel = rightWheel;
return(true);
}
}
public void Drive(float propulsionOverride, float steeringOverride)
{
Vector3D forward = Controller.WorldMatrix.Forward;
Vector3D com = Controller.CenterOfMass;
for (int i = 0; i < Wheels.Length; i++)
{
var wheel = Wheels[i];
IMyMotorSuspension suspension = wheel.Suspension;
float propulsionFactor = (float)suspension.WorldMatrix.Left.Dot(forward);
suspension.SetValueFloat("Propulsion override", propulsionFactor * propulsionOverride);
Vector3D offset = com - suspension.GetPosition();
float steeringFactor = (float)offset.Dot(forward);
suspension.SetValueFloat("Steer override", steeringFactor * steeringOverride);
}
}
float Smoothing(IMyMotorSuspension ThisMotor, string variable, float needed, float Step)
{
float TempFloat = ThisMotor.GetValueFloat(variable), mult;
if (Math.Abs(TempFloat - needed) > Step)
{
if (TempFloat < needed)
{
mult = 1;
}
else
{
mult = -1;
}
TempFloat = TempFloat + mult * Step;
}
else
{
TempFloat = needed;
}
return(TempFloat);
}
public override void Init(MyObjectBuilder_EntityBase objectBuilder)
{
wheel = Entity as IMyMotorSuspension;
NeedsUpdate = MyEntityUpdateEnum.EACH_100TH_FRAME;
}
private float GetPower(IMyMotorSuspension wheel)
{
return(wheel.Power);
}
public static void SetSteerSpeed(IMyMotorSuspension motorSuspension, float steerSpeed)
{
motorSuspension.SetValue <float>("SteerSpeed", steerSpeed);
}
{ //IMyMotorSuspension API wrapper
//PROPERTIES:
///public static void SetSteering(IMyMotorSuspension motorSuspension, bool steering){
/// motorSuspension.SetValue<bool>("Steering", steering);
///}
public static void SetMaxSteerAngle(IMyMotorSuspension motorSuspension, float maxSteerAngle)
{
motorSuspension.SetValue <float>("MaxSteerAngle", maxSteerAngle);
}
public static void SetSpeedLimit(IMyMotorSuspension motorSuspension, float speedLimit) //in kmph
{
motorSuspension.SetValue <float>("Speed Limit", speedLimit);
}
///public static void SetHeight(IMyMotorSuspension motorSuspension, float height){
/// motorSuspension.SetValue<float>("Height", height);
///}
///public static void SetTravel(IMyMotorSuspension motorSuspension, float travel){
/// motorSuspension.SetValue<float>("Travel", travel);
///}
public static float GetSpeedLimit(IMyMotorSuspension motorSuspension) //in kmph
{
return(motorSuspension.GetValue <float>("Speed Limit"));
}
public Wheel(IMyMotorSuspension wheel, String name)
{
this.wheel = wheel;
this.name = name;
}
public Wheel(IMyMotorSuspension block, float maxForce)
{
this.block = block;
this.maxForce = maxForce;
}
///public static void SetInvertSteering(IMyMotorSuspension motorSuspension, bool invertSteering){
/// motorSuspension.SetValue<bool>("InvertSteering", invertSteering);
///}
///public static void SetPropulsion(IMyMotorSuspension motorSuspension, bool propulsion){
/// motorSuspension.SetValue<bool>("Propulsion", propulsion);
///}
///public static void SetInvertPropulsion(IMyMotorSuspension motorSuspension, bool invertPropulsion){
/// motorSuspension.SetValue<bool>("InvertPropulsion", invertPropulsion);
///}
public static void SetPower(IMyMotorSuspension motorSuspension, float power)
{
motorSuspension.SetValue <float>("Power", power);
}
private void Update()
{
{ //Apply handbrake if controller is not in use
if (!controller.IsUnderControl && !controller.HandBrake)
{
ShipController.SetHandBrake(controller, true);
}
}
MyShipVelocities
vWorld = controller.GetShipVelocities();
Vector3D
worldForward = GridToWorld(shipForward, controller.CubeGrid),
vWorldForward = Project(vWorld.LinearVelocity, worldForward);
float
speedForward = (float)(vWorldForward.Length() * kmph_p_mps),
vForward = speedForward * Math.Sign(Vector3D.Dot(vWorldForward, worldForward));
WheelControls
controls = new WheelControls(controller);
//Manage automatic gearbox
if (status.gearsAutomatic)
{
//check which gear we should be in
//-compare to vForward to limit to lowest gear for reversing
if ((status.gear < gearMax) && (vForward >= 0.9f * gears[status.gear].speedLimitMax))
{
status.gear++;
status.modified = true;
}
else if ((status.gear > 0) && (vForward < 0.9f * gears[status.gear - 1].speedLimitMax))
{
status.gear--;
status.modified = true;
}
}
//Pre-calculate traction control values
float
speedLimitMax = gears[status.gear].speedLimitMax,
speedLimit = MyMath.Clamp(speedForward + 1.0f, 1.0f, speedLimitMax);
float
power = gears[status.gear].power;
bool
speedUnsafe = (speedForward >= speedLimitMax * 1.1f),
brake = controls.brake || speedUnsafe;
//Check each suspension unit
//-only control suspension units on our grid
//-check that we have access to the suspension unit
GridTerminalSystem.GetBlocksOfType <IMyMotorSuspension>(temp);
int count = 0;
for (int i = 0; i < temp.Count; i++)
{
IMyMotorSuspension suspension = (IMyMotorSuspension)temp[i];
if (suspension.CubeGrid != controller.CubeGrid)
{
continue;
}
count++;
//Check if we can actually use the suspension unit
if (ValidateBlock(suspension, callbackRequired: false))
{
//Apply traction control
//-limit wheel speed based on vehicle's forward speed
//-apply brakes while speed is unsafe
MotorSuspension.SetSpeedLimit(suspension, speedLimit);
if (!controller.HandBrake)
{
suspension.Brake = brake;
}
//Apply gear ratio
if (suspension.Power != power)
{
MotorSuspension.SetPower(suspension, power);
}
}
} //end for
//Output status
Echo("vF : " + speedForward.ToString("F1") + " kmph");
Echo("gear : " + gears[status.gear].name);
Echo("mode : " + (status.gearsAutomatic ? "Auto" : "Manual"));
//Echo(temp.Count.ToString() +" wheels found.");
//Echo(count.ToString() +" processed.");
}
///public static void SetFriction(IMyMotorSuspension motorSuspension, float friction){
/// motorSuspension.SetValue<float>("Friction", friction);
///}
///public static void SetDamping(IMyMotorSuspension motorSuspension, float damping){
/// motorSuspension.SetValue<float>("Damping", damping);
///}
public static void SetStrength(IMyMotorSuspension motorSuspension, float strength)
{
motorSuspension.SetValue <float>("Strength", strength);
}
public SuspensionWrapper(IMyMotorSuspension suspension, Base6Directions.Direction orientation, bool subgrid = false)
{
Obj = suspension;
OrientationInVehicle = orientation;
IsSubgrid = subgrid;
if (orientation == Base6Directions.Direction.Left)
{
PropulsionSign = -1;
}
else if (orientation == Base6Directions.Direction.Right)
{
PropulsionSign = 1;
}
HeightOffsetMin = suspension.GetMinimum <float>("Height");
HeightOffsetMax = suspension.GetMaximum <float>("Height");
HeightOffsetRange = HeightOffsetMax - HeightOffsetMin;
if (suspension.CubeGrid.GridSizeEnum == MyCubeSize.Small)
{
if (suspension.BlockDefinition.SubtypeName.Contains("5x5"))
{
WheelRadius = 1.25;
}
else if (suspension.BlockDefinition.SubtypeName.Contains("3x3"))
{
WheelRadius = 0.75;
}
else if (suspension.BlockDefinition.SubtypeName.Contains("2x2"))
{
WheelRadius = 0.5; // modded
}
else if (suspension.BlockDefinition.SubtypeName.Contains("1x1"))
{
WheelRadius = 0.25;
}
else // some other modded wheels
{
WheelRadius = suspension.IsAttached ? suspension.Top.WorldVolume.Radius * 0.79 / MathHelper.Sqrt2 : 0;
}
}
else
{
if (suspension.BlockDefinition.SubtypeName.Contains("5x5"))
{
WheelRadius = 6.25;
}
else if (suspension.BlockDefinition.SubtypeName.Contains("3x3"))
{
WheelRadius = 3.75;
}
else if (suspension.BlockDefinition.SubtypeName.Contains("2x2"))
{
WheelRadius = 2.5; // modded
}
else if (suspension.BlockDefinition.SubtypeName.Contains("1x1"))
{
WheelRadius = 1.25;
}
else // some other modded wheels
{
WheelRadius = suspension.IsAttached ? suspension.Top.WorldVolume.Radius * 0.79 / MathHelper.Sqrt2 : 0;
}
}
}
