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