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