DIO Trigger in Autonomous Mode Run for X seconds

[enniska]

I am currently working on trying to use the NavX-MXP board to maintain straight driving in autonomous mode for a set time of 5 seconds and a speed output of 50% for example. So once the DIO is triggered I want the robot to Drive for 5 seconds and have the ability to maintain a straight line. I have been able to get the motors to adjust based on the Yaw of the board but now the problem is getting a “Delay” to work within the autonomous code. Any help is greatly appreciated.
I have posted code below:

package frc.robot;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.SPI;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import com.ctre.phoenix.motorcontrol.can.*;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Joystick;

public class Robot extends TimedRobot implements PIDOutput {
DifferentialDrive myRobot; // class that handles basic drive operations
AHRS ahrs;
PIDController turnController;
double rotateToAngleRate;
Timer m_timer;
DigitalInput input;

static final double kP = 0.05;
static final double kI = 0.00;
static final double kD = 0.00;

static final double kToleranceDegrees = 1.0;    
static final double kTargetAngleDegrees = 0.0;

  //Drive motors
  WPI_TalonSRX left1;
  WPI_TalonSRX right1;
  //Slave motors
  WPI_TalonSRX left2;
  WPI_TalonSRX right2;
  Joystick Stick;

public Robot() {
	Stick = new Joystick(0);
	input = new DigitalInput(0);
	m_timer = new Timer();
	 //Drive motors
	 left1 = new WPI_TalonSRX(1);
	 right1 = new WPI_TalonSRX(0);
	 //Slave motors
	 left2 = new WPI_TalonSRX(2);
	 right2 = new WPI_TalonSRX(3);

	myRobot = new DifferentialDrive (left1, right1);
	myRobot.setExpiration(0.1);
	
	left2.follow(left1);
	right2.follow(right1);

	left1.setSensorPhase(true);
	right1.setSensorPhase(false);
	boolean invertLeft = false;
	boolean invertRight = false;
	left1.setInverted(invertLeft);
	right1.setInverted(invertRight);
	
    try {
        ahrs = new AHRS(SPI.Port.kMXP); 
    } catch (RuntimeException ex ) {
        DriverStation.reportError("Error instantiating navX MXP:  " + ex.getMessage(), true);
    }
    turnController = new PIDController(kP, kI, kD, ahrs, this);
    turnController.setInputRange(-2.0f,  2.0f);
    turnController.setOutputRange(-0.6, 0.6);
    turnController.setAbsoluteTolerance(kToleranceDegrees);
    turnController.setContinuous(true);
    turnController.disable();    
}

public void teleopPeriodic() {
	double forward = Stick.getY();
	double turn = Stick.getZ();
	myRobot.arcadeDrive(forward, -turn);
  }

public void autonomousInit() {
  myRobot.setSafetyEnabled(false);
  myRobot.setExpiration(0.1);
  ahrs.zeroYaw();
  m_timer.reset();
  m_timer.start();
}

 
public void autonomousPeriodic() {
	System.out.println(!input.get()); //Prints out the value of DIO trigger (True/False)
	System.out.println(ahrs.getYaw());
	double speed = -0.5;
	if (!input.get() && isEnabled()) {
  
			//myRobot.tankDrive(speed, speed);
			if(!turnController.isEnabled()) {
				// Acquire current yaw angle, using this as the target angle.
				turnController.setSetpoint(ahrs.getYaw());
				rotateToAngleRate = 0; // This value will be updated in the pidWrite() method.
				turnController.enable();
			}

			//double magnitude = (speed + speed) / 2;
			double leftStickValueY = speed + rotateToAngleRate;
			double leftStickValueX = speed - rotateToAngleRate;
			myRobot.tankDrive(leftStickValueY,  leftStickValueX);
			//Timer.delay(15); // Drive for 2 seconds (This part causes watchdog error when enabled)
			//myRobot.stopMotor();
	  }
	  
	  else {
			/* If the turn controller had been enabled, disable it now. */
			if(turnController.isEnabled()) {
				turnController.disable();
				myRobot.stopMotor(); // stop robot
			}
		}
	}	 

@Override
/* This function is invoked periodically by the PID Controller, */
/* based upon navX MXP yaw angle input and PID Coefficients.    */
public void pidWrite(double output) {
    rotateToAngleRate = output;
}

}

[jdao]

Under autonomousPeriodic(), you could do something like the following:

boolean startAuto = false;

public void autonomousPeriodic() {
    if (!input.get() && !startAuto) { 
        startAuto = true;
        rotateToAngleRate = 0;
    }
    if (startAuto) {
        if(m_timer.get() < 5) {
	    turnController.enable();
            double leftStickValueY = speed + rotateToAngleRate;
            double leftStickValueX = speed - rotateToAngleRate;
            myRobot.tankDrive(leftStickValueY,  leftStickValueX);
        }
        else {
            turnController.disable();
	    myRobot.stopMotor(); // stop robot
        } 
    }
}

[enniska]

Thank you for the quick response.

I inputted the code you wrote however the motors will not shut off after 5 seconds they just continuously run and now the motors will not adjust to the desired yaw tolerance they continuously run at 50%.

I kept the code in the Init the same. Would this potentially be a reason the timer isnt working?

[enniska]

I was able to get the code to work however I am looking to have the timer start once the DIO is triggered. Is there a way to do this? Using a wait(), delay(), or sleep() function?

Once the DIO is triggered (Not as soon as auto is enabled) then drive for 5 seconds after the DIO is triggered. Would changing the class from a TimedRobot to a different class help this issue get solved?

[jdao]

For your first question, I would check if your stopMotor() command is working properly. You can also change then end portion of the code to the following:

        else {
            turnController.disable();
            myRobot.tankDrive(0, 0);
        } 

Motor controllers will tell the motors to continue to move using the last set command they were given, so if you don’t tell them to stop, the will go on forever. Based on what you’re describing it sounds like after 5 seconds, we disable the PIDController but don’t tell the motors to stop, so they just continue with the last ocmmand they were given.

For your second question, if you want to initialize the timer when the DIO is pressed, just do this:

    if (!input.get() && !startAuto) { 
        startAuto = true;
        rotateToAngleRate = 0;
        m_timer.reset();
        m_timer.start();
    }