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);
}
}
}
private double calcEncoderValue(double inches) {
return (1440 * (inches / (Math.PI * presets.getWHEEL_DIAMETER()))) * presets.getGEAR_RATIO();
} // calcEncoderValue