/**
* Holonomic Drive method for omni wheel robots
*
* <p>This is a modified version of the WPILIB Mecannum code. The formula works but there is no
* need to invert the rotations because you rotate but increasing the speed of every wheel
*
* @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
* @param direction The direction the robot should drive. The direction and maginitute are
* independent of the rotation rate.
* @param rotation The rate of rotation for the robot that is completely independent of the
* magnitude or direction. [-1.0..1.0]
*/
public void holonomicDrive(double magnitude, double direction, double rotation) {
// 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.cos(dirInRad);
double wheelSpeeds[] = new double[kMaxNumberOfMotors];
wheelSpeeds[MotorType.kFrontLeft.value] = (sinD * magnitude + rotation);
wheelSpeeds[MotorType.kRearRight.value] = (sinD * magnitude - rotation);
wheelSpeeds[MotorType.kFrontRight.value] = (cosD * magnitude + rotation);
wheelSpeeds[MotorType.kRearLeft.value] = (cosD * magnitude - rotation);
normalize(wheelSpeeds);
byte syncGroup = (byte) 0x80;
m_frontLeftMotor.set(
wheelSpeeds[MotorType.kFrontLeft.value]
* m_invertedMotors[MotorType.kFrontLeft.value]
* m_maxOutput,
syncGroup);
m_frontRightMotor.set(
wheelSpeeds[MotorType.kFrontRight.value]
* m_invertedMotors[MotorType.kFrontRight.value]
* m_maxOutput,
syncGroup);
m_rearLeftMotor.set(
wheelSpeeds[MotorType.kRearLeft.value]
* m_invertedMotors[MotorType.kRearLeft.value]
* m_maxOutput,
syncGroup);
m_rearRightMotor.set(
wheelSpeeds[MotorType.kRearRight.value]
* m_invertedMotors[MotorType.kRearRight.value]
* m_maxOutput,
syncGroup);
if (m_isCANInitialized) {
try {
CANJaguar.updateSyncGroup(syncGroup);
} catch (CANNotInitializedException e) {
m_isCANInitialized = false;
} catch (CANTimeoutException e) {
}
}
if (m_safetyHelper != null) {
m_safetyHelper.feed();
}
}
/**
* Move the arm
*
* @param rpm the speed of the arm
*/
public void moveArm(double speed) {
try {
if (turbomode) {
jag.setX(.6 * (speed));
try {
CANJaguar.updateSyncGroup(syncGroup);
} catch (CANNotInitializedException e) {
}
} else {
jag.setX(.3 * (speed));
try {
CANJaguar.updateSyncGroup(syncGroup);
} catch (CANNotInitializedException e) {
}
}
} catch (CANTimeoutException ex) {
// ex.printStackTrace();
System.out.println("CANNDriveTrain.java:133");
}
if (m_safetyHelper != null) m_safetyHelper.feed();
}
