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