Drive Straight in Autonomous Help

Hello all,

I am a newbie to Java coding however this code should be fairly simple but I am struggling with it. I am currently working on using the Navx-MXP to maintain straight line driving. I have a separate code that once the DIO trigger is enabled then the robot will drive at half speed (0.5) for 5 seconds and then stop in autonomous mode and not using a joystick. This is all that I need to robot to do but I also need to incorporate and maintain a straight line once this plane is broken.

I have used the reference code from Navx website for the rotate to angle with a joystick which I have used to make sure the board itself is working and it is. I keep getting the Error of.

  • DifferentialDrive… Output not updated often enough.
  • Error at edu.wpi.first.wpilibj.MotorSafety.check(MotorSafety.java:101): DifferentialDrive… Output not updated often enough.

I have looked at other posts that say to disable motor safety features and the error will go away however it hasnt in this case.

Any help would be greatly appreciated.

public class Robot extends SampleRobot implements PIDOutput {
DifferentialDrive myRobot; // class that handles basic drive operations
Joystick leftStick; // set to ID in DriverStation
AHRS ahrs;
PIDController turnController;
double rotateToAngleRate;
Timer m_timer;
DigitalInput input;

static final double kP = 0.03;
static final double kI = 0.00;
static final double kD = 0.00;
static final double kF = 0.00;

static final double kToleranceDegrees = 2.0f;    
static final double kTargetAngleDegrees = 0.0f;

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

public Robot() {
 leftStick = new Joystick(0);
	 //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(10);
	
	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, kF, ahrs, this);
    turnController.setInputRange(-180.0f,  180.0f);
    turnController.setOutputRange(-1.0, 1.0);
    turnController.setAbsoluteTolerance(kToleranceDegrees);
    turnController.setContinuous(true);
    turnController.disable();    
}


public void autonomousInit() {

  myRobot.setSafetyEnabled(false);
  myRobot.setExpiration(10);

  m_timer.reset();
  m_timer.start();
}

 
public void autonomousPeriodic() {
  System.out.println(!input.get()); //Prints out the value of DIO trigger (True/False)
  myRobot.setSafetyEnabled(true);
  double speed = -0.5;
  ahrs.zeroYaw();
  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 = magnitude + rotateToAngleRate;
		  double leftStickValueX = magnitude - rotateToAngleRate;
		  myRobot.tankDrive(leftStickValueY,  leftStickValueX);
	}
	
	else {
		  /* If the turn controller had been enabled, disable it now. */
		  if(turnController.isEnabled()) {
			  turnController.disable();
		  }
		  myRobot.stopMotor(); // stop robot
	  }
	  Timer.delay(0.005);		// wait for a motor update time
  }

 

@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;
}

}

I see three issues.

  1. You have SampleRobot as your base class but wrote your code in autonomous methods that are not part of that robot model (they will never be called). I think you want TimedRobot as your base class. There will be other methods to create when you make that change.
  2. You are zeroing the yaw in each call to autonomousPeriodic. You probably meant to put that in autonomousInit.
  3. You are calling Timer.delay in autonomousPeriodic. Never do this. All periodic methods need to return “quickly” and must never block.

Hope this helps,
Steve

Thank you for the insight I greatly appreciate it!!

I have a few more questions following:

  1. You mentioned to not call the Timer.delay within the autonomousPeriodic(). Would I be able to just make it purely autonomous() and then the timer would work?

The main goal is to just have a laser to trigger the robot to move once laser is broken. Once in motion to keep the robot on a straight line while moving for 5 seconds for example.

After making the changes you mentioned I am now getting the error below:

  • Unhandled exception: java.lang.NullPointerException

  • Error at frc.robot.Robot.autonomousInit(Robot.java:78): Unhandled exception:
    The startCompetition() method (or methods called by it) should have handled the exception above

  • Robots should not quit, but yours did!

  • Warning at edu.wpi.first.wpilibj.RobotBase.startRobot(RobotBase.java:274): Robots should not quit, but yours did!

  • The startCompetition() method (or methods called by it) should have handled the exception above.

Any information to resolve this is appreciated.

The NullPointerException is the root of your issue. Take a look at the stack trace that came along with the NullPointerException. It will point to the failing line. It appears to me that you declared and used the DigitalInput input variable but never actually created the object. That would lead to the NullPointerException.

The TimedRobot base class defines a set of methods that it will call at particular points in time. The autonomousPeriodic method is the one that is called every 50ms during autonomous. You cannot simply rename it.

Get rid of the timer delay and instead, in autonomousInit (only called once at start of autonomous period), set the a variable to the current system time in milliseconds. Then, during autonomousPeriodic, get the current time again and compare it to the saved autonomousInit time. Only run the motors if less than 5000ms have elapsed and shut them off otherwise. There are other ways to do this too. After you get this working, You may want to read up on the command based robot model.

Steve

1 Like

Thank you for the tips they worked and the code is working!!! With a few exceptions that the Navx board is not adjusting the wheel speeds to maintain a straight line. I used the NavX code on their website for reference. The board itself is reading yaw values before and after one the robot stops moving. I am looking to stay at 50% output from the motors themselves and then adjust accordingly to the tolerance of 2 degrees from the yaw values.

Any tips?

I am also getting the following error now:

  • Watchdog not fed within 0.020000s

  • Loop time of 0.02s overrun

Did you remove this line from your autonomousPeriodic method?

If that is still there, you are see only zero or near zero values.

Having that call in autonomousInit, however, would be a good idea.

Steve

Here is my updated code. I have been trying to update the timer portion of this however the code I have does not stop the motors. The section commented out below was adjusting the motors based on yaw but I was not able to get the Timer/delay function to work with the code.

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() {
	boolean startAuto = false;
	double speed = -0.5;
	System.out.println(!input.get()); //Prints out the value of DIO trigger (True/False)
	System.out.println(ahrs.getYaw());
	
		
		if (!input.get() && !startAuto) { 
			startAuto = true;
			rotateToAngleRate = 0;
		}
		if (startAuto) {
			if(m_timer.get() < 5) {
			turnController.setSetpoint(ahrs.getYaw());
			rotateToAngleRate = 0; // This value will be updated in the pidWrite() method.
			turnController.enable();
				double leftStickValueY = speed + rotateToAngleRate;
				double leftStickValueX = speed - rotateToAngleRate;
				myRobot.tankDrive(leftStickValueY,  leftStickValueX);
			}
			else {
			turnController.disable();
			myRobot.stopMotor(); // stop robot
			} 
		}
		
			**/*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**
** myRobot.stopMotor();**
** }**


** else {**


** 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;
}

}

Pro Tip - Chief delphi allows you to use three back-tick marks (```) to mark blocks of code.

This is a code block

Example:

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;

    // ...

Doing so is highly recommended for here to make code easier to read, which in turn gets answers faster :smiley: .

More details here.

It does not look like you have tried my suggestion from an earlier post yet…

I would suggest giving it a try. Timer delays will get you nowhere.

Steve

As far as I can tell…

  • turnController.setInputRange() is for defining the minimum/maximum units expected. Unless I’m missing something, this would make your feedback range 4 degrees.

  • turnController.setSetpoint() is for defining the target value that the PID loop is trying to achieve. In this case, driving straight, you would want your setpoint to be forward(0 degrees). Right now, you’re just turning to whatever your angle already is.

  • Your pidWrite() is changing rotateToAngleRate, but you set it to 0 in autonomousPeriodic, effectively resetting it to 0 always.

Correct me if I’m wrong

1 Like