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();
}
public double getAngle() {
return gyro.getAngle();
}
public void turnStop() {
turnPID.disable();
System.out.println("stop turning at " + RobotMap.roundtoTwo(gyro.getAngle()));
}
