Hi, I’m the lead programmer for Team 7913. We’ve been implementing our swerve drive using YAGSL and struggling with a perplexing problem in our drive controls.
The ideal would be that regardless of the rotation of the robot, pushing up on the stick would move the robot along the 0° field heading (away from the driver).
Instead, it seems that the direction the robot considers away relative to the field changes based on its angle.
Pushing the joystick up when facing different directions follows the following guide from our testing. Other directions on the joystick correspond accordingly (if up is toward, then down on the stick is away for the robot, left is right, right is left).
| Robot Rotation (0° is pointing away from driver) | Field-Oriented Heading When Pushing Up On The Joystick |
|---|---|
| The ideal | |
| Any rotation | 0° |
| What actually happens | |
| 0° | 0° |
| 45° | 90° |
| 90° | 180° |
| 135° | 270° |
| 180° | 0° |
| 225° | 90° |
| 270° | 180° |
| 315° | 270° |
We’re mostly using the example code from the library. The call stack for driving looks like this.
RobotContainer command that is set as the subsystem default command
Command driveFieldOrientedDirectAngle = swerveSubsystem.driveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRightX(),
() -> -driverXbox.getRightY());
The drive command supplier function in the swerve subsystem
public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX,
DoubleSupplier headingY) {
// swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control.
return run(() -> {
double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth control out
double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth control out
// Make the robot move
driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(xInput, yInput,
headingX.getAsDouble(),
headingY.getAsDouble(),
swerveDrive.getYaw().getRadians(),
swerveDrive.getMaximumVelocity()));
});
}
The driveFieldOriented function
public void driveFieldOriented(ChassisSpeeds velocity) {
swerveDrive // The YAGSL SwerveDrive object
.driveFieldOriented(velocity);
}
I’m thoroughly confused as to why this is happening. As far as we can tell, our configuration is fine, the NavX IMU is reading properly, and the controls work fine, aside from this issue. We also tried changing the drive command, using one that used the right stick to change the rotation speed of the robot, rather than to set the rotation. The problem persisted in this mode.
If you think the configuration may be to blame, here’s a link to the files on GitHub.
I’d love any help I can get in resolving this problem. Thanks in advance!