public SimulationWheelShaftEncoderState(string name, SimulationMotorState motor, float wheelRadius, int ticksPerRevolution)
{
Name = name;
Motor = motor;
WheelRadius = wheelRadius;
TicksPerRevolution = ticksPerRevolution;
}
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 ApplyMotorForces(SimulationMotorState motorState) {
var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
// Console.WriteLine(forceVectorWorld);
robotBody.ApplyForce(
forceVectorWorld,
robotBody.GetWorldPoint(motorState.RelativePosition)
);
// renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
}
private void DrawMotorBody(IRenderer renderer, SimulationMotorState motorState) {
var position = robotBody.GetWorldPoint(motorState.RelativePosition);
var extents = new Vector2(robotState.Width / 10, robotState.Height / 10);
renderer.DrawCenteredRectangleWorld(
position,
extents,
robotBody.Rotation,
Color.Lime);
var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
}
/// <summary>
/// Given robot :[^]:, tl/br vect is \, tr/bl vect is /
/// </summary>
/// <param name="robotWidth">Width of robot</param>
/// <param name="robotHeight">Height of robot (well, y-axis 'height')</param>
/// <param name="wheelForceAngle">magnitude of mecanum wheels' force vectors' angles (relative to ^ direction) in radians</param>
/// <param name="wheelForceAmplitude">Maximum effective force applied by wheel</param>
/// <returns></returns>
public static SimulationMotorState[] MecanumDrive(float robotWidth, float robotHeight, float wheelForceAngle, float wheelForceAmplitude)
{
var forceVector = new Vector2(0, wheelForceAmplitude);
var forceTopLeft = Vector2.Transform(forceVector, Matrix.CreateRotationZ(wheelForceAngle));
var forceTopRight = Vector2.Transform(forceVector, Matrix.CreateRotationZ(-wheelForceAngle));
var motorStates = new SimulationMotorState[4];
float halfWidth = robotWidth / 2, halfHeight = robotHeight / 2;
float backFrontSpacing = halfHeight / 2;
motorStates[0] = new SimulationMotorState("Drive.Motors.RearRight", new Vector2(halfWidth, -backFrontSpacing), forceTopLeft);
motorStates[1] = new SimulationMotorState("Drive.Motors.RearLeft", new Vector2(-halfWidth, -backFrontSpacing), forceTopRight);
motorStates[2] = new SimulationMotorState("Drive.Motors.FrontLeft", new Vector2(-halfWidth, backFrontSpacing), forceTopLeft);
motorStates[3] = new SimulationMotorState("Drive.Motors.FrontRight", new Vector2(halfWidth, backFrontSpacing), forceTopRight);
return motorStates;
}
/// <summary>
/// Given robot :[^]:, fl/fr is |, rl is \, rr is /
/// </summary>
/// <param name="robotWidth">Width of robot</param>
/// <param name="robotHeight">Height of robot (well, y-axis 'height')</param>
/// <param name="wheelForceAngle">magnitude of mecanum wheels' force vectors' angles (relative to ^ direction) in radians</param>
/// <param name="wheelForceAmplitude">Maximum effective force applied by wheel</param>
/// <returns></returns>
public static SimulationMotorState[] HybridDrive(float robotWidth, float robotHeight, float wheelForceAngle, float wheelForceAmplitude)
{
var forwardForceVector = new Vector2(0, wheelForceAmplitude);
var forceRearLeft = Vector2.Transform(forwardForceVector, Matrix.CreateRotationZ(-wheelForceAngle));
var forceRearRight = Vector2.Transform(forwardForceVector, Matrix.CreateRotationZ(wheelForceAngle));
var motorStates = new SimulationMotorState[4];
float halfWidth = robotWidth / 2, halfHeight = robotHeight / 2;
float backFrontSpacing = halfHeight / 2;
motorStates[0] = new SimulationMotorState("Drive.Motors.RearRight", new Vector2(halfWidth, -backFrontSpacing), forceRearLeft);
motorStates[1] = new SimulationMotorState("Drive.Motors.RearLeft", new Vector2(-halfWidth, -backFrontSpacing), forceRearRight);
motorStates[2] = new SimulationMotorState("Drive.Motors.FrontLeft", new Vector2(-halfWidth, backFrontSpacing), forwardForceVector);
motorStates[3] = new SimulationMotorState("Drive.Motors.FrontRight", new Vector2(halfWidth, backFrontSpacing), forwardForceVector);
return(motorStates);
}
public static SimulationMotorState[] RovDrive(float robotWidth, float robotHeight, float wheelForceAngle, float wheelForceAmplitude)
{
var forceVector = new Vector2(0, wheelForceAmplitude);
var forceTopLeft = Vector2.Transform(forceVector, Matrix.CreateRotationZ(-wheelForceAngle));
var forceTopRight = Vector2.Transform(forceVector, Matrix.CreateRotationZ(wheelForceAngle));
var motorStates = new SimulationMotorState[7];
float halfWidth = robotWidth / 2, halfHeight = robotHeight / 2;
float backFrontSpacing = halfHeight / 2;
motorStates[0] = new SimulationMotorState("Drive.Motors.RearRight", new Vector2(halfWidth, -backFrontSpacing), forceTopLeft);
motorStates[1] = new SimulationMotorState("Drive.Motors.RearLeft", new Vector2(-halfWidth, -backFrontSpacing), forceTopRight);
motorStates[2] = new SimulationMotorState("Drive.Motors.FrontLeft", new Vector2(-halfWidth, backFrontSpacing), forceTopLeft);
motorStates[3] = new SimulationMotorState("Drive.Motors.FrontRight", new Vector2(halfWidth, backFrontSpacing), forceTopRight);
// dummy vertical motors
motorStates[4] = new SimulationMotorState("Drive.Motors.VertFrontLeft", Vector2.Zero, forceVector);
motorStates[5] = new SimulationMotorState("Drive.Motors.VertFrontRight", Vector2.Zero, forceVector);
motorStates[6] = new SimulationMotorState("Drive.Motors.VertRear", Vector2.Zero, forceVector);
return(motorStates);
}
public static SimulationWheelShaftEncoderState FromMotor(SimulationMotorState motor, float wheelRadius, int ticksPerRevolution)
{
return(new SimulationWheelShaftEncoderState(motor.Name + ".Encoder", motor, wheelRadius, ticksPerRevolution));
}
public static SimulationWheelShaftEncoderState FromMotor(SimulationMotorState motor, float wheelRadius, int ticksPerRevolution)
{
return new SimulationWheelShaftEncoderState(motor.Name + ".Encoder", motor, wheelRadius, ticksPerRevolution);
}
public static SimulationWheelShaftEncoderState[] FromMotors(SimulationMotorState[] motors, float wheelRadius, int ticksPerRevolution)
{
return Util.Generate(motors.Length, i => FromMotor(motors[i], wheelRadius, ticksPerRevolution));
}
private void ApplyMotorForces(SimulationMotorState motorState)
{
var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
// Console.WriteLine(forceVectorWorld);
robotBody.ApplyForce(
forceVectorWorld,
robotBody.GetWorldPoint(motorState.RelativePosition)
);
// renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
}
private void DrawMotorBody(IRenderer renderer, SimulationMotorState motorState)
{
var position = robotBody.GetWorldPoint(motorState.RelativePosition);
var extents = new Vector2(robotState.Width / 10, robotState.Height / 10);
renderer.DrawCenteredRectangleWorld(
position,
extents,
robotBody.Rotation,
Color.Lime);
var forceVectorWorld = Vector2.Transform(motorState.CurrentForceVector, Matrix.CreateRotationZ(robotBody.Rotation));
renderer.DrawForceVectorWorld(position, forceVectorWorld, Color.Cyan);
}
