/// <summary>
/// Polar drive method for Mecanum wheeled robots.
/// </summary>
/// <remarks>A method for driving Mecanum wheeled robots in the polar 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 raw vectors.</remarks>
/// <param name="magnitude">The magnitude vector</param>
/// <param name="direction">The direction vector to drive in.</param>
/// <param name="rotation">The rate of rotation. This is independant from translation.</param>
public void mecanumDrive_Polar(double magnitude, double direction, double rotation)
{
if (!MecanumPolarReported)
{
HAL.Base.HAL.Report(ResourceType.kResourceType_RobotDrive, Instances.kRobotDrive_MecanumPolar, (byte)NumMotors);
MecanumPolarReported = true;
}
// Normalized for full power along the Cartesian axes.
magnitude = Limit(magnitude) * Math.Sqrt(2.0);
// The rollers are at 45 degree angles.
double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
double cosD = Math.Cos(dirInRad);
double sinD = Math.Sin(dirInRad);
double[] wheelSpeeds = new double[MaxNumberOfMotors];
wheelSpeeds[(int)MotorType.FrontLeft] = (sinD * magnitude + rotation);
wheelSpeeds[(int)MotorType.FrontRight] = (cosD * magnitude - rotation);
wheelSpeeds[(int)MotorType.RearLeft] = (cosD * magnitude + rotation);
wheelSpeeds[(int)MotorType.RearRight] = (sinD * magnitude - 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>
/// 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>
/// 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();
}
