diff --git a/VectoCore/Models/SimulationComponent/Impl/Wheels.cs b/VectoCore/Models/SimulationComponent/Impl/Wheels.cs index 9f547905e0a2b284752fab5da91b9a753e5a6d9f..ca8b53b31680c69707403aa10c0e33486877f0f0 100644 --- a/VectoCore/Models/SimulationComponent/Impl/Wheels.cs +++ b/VectoCore/Models/SimulationComponent/Impl/Wheels.cs @@ -63,24 +63,27 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl Log.Debug("request: force: {0}, velocity: {1}", force, velocity); CurrentState.AngularVelocity = velocity / _dynamicWheelRadius; - var avgAngularSpeed = CurrentState.AngularVelocity - PreviousState.AngularVelocity; - CurrentState.InertiaTorqueLoss = (_totalWheelsInertia * avgAngularSpeed / dt).Cast<NewtonMeter>(); - CurrentState.Torque = force * _dynamicWheelRadius + CurrentState.InertiaTorqueLoss; - var retVal = NextComponent.Request(absTime, dt, CurrentState.Torque, CurrentState.AngularVelocity, + var avgAngularSpeed = (CurrentState.AngularVelocity + PreviousState.AngularVelocity) / 2.0; + CurrentState.InertiaTorqueLoss = avgAngularSpeed.IsEqual(0.SI<PerSecond>()) + ? 0.SI<NewtonMeter>() + : Formulas.InertiaPower(CurrentState.AngularVelocity, PreviousState.AngularVelocity, _totalWheelsInertia, dt) / + avgAngularSpeed; //(_totalWheelsInertia * avgAngularSpeed / dt).Cast<NewtonMeter>(); + CurrentState.TorqueIn = force * _dynamicWheelRadius + CurrentState.InertiaTorqueLoss; + var retVal = NextComponent.Request(absTime, dt, CurrentState.TorqueIn, CurrentState.AngularVelocity, dryRun); - retVal.WheelsPowerRequest = CurrentState.Torque * avgAngularSpeed; + retVal.WheelsPowerRequest = CurrentState.TorqueIn * avgAngularSpeed; return retVal; } public IResponse Initialize(Newton force, MeterPerSecond velocity) { - PreviousState.Torque = force * _dynamicWheelRadius; + PreviousState.TorqueIn = force * _dynamicWheelRadius; PreviousState.AngularVelocity = velocity / _dynamicWheelRadius; PreviousState.InertiaTorqueLoss = 0.SI<NewtonMeter>(); - return NextComponent.Initialize(PreviousState.Torque, PreviousState.AngularVelocity); + return NextComponent.Initialize(PreviousState.TorqueIn, PreviousState.AngularVelocity); } #endregion @@ -100,7 +103,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl { var avgAngularSpeed = (CurrentState.AngularVelocity + PreviousState.AngularVelocity) / 2.0; - container[ModalResultField.P_wheel_in] = CurrentState.Torque * avgAngularSpeed; + container[ModalResultField.P_wheel_in] = CurrentState.TorqueIn * avgAngularSpeed; container[ModalResultField.P_wheel_inertia] = CurrentState.InertiaTorqueLoss * avgAngularSpeed; } @@ -114,7 +117,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl public class WheelsState { public PerSecond AngularVelocity; - public NewtonMeter Torque; + public NewtonMeter TorqueIn; public NewtonMeter InertiaTorqueLoss; } }