Skip to content

Instantly share code, notes, and snippets.

@nilayp
Created November 6, 2024 20:17
Show Gist options
  • Save nilayp/1a00b8d6c620dac5ef95e58d3b3190a8 to your computer and use it in GitHub Desktop.
Save nilayp/1a00b8d6c620dac5ef95e58d3b3190a8 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 timers.
@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 a timed duration of 6.5 seconds
turnLeftTimed(0.5, 6500); // Power 0.5, duration 6500 ms (6.5 seconds)
// 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 a timed duration
public void turnLeftTimed(double power, int duration) {
// Set power for turning left: left motor moves backward, right motor moves forward
backLeftDrive.setPower(-power);
backRightDrive.setPower(power);
// Display motor power in telemetry while turning
telemetry.addData("Turning Left - Left Motor Power", backLeftDrive.getPower());
telemetry.addData("Turning Left - Right Motor Power", backRightDrive.getPower());
telemetry.update();
// Sleep for the specified duration (6.5 seconds in this case)
sleep(duration);
// Stop the motors after the turn
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.update();
}
// Method to drive forward until blue is detected using color percentages
public void driveUntilBlue(double power) {
backLeftDrive.setPower(power);
backRightDrive.setPower(power);
// Continue driving until blue is detected
while (opModeIsActive()) {
int red = color1.red();
int green = color1.green();
int blue = color1.blue();
// Calculate total color value
int total = red + green + blue;
// Avoid division by zero
if (total > 0) {
int redPercent = (red * 100) / total;
int greenPercent = (green * 100) / total;
int bluePercent = (blue * 100) / total;
// Check if blue is the dominant color with a threshold
if (bluePercent > redPercent && bluePercent > greenPercent && bluePercent > 50) { // Adjust threshold as needed
backLeftDrive.setPower(0);
backRightDrive.setPower(0);
telemetry.addData("Status", "Blue detected, stopping.");
telemetry.update();
break;
}
// Update telemetry with color percentages
telemetry.addData("Red %", redPercent);
telemetry.addData("Green %", greenPercent);
telemetry.addData("Blue %", bluePercent);
telemetry.update();
}
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment