/// <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();
}
/// <summary> /// Feeds the Motor Safety Timer /// </summary> /// <remarks>This method should be called by the derived class whenever its /// setpoint gets updated, in order to feed the watchdog.</remarks> public void Feed() => m_safetyHelper.Feed();