Created
November 6, 2024 20:17
-
-
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.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
@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