/// <summary>
/// Cartesian drive method for Mecanum wheeled robots
/// </summary>
/// <remarks>A method for driving Mecanum wheeled robots in the cartesian plane.
/// There are 4 wheels on the robot, arranged so that the front and back wheels are
/// toed in 45 degrees. When looking at the wheels from the top, the roller axles should
/// form an X across the robot.
/// <para/>This is designed to be directly driven by joystick axes.</remarks>
/// <param name="x">The speed that the robot should drive in the X direction.</param>
/// <param name="y">The speed that the robbot should drive in the Y direction.</param>
/// <param name="rotation">The rate of rotation for the robot that is independed of translation.</param>
/// <param name="gyroAngle">The current angle reading from the gyro. Use this to implement field-oriented controls.</param>
public void MecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle = 0)
{
if (!MecanumCartesianReported)
{
HAL.Base.HAL.Report(ResourceType.kResourceType_RobotDrive, Instances.kRobotDrive_MecanumCartesian, (byte)NumMotors);
MecanumCartesianReported = true;
}
double xIn = x;
double yIn = y;
// Negate y for the joystick.
yIn = -yIn;
// Compenstate for gyro angle.
RotateVector(ref xIn, ref yIn, gyroAngle);
double[] wheelSpeeds = new double[MaxNumberOfMotors];
wheelSpeeds[(int)MotorType.FrontLeft] = xIn + yIn + rotation;
wheelSpeeds[(int)MotorType.FrontRight] = -xIn + yIn - rotation;
wheelSpeeds[(int)MotorType.RearLeft] = -xIn + yIn + rotation;
wheelSpeeds[(int)MotorType.RearRight] = xIn + yIn - rotation;
Normalize(wheelSpeeds);
FrontLeftMotor.Set(wheelSpeeds[(int)MotorType.FrontLeft] * MaxOutput, SyncGroup);
FrontRightMotor.Set(wheelSpeeds[(int)MotorType.FrontRight] * MaxOutput, SyncGroup);
RearLeftMotor.Set(wheelSpeeds[(int)MotorType.RearLeft] * MaxOutput, SyncGroup);
RearRightMotor.Set(wheelSpeeds[(int)MotorType.RearRight] * MaxOutput, SyncGroup);
if (SyncGroup != 0)
{
CANJaguar.UpdateSyncGroup(SyncGroup);
}
SafetyHelper?.Feed();
}
public static void SetVoltage(this ISpeedController @this, ElectricPotential potential)
{
if (@this == null)
{
throw new ArgumentNullException(nameof(@this));
}
@this.Set(potential / RobotController.BatteryVoltage);
}
public bool Reset()
{
Initialize();
bool wasReset = true;
m_motor.Inverted = false;
m_motor.Set(0);
Timer.Delay(TestBench.MotorStopTime);
m_encoder.Reset();
foreach (var c in m_counters)
{
c.Reset();
}
wasReset = wasReset && m_motor.Get() == 0;
wasReset = wasReset && m_encoder.Get() == 0;
return(m_counters.Aggregate(wasReset, (current, c) => current && c.Get() == 0));
}
/// <summary>
/// Sets the speed of the left and right drive motors
/// </summary>
/// <param name="leftOutput">The speed to send to the left side.</param>
/// <param name="rightOutput">The speed to send to the right side.</param>
public void SetLeftRightMotorOutputs(double leftOutput, double rightOutput)
{
if (RearLeftMotor == null || RearRightMotor == null)
{
throw new NullReferenceException("The motor controllers have been set to null.");
}
FrontLeftMotor?.Set(Limit(leftOutput) * MaxOutput, SyncGroup);
RearLeftMotor.Set(Limit(leftOutput) * MaxOutput, SyncGroup);
FrontRightMotor?.Set(-Limit(rightOutput) * MaxOutput, SyncGroup);
RearRightMotor.Set(-Limit(rightOutput) * MaxOutput, SyncGroup);
if (SyncGroup != 0)
{
CANJaguar.UpdateSyncGroup(SyncGroup);
}
SafetyHelper?.Feed();
}
/// <summary>
/// Output to the motor to turn.
/// </summary>
/// <param name="value"></param>
public void PidWrite(double value)
{
_SpeedController.Set(value);
}