private void initTurnPID() {
turnPID =
new SendablePIDController(
turnKp,
turnKi,
turnKd,
gyro,
new PIDOutput() {
public void pidWrite(double output) {
tankDrive(-output, output);
SmartDashboard.putDouble("left pwm", RobotMap.roundtoTwo(-output));
SmartDashboard.putDouble("right pwm", RobotMap.roundtoTwo(output));
SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(gyro.getAngle()));
SmartDashboard.putDouble("turn error", RobotMap.roundtoTwo(turnPID.getError()));
}
});
turnPID.setTolerance(2.0);
turnPID.setInputRange(-90, 90);
turnPID.setOutputRange(-.55, .55);
SmartDashboard.putData("turn PID", turnPID);
// affects turning, .5 is default set in RobotDrive class.
// drive.setSensitivity(.5);
}
public void autoDrive() {
double speed = 0;
if (distPID.isEnable()) {
speed = distPID.get();
}
double turn = 0;
if (turnPID.isEnable()) {
turn = turnPID.get();
}
if (speed > 0) {
drive.drive(speed, turn);
} else {
if (turn > 0) {
// turn right
tankDrive(turn, -turn);
} else if (turn < 0) {
tankDrive(-turn, turn);
}
}
SmartDashboard.putDouble("left pwm", leftJag.get());
SmartDashboard.putDouble("right pwm", rightJag.get());
SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
SmartDashboard.putDouble("turn error", turnPID.getError());
// System.out.println("driving " + speed + " " + turn);
}
public void turnStart(double degrees) {
turnPID.reset();
gyro.reset();
turnPID.setSetpoint(degrees);
turnPID.enable();
Timer.delay(.1);
System.out.println(
"start turning from " + RobotMap.roundtoTwo(gyro.getAngle()) + " to setpoint " + degrees);
}
public boolean isTurnDone() {
System.out.println(
"is turn done "
+ turnPID.onTarget()
+ " deg="
+ RobotMap.roundtoTwo(gyro.getAngle())
+ " out="
+ RobotMap.roundtoTwo(turnPID.get())
+ " err="
+ RobotMap.roundtoTwo(turnPID.getError()));
return turnPID.onTarget();
}
private SendablePIDController initDistancePID() {
SendablePIDController pid =
new SendablePIDController(
distKp,
distKi,
distKd,
ultrasonic,
new PIDOutput() {
public void pidWrite(double output) {
// tankDrive(output, -output);
}
});
pid.setTolerance(10.0);
pid.setInputRange(6, 254);
pid.setOutputRange(-.8, .8);
SmartDashboard.putData("drive PID", pid);
return pid;
}
public boolean isDistDone() {
return distPID.onTarget();
}
public void setDistanceSetpoint(double inches) {
distPID.setSetpoint(inches);
}
public void distStop() {
distPID.disable();
}
public void distStart(double inches) {
distPID.setSetpoint(inches);
distPID.enable();
}
public void setTurnSetpoint(double degrees) {
turnPID.setSetpoint(degrees);
}
public void turnStop() {
turnPID.disable();
System.out.println("stop turning at " + RobotMap.roundtoTwo(gyro.getAngle()));
}
public NetworkTable getTable() {
return controller.getTable();
}
/**
* Returns the setpoint.
*
* @return the setpoint
*/
protected double getSetpoint() {
return controller.getSetpoint();
}
/**
* Sets the setpoint to the given value. If {@link PIDCommand#setRange(double, double)
* setRange(...)} was called, then the given setpoint will be trimmed to fit within the range.
*
* @param setpoint the new setpoint
*/
protected void setSetpoint(double setpoint) {
controller.setSetpoint(setpoint);
}
void _end() {
controller.disable();
}
void _initialize() {
controller.enable();
}
