public void ultrasonicDriveWithCorrection(double speed, double ThreshholdDistance) {
if (stateManager.getStateStage() == 0) {
motorManager.refreshEncoders();
if (speed > 0) {
motorManager.setLeftPower(speed);
motorManager.setRightPower(speed);
} else if (speed < 0) {
motorManager.setLeftPower(-speed);
motorManager.setRightPower(-speed);
}
stateManager.continueCommand();
} else if (stateManager.getStateStage() == 1) {
if (ThreshholdDistance >= sensorManager.getUltrasonicSensorHolder().getLevel()
&& sensorManager.getUltrasonicSensorHolder().getLevel() != 0) {
motorManager.stopAllMotors();
stateManager.continueProgram();
} else {
gyroCorrection.GyroCorrection(speed);
}
}
}
