Our Autobalance code will turn on, but not off

FIRST General Forum
1

Hey! In our teleop period i have some code for our Auto Balance thats supposed to run when I press Joystick Button 7, and the code does work once I press the button. The problem is is when I press the button again to turn off the code it continues to run, even if I disable the robot. The only way to go back to regular joystick-controlled robot is if i re-deploy the code. Any ideas why this is? Thanks.

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot;
//import frc.robot.commands.ToggleBalance;

import com.kauailabs.navx.frc.AHRS;
//import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
//import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj.I2C;
//import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj.Timer;

public class Robot extends TimedRobot {
  private static final int kJoystickChannel = 0;

  private static MecanumDrive m_robotDrive;
  private static Joystick m_stick;
  AHRS ahrs;
  boolean autoBalanceXMode;
  boolean autoBalanceYMode;
  boolean isBalanceEnabled = false;

  static final double kOffBalanceAngleThresholdDegrees = 10;
  static final double kOonBalanceAngleThresholdDegrees  = 5;

  //Joystick Vars
  //private JoystickButton m_toggleBalance;


  @Override
  public void robotInit() {
    PWMSparkMax frontLeft = new PWMSparkMax(Constants.kFrontLeftChannel);
    PWMSparkMax rearLeft = new PWMSparkMax(Constants.kRearLeftChannel);
    PWMSparkMax frontRight = new PWMSparkMax(Constants.kFrontRightChannel);
    PWMSparkMax rearRight = new PWMSparkMax(Constants.kRearRightChannel);

    // Invert the right side motors.
    // You may need to change or remove this to match your robot.
    frontRight.setInverted(true);
    rearRight.setInverted(true);

    m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);

    m_stick = new Joystick(kJoystickChannel);
    //m_toggleBalance = new JoystickButton(m_stick, Constants.toggleBalance);
    ahrs = new AHRS(I2C.Port.kMXP); /* Alternatives:  SPI.Port.kMXP, I2C.Port.kMXP or SerialPort.Port.kUSB */
  }

  @Override
  public void teleopPeriodic() {
    // Use the joystick X axis for lateral movement, Y axis for forward
    // movement, and Z axis for rotation.
    m_robotDrive.driveCartesian(-m_stick.getY(), m_stick.getX(), m_stick.getZ());
    //if doesnt work, add gyroAnlge arg = 1.0
    if(m_stick.getRawButtonPressed(7)) {
      isBalanceEnabled = true;
    } 
    if(m_stick.getRawButtonPressed(8)) {
      isBalanceEnabled = false;
    }

    if(isBalanceEnabled){
      while(isTeleop() && isEnabled() && isBalanceEnabled) {
        double xAxisRate            = m_stick.getX();
        double yAxisRate            = m_stick.getY();
        double pitchAngleDegrees    = ahrs.getPitch();
        double rollAngleDegrees     = ahrs.getRoll();
      
        if(m_stick.getRawButtonPressed(7)) {
          isBalanceEnabled = !isBalanceEnabled;
        } 
        
        if ( !autoBalanceXMode && 
            (Math.abs(pitchAngleDegrees) >= 
            Math.abs(kOffBalanceAngleThresholdDegrees))) {
            autoBalanceXMode = true;
        }
        else if ( autoBalanceXMode && 
                (Math.abs(pitchAngleDegrees) <= 
                Math.abs(kOonBalanceAngleThresholdDegrees))) {
            autoBalanceXMode = false;
        }
        if ( !autoBalanceYMode && 
            (Math.abs(pitchAngleDegrees) >= 
            Math.abs(kOffBalanceAngleThresholdDegrees))) {
            autoBalanceYMode = true;
        }
        else if ( autoBalanceYMode && 
                (Math.abs(pitchAngleDegrees) <= 
                Math.abs(kOonBalanceAngleThresholdDegrees))) {
            autoBalanceYMode = false;
        }
        
        // Control drive system automatically, 
        // driving in reverse direction of pitch/roll angle,
        // with a magnitude based upon the angle
        
        if (autoBalanceXMode) {
            double pitchAngleRadians = pitchAngleDegrees * (Math.PI / 180.0);
            xAxisRate = Math.sin(pitchAngleRadians) * -1;
            System.out.println(pitchAngleDegrees);
        }
        if (autoBalanceYMode) {
            double rollAngleRadians = rollAngleDegrees * (Math.PI / 180.0);
            yAxisRate = Math.sin(rollAngleRadians) * -1;
            System.out.println(rollAngleDegrees);
        }
        m_robotDrive.driveCartesian(xAxisRate, yAxisRate, m_stick.getTwist());
        Timer.delay(0.005);		// wait for a motor update time
      }
    }
  }
}
1 Like
2

The problem is that you’re using a blocking while loop. You should pretty much never use them in your robot code. You can try putting those conditions in your if statement as teleopPeriodic() gets called 50 times a second.

3

What is the button? Your code has 7 to start and 8 to stop. Your documentation implies 7 to start and 7 to stop. Which did you mean and what is the action that is or isn’t working?
edit and the above response is right on - no loops like that in periodic functions; it’s no longer periodic but nearly CPU “locked” even with a little wait in it.

4

Which if statement are you referencing?

5

I would try moving all the conditionals in this while loop into the if statement above it.

6

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