Skip to content

Instantly share code, notes, and snippets.

@nilayp
Last active November 6, 2024 20:16
Show Gist options
  • Save nilayp/a5b15b505327718fe3ff1a36d3256620 to your computer and use it in GitHub Desktop.
Save nilayp/a5b15b505327718fe3ff1a36d3256620 to your computer and use it in GitHub Desktop.
This auto mode should move forward, turn left and go until the robot touches the blue tape. The turn is handled via encoders.
@Autonomous(name = "My FIRST Java OpMode", group = "Autonomous")
public class MyFIRSTJavaOpMode extends LinearOpMode {
// Declare hardware variables for the drive motors
private DcMotor backLeftDrive;
private DcMotor backRightDrive;
// Declare color sensor
private ColorSensor color1;
@Override
public void runOpMode() {
// Initialize hardware for the motors
backLeftDrive = hardwareMap.get(DcMotor.class, "backLeftDrive");
backRightDrive = hardwareMap.get(DcMotor.class, "backRightDrive");
// Initialize the color sensor
color1 = hardwareMap.get(ColorSensor.class, "color1");
// Set motor directions with reversed configuration
backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
backRightDrive.setDirection(DcMotor.Direction.FORWARD);
// Set zero power behavior
backLeftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
backRightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// Reset and set encoder modes
backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Display initialization message
telemetry.addData("Status", "Initialized");
telemetry.update();
// Wait for the start button to be pressed
waitForStart();
// Autonomous actions start here
if (opModeIsActive()) {
// Drive forward for 1 second
driveForward(0.5, 1000);
// Turn left using encoder values
turnLeft(2700); // Adjusted to the 2700 value as requested
// Drive forward until blue is detected
driveUntilBlue(0.5);
}
}
// Method to drive forward
public void driveForward(double power, int duration) {
backLeftDrive.setPower(power);
backRightDrive.setPower(power);
// Display motor power and encoder values in telemetry
telemetry.addData("Left Motor Power", backLeftDrive.getPower());
telemetry.addData("Right Motor Power", backRightDrive.getPower());
telemetry.addData("Left Encoder", backLeftDrive.getCurrentPosition());
telemetry.addData("Right Encoder", backRightDrive.getCurrentPosition());
telemetry.update();
// Keep driving for the specified duration
sleep(duration);
// Stop the motors
backLeftDrive.setPower(0);
backRightDrive.setPower(0);
// Update telemetry after stopping
telemetry.addData("Left Motor Power", backLeftDrive.getPower());
telemetry.addData("Right Motor Power", backRightDrive.getPower());
telemetry.addData("Left Encoder", backLeftDrive.getCurrentPosition());
telemetry.addData("Right Encoder", backRightDrive.getCurrentPosition());
telemetry.update();
}
// Method to turn left using encoders
public void turnLeft(int targetPosition) {
// Reset encoders for the turn
backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// Set target position for the turn
backLeftDrive.setTargetPosition(-targetPosition);
backRightDrive.setTargetPosition(targetPosition);
// Set mode to RUN_TO_POSITION
backLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
backRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// Set power to the motors
backLeftDrive.setPower(0.5);
backRightDrive.setPower(0.5);
// Loop until both motors reach their target positions
while (opModeIsActive() && (backLeftDrive.isBusy() || backRightDrive.isBusy())) {
telemetry.addData("Left Encoder", backLeftDrive.getCurrentPosition());
telemetry.addData("Right Encoder", backRightDrive.getCurrentPosition());
telemetry.addData("Left Target", backLeftDrive.getTargetPosition());
telemetry.addData("Right Target", backRightDrive.getTargetPosition());
telemetry.addData("Left Motor Speed", backLeftDrive.getPower());
telemetry.addData("Right Motor Speed", backRightDrive.getPower());
telemetry.update();
}
// Stop the motors after reaching the target
backLeftDrive.setPower(0);
backRightDrive.setPower(0);
// Final telemetry update after motors stop
telemetry.addData("Left Motor Power", backLeftDrive.getPower());
telemetry.addData("Right Motor Power", backRightDrive.getPower());
telemetry.addData("Left Encoder", backLeftDrive.getCurrentPosition());
telemetry.addData("Right Encoder", backRightDrive.getCurrentPosition());
telemetry.update();
}
// Method to drive forward until blue is detected
public void driveUntilBlue(double power) {
backLeftDrive.setPower(power);
backRightDrive.setPower(power);
// Continue driving until blue is detected
while (opModeIsActive()) {
int blue = color1.blue();
int red = color1.red();
int green = color1.green();
// Check if blue is the dominant color
if (blue > red && blue > green && blue > 200) { // Threshold for blue detection
backLeftDrive.setPower(0);
backRightDrive.setPower(0);
telemetry.addData("Status", "Blue detected, stopping.");
telemetry.update();
break;
}
// Update telemetry with color sensor values
telemetry.addData("Red", red);
telemetry.addData("Green", green);
telemetry.addData("Blue", blue);
telemetry.update();
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment