My team is having a problem with our PID turn to angle command. When we have more than one TurnToAngle command in a SequentialCommandGroup, only the first one runs and the rest end almost instantly. Because this website won’t allow me to add a file, here’s the TurnToAngle command:
package frc.robot.commands.auto;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj2.command.PIDCommand;
import frc.robot.Constants;
import frc.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
public class TurnToAngle extends PIDCommand {
private Drive drive;
private int count = 0;
/** Creates a new TurnToAngle. */
//positive angles turn clockwise, negative counterclockwise.
public TurnToAngle(Drive drive, double angle, double maxSpeed) {
super(
// The controller that the command will use
new PIDController(Constants.ANGLE_P, 0, Constants.ANGLE_D),
// This should return the measurement
drive::getHeading,
// This should return the setpoint (can also be a constant)
() -> angle,
// This uses the output
output -> {
// Use the output here
double power;
if (output > maxSpeed) {
power = maxSpeed;
} else if (output < -1 * maxSpeed) {
power = -1 * maxSpeed;
} else {
power = output;
}
drive.setRight(-1 * power);
drive.setLeft(power);
});
this.drive = drive;
addRequirements(this.drive);
// Use addRequirements() here to declare subsystem dependencies.
// Configure additional PID options by calling `getController` here.
getController().enableContinuousInput(-180, 180);
getController().setTolerance(Constants.ANGLE_TOLERANCE);
}
@Override
public void initialize() {
drive.resetAngle();
getController().reset();
System.out.println("Turn to angle started");
}
@Override
public void end(boolean interrupted) {
super.end(interrupted);
System.out.println("Turn to angle ended");
if (count >= Constants.PID_AT_TARGET_LOOPS) {
System.out.println("Angle reached");
} else {
System.out.println("Turn to angle timed out");
}
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (getController().atSetpoint()) {
count++;
} else {
count = 0;
}
System.out.println(count);
return count >= Constants.PID_AT_TARGET_LOOPS;
}
}
(PID_AT_TARGET_LOOPS is 20)
I added some print statements in an attempt to debug and got some weird results. The setup we had was with the robot on a cart, so the turn commands timed out rather than reaching their position. Based on my print statements, the first turn command went completely as expected, with the following output:
Turn to angle started
0
0
0
[a lot of zeroes]
0
0
0
Turn to angle ended
Turn to angle timed out
Then, this was the output for the second TurnToAngle:
Turn to angle started
0
0
Turn to angle ended
Turn to angle timed out
Over multiple trials it always outputted 0 exactly two times, which is very confusing.
We are in the middle of a regional, so a quick response would be excellent. Thanks!
UPDATE: I fixed the bug; the problem was in another file and the print statements make perfect sense now.