We’re continuing to make progress with YAGSL on our robot, but we’re running into an issue with robot rotation when using the stock SwerveController in YAGSL. We’re using the following drive Command:
TestDriveCommand StandardYagslDrive = new TestDriveCommand(drivebase,
() -> MathUtil.applyDeadband(-driverXbox.getLeftY(),
OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(-driverXbox.getLeftX(),
OperatorConstants.LEFT_X_DEADBAND),
() -> -driverXbox.getRawAxis(4), () -> true);
When we enable the robot, everything looks good. When we apply ccw rotation, it starts to rotate properly, but if we remove the right_stick_x input, the robot continues to slowly rotate. Eventually, the IMU “hangs” at given heading, but the robot continues to rotate. If you take a look at these two videos, you can see the FRC web components view – the IMU stops at about 36s, but the robot continues to rotate. The second video is a video of the robot from my phone.
To troubleshoot this issue, we created a new, simple swerve controller that only tries to keep a consistent heading. With our simple Swerve Controller, the robot rotates properly and we never get our IMU out of alignment.
public class SuperSwerveController {
double headingSetpoint;
PIDController headingPID;
private double kSpinP = 0.0055;
private double kSpinI = 0.00000;
private double kSpinD = 0.00;
SwerveSubsystem drivebase;
double targetOmega;
public SuperSwerveController(SwerveSubsystem drivebase) {
headingPID = new PIDController(kSpinP, kSpinI, kSpinD);
headingPID.enableContinuousInput(-180, 180); //sets a circular range instead of a linear one.
headingPID.setTolerance(3);
this.drivebase = drivebase;
}
public void doTeleop(SwerveSubsystem swerve, double rX, double rY, double rOmega) {
if (Math.abs(rOmega) < 0.075) { //TODO: Change minimum Omega value to CONSTANT
// There is not enough rotational input -- Keep the previous heading value
//get heading
targetOmega = headingPID.calculate(drivebase.getHeading().getDegrees(), headingSetpoint);
} else {
// The driver is providing rotational input. Set Omega accordin to driver input
// and store current heading
targetOmega = rOmega;
headingSetpoint = drivebase.getHeading().getDegrees();
}
targetOmega = MathUtil.clamp(targetOmega, -1, 1);
swerve.drive(new Translation2d(rX, rY), targetOmega, true);
SmartDashboard.putNumber("SuperSwerve.targetOmega", targetOmega);
SmartDashboard.putNumber("SuperSwerve.rOmega", rOmega);
SmartDashboard.putNumber("SuperSwerve.headingSetpoint", headingSetpoint);
}
}
I’m looking for guidance on how to troubleshoot the SwerveController as we’d much rather stick with the stock YAGSL.
We though maybe our DriveConversionFactor was off (it was – just a little) but we fixed that today, and it seems like we can go longer before the problem starts, but we still have the same problem.
Thanks in advance for your help-