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