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);
}
}
}
public void driveWithCorrection(double speed, int ThreshholdDistance) {
if (stateManager.getStateStage() == 0) {
if (sensorManager.getColorSensorHolder().getColorSensor().blue()
> sensorManager.getColorSensorHolder().getColorSensor().red()) {
servoManager.getBeaconServo().setPosition(0); // Update this value
} else {
servoManager.getBeaconServo().setPosition(0); // Update this value
}
stateManager.continueCommand();
} else if (stateManager.getStateStage() == 1) {
motorManager.getRightMotor2().setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorManager.getLeftMotor2().setMode(DcMotorController.RunMode.RESET_ENCODERS);
motorManager.getLeftMotor2().setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
motorManager.getRightMotor2().setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
if (speed > 0) {
motorManager.getLeftMotor1().setPower(speed);
motorManager.getRightMotor1().setPower(-speed);
motorManager.getLeftMotor2().setPower(speed);
motorManager.getRightMotor2().setPower(-speed);
} else if (speed < 0) {
motorManager.getLeftMotor1().setPower(-speed);
motorManager.getRightMotor1().setPower(speed);
motorManager.getLeftMotor2().setPower(-speed);
motorManager.getRightMotor2().setPower(speed);
}
stateManager.continueCommand();
} else if (stateManager.getStateStage() == 2) {
if (ThreshholdDistance >= sensorManager.getUltrasonicSensorHolder().getLevel()
&& sensorManager.getUltrasonicSensorHolder().getLevel() != 0) {
motorManager.getLeftMotor1().setPower(0);
motorManager.getRightMotor1().setPower(0);
motorManager.getLeftMotor2().setPower(0);
motorManager.getRightMotor2().setPower(0);
stateManager.continueProgram();
} else {
leftCorrecion =
speed
- (sensorManager.getGyroscopeHolder().getRotation()
* presets.getGYRO_ERROR_CORRECTION());
rightCorrection =
speed
+ (sensorManager.getGyroscopeHolder().getRotation()
* presets.getGYRO_ERROR_CORRECTION());
if (leftCorrecion > 1) {
leftCorrecion = 1;
} else if (leftCorrecion < -1) {
leftCorrecion = -1;
}
if (rightCorrection > 1) {
rightCorrection = 1;
} else if (rightCorrection < -1) {
rightCorrection = -1;
}
motorManager.getLeftMotor1().setPower(leftCorrecion);
motorManager.getRightMotor1().setPower(rightCorrection);
motorManager.getLeftMotor2().setPower(leftCorrecion);
motorManager.getRightMotor2().setPower(rightCorrection);
}
}
}
