I’m having some trouble getting mecanum to work the way I believe it’s supposed to. According to the Java docs, if I supply a positive X speed, the bot should move forward. The java docs also state that the MecanumDrive inverts the motors on the right so (usually) I wouldn’t have to.
After spending a while trying to make sense of what I am seeing, I decided to look at the MecanumDrive class and 1) I don’t find any motor inversion and, 2) I’m concerned about the line in DriveCartesionIK which reads “Vector2d input = new Vector2d(ySpeed, xSpeed);”. This line puts the Y value in the x location in the vector and the y value in the X location. The result is that the positive X value (to move all wheels forward together) actually results in negative values for front right and rear left. Is this right? Seems like if the vector was built as X,Y (instead of Y,X), all for motor values would be positive.
As a test, I made my own version of this class and switched the xSpeed and ySpeed values on line 231, and inverted the right side motors in my subsystem code (since the drive really doesn’t do that) and it works perfectly. I hate to go messing around in the WPI code like that so I’m putting this out there in case I misunderstand something.
In what Javadocs did you read this? All right-side-inversion stuff was removed in 2022. Invert your motor controller objects (if needed) before passing them into the *Drive object.
Bet it is related to this regardless of what the docs say.
“Drive class functions such as DifferentialDriveno longer invert the right side by default. Please use setInverted on your motor controllers to invert the right side to maintain the same behavior”
Good documentation are a god send, but docs are often wrong even when they are good.
Yeah, I saw that and read it as DifferentialDrive but probably should have interpreted it more generally. It still doesn’t address the other issue I describe. Inverting them without changing the ySpeed, xSpeed order in the vector results in erroneous behavior (at least to me).
/**
* Cartesian inverse kinematics for Mecanum platform.
*
* <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
* from its angle or rotation rate.
*
* @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
* @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
* @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
* positive.
* @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
* to implement field-oriented controls.
* @return Wheel speeds.
*/
@SuppressWarnings("ParameterName")
public static WheelSpeeds driveCartesianIK(
double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
// Compensate for gyro angle.
Vector2d input = new Vector2d(ySpeed, xSpeed);
input.rotate(-gyroAngle);
double[] wheelSpeeds = new double[4];
wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y - zRotation;
wheelSpeeds[MotorType.kRearLeft.value] = input.x - input.y + zRotation;
wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
normalize(wheelSpeeds);
return new WheelSpeeds(
wheelSpeeds[MotorType.kFrontLeft.value],
wheelSpeeds[MotorType.kFrontRight.value],
wheelSpeeds[MotorType.kRearLeft.value],
wheelSpeeds[MotorType.kRearRight.value]);
}
Looks like it got the X and Y components of the input vector mixed up. ySpeed being the x component is intentional to compensate for the gyro convention (North-East-Up) being different from math convention (North-West-Up). However, that should also change the equations in the wheelSpeeds assignments.
Could be, but it says X is forward in multiple places in the documentation I sent the link for. Also, this document says that “The direction the robot is facing is the positive x axis, and the positive y axis is perpendicular, to the left of the robot.” Coordinate Systems — FIRST Robotics Competition documentation(or,the%20robot%20is%20the%20origin.&text=WPILib’s%20Gyro%20class%20is%20clockwise,rotation%20with%20either%20coordinate%20system.
PRs have been submitted to frc-docs to specifically call out MecanumDrive as well as Differential drive, and remove the comments in the javadocs about inversion in MecanumDrive.
What about all the places the docs say “The positive X axis points ahead, the positive Y axis points right, and the positive Z axis points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is positive.” and " ySpeed - The robot’s speed along the Y axis [-1.0…1.0]. Right is positive.xSpeed - The robot’s speed along the X axis [-1.0…1.0]. Forward is positive." I find these statements misleading if Y is really straight ahead. Also, Y straight ahead doesn’t match other docs regarding the robot coordinate system.