diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java index ef6a498..e08369c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoBlueColorSensor.java @@ -1,35 +1,22 @@ package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.ColorSensor; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; - @Autonomous public class AutoBlueColorSensor extends LinearOpMode { - SampleMecanumDrive mecanumDrive; - @Override public void runOpMode() throws InterruptedException { ColorSensor sensor = hardwareMap.get(ColorSensor.class, "color"); - mecanumDrive = new SampleMecanumDrive(hardwareMap); API api = new API(this); + MovementAPI movementAPI = new MovementAPI(api); waitForStart(); - Trajectory firstMove = api.makeTrajectories( - mecanumDrive, - new Pose2d(new Vector2d(-34, 60), Math.toRadians(270)), - new int[]{-34}, - new int[]{40} - ); - - mecanumDrive.followTrajectory(firstMove); + movementAPI.move(0, 0.7); + api.pause(0.5); + movementAPI.stop(); api.pause(5); int r = sensor.red(); @@ -38,41 +25,26 @@ public class AutoBlueColorSensor extends LinearOpMode { int largest = api.getLargest(r, g, b); - Trajectory endpos; if (largest == g) { // move to position 1 - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -12, -12}, - new int[]{60, 60, 12} - ); + movementAPI.move(90, 0.7); + api.pause(0.25); } else if (largest == r) { // move to position 2 - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -12, -12, -34}, - new int[]{60, 60, 12, 12} - ); + movementAPI.move(0, 0.7); + api.pause(0.25); } else if (largest == b) { // move to position 3 - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -70, -60}, - new int[]{60, 60, 30} - ); + movementAPI.move(-90, 0.7); + api.pause(0.25); } else { // Color sensor broke, move to backup position - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -70}, - new int[]{60, 60} - ); + movementAPI.move(-180, 0.7); + api.pause(0.5); + movementAPI.move(90, 0.7); + api.pause(0.7); } - mecanumDrive.followTrajectory(endpos); + movementAPI.stop(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java index 40f67f5..0dad3a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/createdcode/enhancedautos/AutoRedColorSensor.java @@ -1,36 +1,22 @@ package org.firstinspires.ftc.teamcode.createdcode.enhancedautos; -import com.acmerobotics.roadrunner.geometry.Pose2d; -import com.acmerobotics.roadrunner.geometry.Vector2d; -import com.acmerobotics.roadrunner.trajectory.Trajectory; -import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.ColorSensor; -import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive; - @Autonomous public class AutoRedColorSensor extends LinearOpMode { - SampleMecanumDrive mecanumDrive; - @Override public void runOpMode() throws InterruptedException { ColorSensor sensor = hardwareMap.get(ColorSensor.class, "color"); - mecanumDrive = new SampleMecanumDrive(hardwareMap); API api = new API(this); - + MovementAPI movementAPI = new MovementAPI(api); waitForStart(); - Trajectory firstMove = api.makeTrajectories( - mecanumDrive, - new Pose2d(new Vector2d(-34, -60), Math.toRadians(90)), - new int[]{-34}, - new int[]{-40} - ); - - mecanumDrive.followTrajectory(firstMove); + movementAPI.move(0, 0.7); + api.pause(0.5); + movementAPI.stop(); api.pause(5); int r = sensor.red(); @@ -39,41 +25,26 @@ public class AutoRedColorSensor extends LinearOpMode { int largest = api.getLargest(r, g, b); - Trajectory endpos; if (largest == g) { // move to position 1 - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -12, -12}, - new int[]{-60, -60, -12} - ); + movementAPI.move(-90, 0.7); + api.pause(0.25); } else if (largest == r) { // move to position 2 - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -12, -12, -34}, - new int[]{-60, -60, -12, -12} - ); + movementAPI.move(0, 0.7); + api.pause(0.25); } else if (largest == b) { // move to position 3 - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -70, -60}, - new int[]{-60, -60, -30} - ); + movementAPI.move(90, 0.7); + api.pause(0.25); } else { // Color sensor broke, move to backup position - endpos = api.makeTrajectories( - mecanumDrive, - firstMove.end(), - new int[]{-34, -70}, - new int[]{-60, -60} - ); + movementAPI.move(-180, 0.7); + api.pause(0.5); + movementAPI.move(-90, 0.7); + api.pause(0.7); } - mecanumDrive.followTrajectory(endpos); + movementAPI.stop(); } }