Robot keeps running Autonomous code in Teleop

Technical Programming
1

Hey there, new programmer here who because of our school size is now the lead programmer. Even though I have this title I still have no idea what I am doing. I have got the teleop working and some (really bad) auto working on the test robot, but I have this problem with the autonomous code. When the robot is in Auto the drive is set and it does it’s thing, but when switched back to auto it keeps going and doesn’t ever stop.

Thanks!
Tom.

public void gyroRotate(double desired,double speed) throws AutoEndException{
	while(c.isAuto()){
		c.gyro.reset();
		if(desired > 1){
			while(c.gyro.getAngle() < desired){
				c.drive.drive(speed,  speed);
			}
			c.drive.drive(0, 0);
			
		}
		else if(desired < 1){
			while(c.gyro.getAngle() < desired){
				c.drive.drive(-speed,  -speed);
			}
			c.drive.drive(0, 0);
		}
	}
	c.drive.drive(0,0);
	c.killMotors();

	
	
}


package org.usfirst.frc.team5033.robot;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;



@SuppressWarnings("unused")
public class Robot extends IterativeRobot {
	Componants c = new Componants(() -> isEnabled() && isAutonomous());
	Controls controls = new Controls();
	



  // class that handles basic drive		


	public void robotInit() {
		c.chooser = new SendableChooser();
		c.chooser.addDefault("TEST_AUTO (DEFAULT)", define.AutonomousRoutines.TEST_AUTO);
		c.chooser.addObject("NUMBER 2", define.AutonomousRoutines.NUMBER_2);
		SmartDashboard.putData("AUTONOMOUS MODES", c.chooser);
		
		CameraServer.getInstance().startAutomaticCapture("pleaseffs", 0);
		
		//c.chooser = new SendableChooser();
		//c.chooser.addDefault("TEST_AUTO (DEFAULT)", define.AutonomousRoutines.TEST_AUTO);
		//c.chooser.addObject("NUMBER 2", define.AutonomousRoutines.NUMBER_2);
		//SmartDashboard.putData("AUTONOMOUS MODES", c.chooser);
		c.drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
	    c.drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

		}
	public void autonomousInit(){ //Initialize for autonomous
		SmartDashboard.putNumber("Gyro angle", c.gyro.getAngle());
		//c.resetAll();
		c.setSensors();
		c.killMotors();
		//c.chooser.addDefault("TEST_AUTO (DEFAULT)", define.AutonomousRoutines.TEST_AUTO);
		//c.chooser.addObject("NUMBER 2", define.AutonomousRoutines.NUMBER_2);
		//SmartDashboard.putData("AUTONOMOUS MODES", c.chooser);

		try {
			c.auto.run(c.auto.routines, c);
		} catch (AutoEndException se) {

		} catch (Exception e) {
			e.printStackTrace();
		}

	}
	public void autonomousPeriodic(){
		SmartDashboard.putNumber("Gyro angle", c.gyro.getAngle());
		SmartDashboard.putNumber("Distance amount", c.rightDriveEncoder.getDistance());
	}
	
	public void teleopInit(){
		c.drive.stopMotor();
		c.setSensors();
		c.rightDriveEncoder.reset();
		c.killMotors();
		
		//Initialize for teleop
		
	}
	

	public void teleopPeriodic() { 
		SmartDashboard.putNumber("Gyro angle", c.gyro.getAngle());
		SmartDashboard.putNumber("Distance amount", c.rightDriveEncoder.getDistance());

		try{
			if(controls.joystickTrigger())
			{
				c.drive.arcadeDrive(controls.joystickY(), -controls.joystickX());
			}
			else
			{
				c.drive.arcadeDrive(-controls.joystickY(), -controls.joystickX());				
			}
			if(controls.rope())
			{
				c.ropeDrive.set(1);
			}
			else
			{
				c.ropeDrive.set(0);
			}
			if(controls.gearForward()){
				c.gearDrive.set(0.3);
			}
			else if(controls.gearBackwards() != true){
				c.gearDrive.set(0);
			}
			if(controls.gearBackwards()){
				c.gearDrive.set(-0.3);
			}
			else if(controls.gearForward() != true){
				c.gearDrive.set(0);
			}
			// 
			// 
		
		}
			catch (Exception e){
				e.printStackTrace();
			}
			}
			
		}
2

Nothing makes you leave the nested while loops. In general, while loops in code that loops iteratively is very bad practice because you will not be able to run other code until the loop completes. While the gyro angle isn’t correct, it will never check the state of autonomous, for example.

Can you rewrite the code to use just if statements and maybe a few variables to track state changes between loops?

3

^^ What Chris said.

Also, do you really mean to reset the gyro each pass? Usually you just do this in the init methods or as a callback in response to a button press or when a sensor lets you know you’re aligned to the field.

4

Hey Guys! Thanks for the replies I will try what you suggested when I get back to school on Monday, hopefully I will have good results. (yeah and I didn’t mean to reset that gyro each time oops thanks Gee). Ill get back to you on what the test board does!

Thanks.

5

Another thing you can do is add in an argument for any loops you have (isEnabled()) this will kick the robot out of the loop in transition from auto to tele-op. Also, you can supply your own custom disabled() method that sets all motors to 0, breaks out of loops, ends threads, and resets any sensors.