Strange SequentialCommandGroup behavior

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.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.