Robot continues in auto even after disabling and enabling teleop

[Gandagorn]

So when I run auto, disable it in the middle of it’s routine, and enable teleop after, the robot just continues to move forward like auto was never disabled. does anyone have any suggestions? Here is my code:

package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;

import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class DriveTrain extends SubsystemBase {

    CANSparkMax leftLeader;
    CANSparkMax rightLeader;
    
    CANSparkMax leftFollower;
    CANSparkMax rightFollower;

    RelativeEncoder leftEncoder;
    RelativeEncoder rightEncoder;

    SparkMaxPIDController leftController;
    SparkMaxPIDController rightController;

    DifferentialDrive drive;

    ADXRS450_Gyro gyro;
    
    double setpoint;
    double angle;
    double leftEncoderPosition;
    double rightEncoderPosition;
    double desiredAngle;

    public DriveTrain() {
        leftLeader = new CANSparkMax(1, MotorType.kBrushless);
        rightLeader = new CANSparkMax(3, MotorType.kBrushless);

        leftFollower = new CANSparkMax(2, MotorType.kBrushless);
        rightFollower = new CANSparkMax(4, MotorType.kBrushless);

        leftEncoder = leftLeader.getEncoder();
        rightEncoder = rightLeader.getEncoder();

        leftController = leftLeader.getPIDController();
        rightController = rightLeader.getPIDController();

        drive = new DifferentialDrive(leftLeader, rightLeader);

        gyro = new ADXRS450_Gyro();
        gyro.calibrate();

        leftFollower.follow(leftLeader);
        rightFollower.follow(rightLeader);
        leftController.setP(Constants.kP);
        leftController.setI(Constants.kI);
        leftController.setD(Constants.kD);
        rightController.setP(Constants.kP);
        rightController.setI(Constants.kI);
        rightController.setD(Constants.kD);

        leftController.setOutputRange(-0.25, 0.25);
        rightController.setOutputRange(-0.25, 0.25);

        drive.setSafetyEnabled(false);
    }

    public void resetEncoders() {
        leftEncoder.setPosition(0);
        rightEncoder.setPosition(0);
    }

    public void drive(double f, double t) {
        drive.arcadeDrive(f, t);
        SmartDashboard.putNumber("leftEncoder", leftEncoder.getPosition());
        SmartDashboard.putNumber("rightEncoder", rightEncoder.getPosition());
        SmartDashboard.putNumber("angle", gyro.getAngle());
    }

    public void setSetpoint(double s) {
        setpoint = s * (Constants.GEAR_BOX_RATIO / Constants.WHEEL_CIRCUMFERENCE) * Constants.ELLIOT_COEFFICIENT;  // MAKE SURE TO CONVERT FROM METERS TO ROTATIONS BEFORE MULTIPLYING BY THIS COEFFICIENT 1 ROTATION = THE COEFFICIENT
    }

    public void drivePID() {
        leftController.setReference(setpoint, CANSparkMax.ControlType.kPosition);
        rightController.setReference(-setpoint, CANSparkMax.ControlType.kPosition);

        SmartDashboard.putNumber("leftEncoder", Math.abs(leftEncoder.getPosition()));
        SmartDashboard.putNumber("rightEncoder", Math.abs(rightEncoder.getPosition()));
        SmartDashboard.putNumber("angle", Math.abs(getAngle() % 360));
    }

    public double getAngle() {
        return Math.abs(gyro.getAngle() % 360);
    }

    public boolean angleCorrect() {
        if (getAngle() < desiredAngle) {
            while (getAngle() < desiredAngle) {
                leftLeader.set(0.25);
                rightLeader.set(-0.25);
                angle = gyro.getAngle();
                SmartDashboard.putNumber("angle", gyro.getAngle());
            }
        }
        
        if (getAngle() > desiredAngle) {
            while (getAngle() > desiredAngle) {
                leftLeader.set(-0.25);
                rightLeader.set(0.25);
                angle = gyro.getAngle();
                SmartDashboard.putNumber("angle", gyro.getAngle());
            }
        }
        return true;
    }

    public void resetGyro() {
        gyro.reset();
    }

    public boolean leftAtSetpoint() {
        if ((leftEncoderPosition == Math.abs(setpoint + 2)) || (leftEncoderPosition == Math.abs(setpoint - 2))) {
            return true;
        }
        return false;
    }

    public boolean rightAtSetpoint() {
        if ((rightEncoderPosition == Math.abs(setpoint + 2)) || (rightEncoderPosition == Math.abs(setpoint - 2))) {
            return true;
        }
        return false;
    }

    public double getleftVelocity() {
        return leftEncoder.getVelocity();
    }

    public double getRightVelocity() {
        return rightEncoder.getVelocity();
    }

    public double getVelocity() {
        return (getleftVelocity() + getRightVelocity()) / 2;
    }

    public boolean atSetpoint() {
        if (setpoint <= leftEncoder.getPosition()) {
            System.out.print("yay");
            return true;
        }
        return false;
    }

    public void update() {
        leftEncoderPosition = Math.abs(leftEncoder.getPosition());
        rightEncoderPosition = Math.abs(rightEncoder.getPosition());
        angle = gyro.getAngle();
        SmartDashboard.updateValues();
    }

    public boolean atAngle() {
        if (getAngle() >= desiredAngle) {
            return true;
        }
        return false;
    }

    public void setAngle(double a) {
        desiredAngle = a;
    }
}

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.DriveTrain;

public class DriveDistance extends CommandBase {
    double setpoint;
    boolean finished;
    DriveTrain driveTrain;

    public DriveDistance(DriveTrain DT, double s) {
        driveTrain = DT;
        setpoint = s;

        addRequirements(driveTrain);
    }

    // Called when the command is initially scheduled.
    @Override
    public void initialize() {
        driveTrain.resetGyro();
        driveTrain.resetEncoders();
        driveTrain.setSetpoint(setpoint);
        driveTrain.drivePID();
    }

    // Called every time the scheduler runs while the command is scheduled.
    @Override
    public void execute() {
        if (driveTrain.atSetpoint()) {
            finished = true;
        } else {
            driveTrain.update();
        }
    }

    // Called once the command ends or is interrupted.
    @Override
    public void end(boolean interrupted) {
        driveTrain.drive(0, 0);
    }

    // Returns true when the command should end.
    @Override
    public boolean isFinished() {
        return finished;
    }
}

thanks!!

[Fletch1373]

If you’re using PID control onboard the motor controllers (CANSparkMax objects), they will not know anything about the enabled/disabled state of the robot beyond whether it’s receiving heartbeat data from the roborio. When in disabled mode (for the brief moment between auto and teleop), the roborio will command all controllers to be disabled, but once teleop starts, it’ll return to its previous configuration (aka following the PID controller outputs until the setpoint is met).

[NewtonCrosby]

Is your DriveDistance command ever finishing?

Does your drivetrain have a default command?

Check the scheduler widget in Shuffleboard while this is happening to see what commands are scheduled (if any) when this undesirable behavior happens.

[Gandagorn]

the drivedistance command wasnt finishing. fixed. thanks!

[NewtonCrosby]

For safety you can always clear the scheduling queue as you go into the disabled state (or conversely, teleopinit).

[2020]

Are you canceling your autonomous commands when teleop starts?

Like this for instance:

  @Override
  public void teleopInit() {
    // This makes sure that the autonomous stops running when
    // teleop starts running. If you want the autonomous to
    // continue until interrupted by another command, remove
    // this line or comment it out.
    if (m_autonomousCommand != null) {
      m_autonomousCommand.cancel();
    }
  }