From a2055425a2857963db480303e6f700041746cf1d Mon Sep 17 00:00:00 2001 From: Nishk04 Date: Sun, 5 Nov 2023 20:59:43 -0800 Subject: [PATCH] Dumbest autonomous strafing to the right. --- .../teamcode/DirtyStrafeToParkingZone.java | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DirtyStrafeToParkingZone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DirtyStrafeToParkingZone.java index 8d5e395a0b2c..ae918de5e57a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DirtyStrafeToParkingZone.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DirtyStrafeToParkingZone.java @@ -3,7 +3,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import org.firstinspires.ftc.teamcode.configurations.RobotConfiguration; import org.firstinspires.ftc.teamcode.utility.MotorSet; @Autonomous @@ -16,8 +18,39 @@ public double OdoToInches (double ticks){ } @Override public void runOpMode() { + RobotConfiguration robot = RobotConfiguration.currentConfiguration().invoke(hardwareMap); MotorSet driveMotors = robot.driveMotors(); + DcMotor intake = hardwareMap.get(DcMotor.class, "intake"); + driveMotors.frontLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + driveMotors.frontLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + driveMotors.backRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + driveMotors.backRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + waitForStart(); + telemetry.addData("Distance Driven Forward:", OdoToInches((driveMotors.backRight.getCurrentPosition() + driveMotors.frontLeft.getCurrentPosition())/2.0)); + + telemetry.update(); + double distance = OdoToInches((driveMotors.backRight.getCurrentPosition() + driveMotors.frontLeft.getCurrentPosition())/2.0); + double strafeDistance = OdoToInches(intake.getCurrentPosition()); + while (distance < 30) { + distance = OdoToInches((driveMotors.backRight.getCurrentPosition() + driveMotors.frontLeft.getCurrentPosition())/2.0); + if (strafeDistance <= 2) { + strafeDistance = OdoToInches(intake.getCurrentPosition()); + driveMotors.frontLeft.setPower(0.4); + driveMotors.backLeft.setPower(-0.4); + driveMotors.frontRight.setPower(-0.4); + driveMotors.backRight.setPower(0.4); + } else { + driveMotors.frontLeft.setPower(0.4); + driveMotors.backLeft.setPower(0.4); + driveMotors.frontRight.setPower(0.4); + driveMotors.backRight.setPower(0.4); + telemetry.addData("Distance Driven Forward: ", distance); + telemetry.update(); + } + } } }