Last active
November 6, 2024 20:16
-
-
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.
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 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