public SimulationRobotState(float width, float height, float density, SimulationMotorState[] motorStates, SimulationWheelShaftEncoderState[] wheelShaftEncoderStates, SimulationGyroscopeState yawGyroscopeState)
{
this.width = width;
this.height = height;
this.density = density;
this.motorStates = motorStates;
this.wheelShaftEncoderStates = wheelShaftEncoderStates;
this.yawGyroscopeState = yawGyroscopeState;
}
private void UpdateWheelShaftEncoder(float dtSeconds, SimulationWheelShaftEncoderState wheelShaftEncoderState)
{
var motor = wheelShaftEncoderState.Motor;
var currentWorldPosition = robotBody.GetWorldPoint(motor.RelativePosition);
if (!wheelShaftEncoderState.HasBeenUpdated) {
wheelShaftEncoderState.HasBeenUpdated = true;
} else {
var deltaPositionAbsolute = currentWorldPosition - wheelShaftEncoderState.LastWorldPosition;
var deltaPositionRelative = Vector2.Transform(deltaPositionAbsolute, Matrix.CreateRotationZ(-robotBody.Rotation));
var countedDirection = motor.MaxForceVector;
countedDirection.Normalize();
var deltaPosition = Vector2.Dot(deltaPositionRelative, countedDirection);
// var deltaPosition = deltaPositionRelative.Length();
var velocity = deltaPosition / dtSeconds;
// arc length = theta * r => theta = arc length / r
wheelShaftEncoderState.Angle += deltaPosition / wheelShaftEncoderState.WheelRadius;
// omega = delta arc length / r
wheelShaftEncoderState.AngularVelocity = velocity / wheelShaftEncoderState.WheelRadius;
}
wheelShaftEncoderState.LastWorldPosition = currentWorldPosition;
}
public SimulationIncrementalRotaryEncoderAdapter(SimulationWheelShaftEncoderState state, float ticksPerRevolution)
: base(state.Name, DeviceType.RotaryEncoder)
{
this.state = state;
this.radiansPerTick = (float)(2.0 * Math.PI / ticksPerRevolution);
}
public SimulationIncrementalRotaryEncoderAdapter(SimulationWheelShaftEncoderState state, float ticksPerRevolution) : base(state.Name, DeviceType.RotaryEncoder)
{
this.state = state;
this.radiansPerTick = (float)(2.0 * Math.PI / ticksPerRevolution);
}
