diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java index 0f63873..ec797d9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV1.java @@ -8,10 +8,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; import org.firstinspires.ftc.teamcode.subsystems.HWC; -import org.firstinspires.ftc.teamcode.subsystems.roadrunner.trajectorysequence.TrajectorySequence; @Autonomous(name = "AutonomousV1") public class AutonomousV1 extends OpMode { @@ -90,7 +88,7 @@ public void init_loop() { // ------ Set Trajectories based on Alliance Color ------ // - switch(allianceColor) { + switch (allianceColor) { case RED: // ------ Set Robot Start Pose ------ // robot.drive.setPoseEstimate(START_POSE_RED); @@ -108,7 +106,7 @@ public void init_loop() { // Drive to Left Line toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_RED) - .splineToLinearHeading(new Pose2d(8, -39, Math.toRadians(135)), Math.toRadians(135)) + .splineToLinearHeading(new Pose2d(7, -39, Math.toRadians(135)), Math.toRadians(135)) .build(); // Drive to Backboard from Center @@ -159,7 +157,7 @@ public void init_loop() { // Drive to Left Line toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE) - .splineToLinearHeading(new Pose2d(8, 39, Math.toRadians(225)), Math.toRadians(225)) + .splineToLinearHeading(new Pose2d(7, 39, Math.toRadians(225)), Math.toRadians(225)) .build(); // Drive to Backboard from Center @@ -169,7 +167,7 @@ public void init_loop() { // Drive to Backboard from Right toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) - .lineToLinearHeading(new Pose2d(49, 42, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(49, 41, Math.toRadians(180))) .build(); // Drive to Backboard from Left @@ -250,9 +248,9 @@ public void loop() { } // ------ State Methods ------ // - private void drivingToDepositPurplePixel() { + private void drivingToDepositPurplePixel() { // ------ Select Trajectory ------ // - if(firstRun) { + if (firstRun) { firstRun = false; if (elementLocation == HWC.Location.CENTER) { activeTrajectory = "toDepositCenter"; @@ -271,7 +269,7 @@ private void drivingToDepositPurplePixel() { state = State.DEPOSITING_PURPLE_PIXEL; firstRun = true; } - } + } // Deposit Purple Pixel private void depositingPurplePixel() { @@ -287,7 +285,7 @@ private void depositingPurplePixel() { // Drive to Backboard private void drivingToBackboard() { // ------ Select Trajectory ------ // - if(firstRun) { + if (firstRun) { firstRun = false; if (elementLocation == HWC.Location.CENTER) { activeTrajectory = "toBackboardFromInitial"; @@ -332,7 +330,7 @@ private void depositingYellowPixel() { // Drive to Park private void drivingToPark() { // ------ Select Trajectory ------ // - if(firstRun) { + if (firstRun) { firstRun = false; activeTrajectory = "toParkFromBackboard2"; if (elementLocation == HWC.Location.CENTER) { @@ -359,8 +357,8 @@ private void deliver() { robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos); robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos); robot.wrist.setPosition(HWC.wristDeliveryPos); - robot.pulleyLComponent.setTarget(HWC.slidePositions[1] / 4.0); - robot.pulleyRComponent.setTarget(HWC.slidePositions[1] / 4.0); + robot.pulleyLComponent.setTarget(-170); + robot.pulleyRComponent.setTarget(-170); } // Method to move to Intake Position @@ -371,21 +369,4 @@ private void intake() { robot.pulleyLComponent.setTarget(HWC.slidePositions[0]); robot.pulleyRComponent.setTarget(HWC.slidePositions[0]); } - - // Method to Check Claws & Close - private void checkClaws() { - // Close Claws when Pixel Detected - if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 2) { - robot.clawL.setPosition(0.5); - } - - if (robot.colorRight.getDistance(DistanceUnit.CM) <= 2) { - robot.clawR.setPosition(0.5); - } - - // If both Pixels are Detected, Stop Intake - if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) { - robot.intakeMotor.setPower(0); - } - } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java index 213f6b0..847d3a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/AutonomousV2.java @@ -7,6 +7,7 @@ import com.acmerobotics.roadrunner.trajectory.Trajectory; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; @@ -39,10 +40,11 @@ private enum State { private Trajectory toPixelStackFromRight; private Trajectory toPixelStackFromLeft; private Trajectory knockingPixelStack; - private Trajectory intakingPixels; - private Trajectory intakingPixelsSweep; + private Trajectory intakingPixels1; + private Trajectory intakingPixels2; + private Trajectory intakingPixels3; private TrajectorySequence toBackboardFromPixelStack; - private Trajectory toParkFromBackboard2; + private Trajectory toPark; // ------ Starting Positions ------ // private final Pose2d START_POSE_RED = new Pose2d(12, -60, Math.toRadians(90.00)); @@ -68,7 +70,10 @@ public void init() { robot.clawR.setPosition(0); // ------ Close Claw for Yellow Pixel ------ // - robot.toggleClaw('L'); + robot.clawL.setPosition(0.5); + + // ------ Reset Slide Encoders ------ // + resetSlideEncoders(); // ------ Telemetry ------ // telemetry.addData("Status", "Initialized"); @@ -85,17 +90,25 @@ public void init_loop() { robot.currentGamepad1.copy(gamepad1); robot.currentGamepad2.copy(gamepad2); + telemetry.addLine("Gamepad"); + telemetry.update(); + // ------ Save Element Position ------ // elementLocation = robot.detectElement(allianceColor); + telemetry.addLine("Element detected"); + telemetry.update(); + // ------ Alliance Color Selection ------ // if (robot.currentGamepad1.a && !robot.previousGamepad1.a) { allianceColor = allianceColor.equals(AllianceColor.RED) ? AllianceColor.BLUE : AllianceColor.RED; } + telemetry.addLine("Alliance Color selected"); + telemetry.update(); // ------ Set Trajectories based on Alliance Color ------ // - switch(allianceColor) { + switch (allianceColor) { case RED: // ------ Set Robot Start Pose ------ // robot.drive.setPoseEstimate(START_POSE_RED); @@ -103,27 +116,27 @@ public void init_loop() { // ------ Declare Trajectories ------ // // Drive to Center Line toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_RED) - .lineTo(new Vector2d(12.0, -34)) + .lineTo(new Vector2d(12.0, -32)) .build(); // Drive to Right Line toDepositRight = robot.drive.trajectoryBuilder(START_POSE_RED) - .strafeTo(new Vector2d(22, -43)) + .lineToLinearHeading(new Pose2d(22, -43, Math.toRadians(45))) .build(); // Drive to Left Line toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_RED) - .splineToLinearHeading(new Pose2d(7, -39, Math.toRadians(135)), Math.toRadians(135)) + .splineToLinearHeading(new Pose2d(7, -37, Math.toRadians(135)), Math.toRadians(135)) .build(); // Drive to Backboard from Center toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) - .lineToLinearHeading(new Pose2d(49, -35, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(49, -34, Math.toRadians(180))) .build(); // Drive to Backboard from Right toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) - .lineToLinearHeading(new Pose2d(49, -37, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(49, -40, Math.toRadians(180))) .build(); // Drive to Backboard from Left @@ -131,49 +144,56 @@ public void init_loop() { .lineToLinearHeading(new Pose2d(49, -30, Math.toRadians(180))) .build(); - // Drive to Pixel Stack from Center Backboard + // Drive to Pixel Stack from Center toPixelStackFromCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) - .splineTo(new Vector2d(7, -9), Math.toRadians(180)) - .splineTo(new Vector2d(-60, -24), Math.toRadians(180)) + .splineTo(new Vector2d(24, -48), Math.toRadians(180 + 40)) + .splineTo(new Vector2d(-12, -60), Math.toRadians(180)) + .splineTo(new Vector2d(-48, -48), Math.toRadians(180 - 40)) + .splineTo(new Vector2d(-60, -40), Math.toRadians(180)) .build(); - // Drive to Pixel Stack from Right Backboard + // Drive to Pixel Stack from Right toPixelStackFromRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) - .splineTo(new Vector2d(7, -9), Math.toRadians(180)) - .splineTo(new Vector2d(-60, -24), Math.toRadians(180)) + .splineTo(new Vector2d(24, -48), Math.toRadians(180 + 40)) + .splineTo(new Vector2d(-12, -60), Math.toRadians(180)) + .splineTo(new Vector2d(-48, -48), Math.toRadians(180 - 40)) + .splineTo(new Vector2d(-60, -40), Math.toRadians(180)) .build(); - // Drive to Pixel Stack from Left Backboard + // Drive to Pixel Stack from Left toPixelStackFromLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) - .splineTo(new Vector2d(7, -9), Math.toRadians(180)) - .splineTo(new Vector2d(-60, -24), Math.toRadians(180)) + .splineTo(new Vector2d(24, -48), Math.toRadians(180 + 40)) //new + .splineTo(new Vector2d(-12, -60), Math.toRadians(180)) + .splineTo(new Vector2d(-48, -48), Math.toRadians(180 - 40)) + .splineTo(new Vector2d(-60, -40), Math.toRadians(180)) .build(); // Knock Pixel Stack knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) - .splineToLinearHeading(new Pose2d(-60, -25, Math.toRadians(270)), Math.toRadians(270)) + .splineToLinearHeading(new Pose2d(-62, -34, Math.toRadians(180 - 40)), Math.toRadians(180 - 40)) .build(); - // Intake Pixels - intakingPixels = robot.drive.trajectoryBuilder(knockingPixelStack.end()) - .splineToLinearHeading(new Pose2d(-60, -24, Math.toRadians(180)), Math.toRadians(180)) - .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixelsSweep)) + // Intake Pixels (1) + intakingPixels1 = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .strafeTo(new Vector2d(-60, -36)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels2)) .build(); - // Intaking Pixels Sweep - intakingPixelsSweep = robot.drive.trajectoryBuilder(intakingPixels.end()) - .splineToLinearHeading(new Pose2d(-60, -25, Math.toRadians(270)), Math.toRadians(270)) + // Intake Pixels (2) + intakingPixels2 = robot.drive.trajectoryBuilder(intakingPixels1.end()) + .strafeLeft(2) .build(); - // Drive to Backboard 2 from Pixel Stack - toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(knockingPixelStack.end()) + // Drive to Backboard from Pixel Stack + toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) .setReversed(true) - .splineTo(new Vector2d(7, -9), Math.toRadians(0)) - .splineTo(new Vector2d(48, -35), Math.toRadians(0)) + .splineTo(new Vector2d(-12, -60), Math.toRadians(0)) + .splineTo(new Vector2d(24, -48), Math.toRadians(0 + 40)) + .splineTo(new Vector2d(46, -35), Math.toRadians(0)) .build(); - // Drive to Park from Backboard 2 - toParkFromBackboard2 = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) + // Drive to Park + toPark = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) .strafeTo(new Vector2d(48, -60)) .build(); @@ -189,80 +209,84 @@ public void init_loop() { .build(); // Drive to Right Line - toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_BLUE) - .strafeTo(new Vector2d(23, 45)) + toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .splineToLinearHeading(new Pose2d(7, 39, Math.toRadians(180 + 45)), Math.toRadians(180 + 45)) .build(); // Drive to Left Line - toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE) - .splineToLinearHeading(new Pose2d(7, -39, Math.toRadians(135)), Math.toRadians(135)) + toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .lineToLinearHeading(new Pose2d(22, 43, Math.toRadians(270 + 45))) .build(); // Drive to Backboard from Center toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) - .lineToLinearHeading(new Pose2d(49, 35, Math.toRadians(180))) + .lineToLinearHeading(new Pose2d(49, 34, Math.toRadians(180))) .build(); // Drive to Backboard from Right - toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) - .lineToLinearHeading(new Pose2d(49, 41, Math.toRadians(180))) - .build(); - - // Drive to Backboard from Left toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) .lineToLinearHeading(new Pose2d(49, 30, Math.toRadians(180))) .build(); // Drive to Backboard from Left - toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) - .lineToLinearHeading(new Pose2d(47, 30, Math.toRadians(180))) + toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(49, 40, Math.toRadians(180))) .build(); - // Drive to Pixel Stack from Center Backboard + // Drive to Pixel Stack from Center toPixelStackFromCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) - .splineTo(new Vector2d(7, 9), Math.toRadians(180)) - .splineTo(new Vector2d(-60, 24), Math.toRadians(180)) + .splineTo(new Vector2d(-12, 60), Math.toRadians(180)) + .splineTo(new Vector2d(-63, 40), Math.toRadians(180)) .build(); - // Drive to Pixel Stack from Right Backboard + // Drive to Pixel Stack from Right + // TODO: Will Likely Hit Purple Pixel, Adjust Trajectory after Testing toPixelStackFromRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) - .splineTo(new Vector2d(7, 9), Math.toRadians(180)) - .splineTo(new Vector2d(-60, 24), Math.toRadians(180)) + .splineTo(new Vector2d(-12, 60), Math.toRadians(180)) + .splineTo(new Vector2d(-63, 40), Math.toRadians(180)) .build(); - // Drive to Pixel Stack from Left Backboard + // Drive to Pixel Stack from Left toPixelStackFromLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) - .splineTo(new Vector2d(7, 9), Math.toRadians(180)) - .splineTo(new Vector2d(-60, 24), Math.toRadians(180)) + .splineTo(new Vector2d(-12, 60), Math.toRadians(180)) + .splineTo(new Vector2d(-63, 40), Math.toRadians(180)) //used to be 60? .build(); // Knock Pixel Stack knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) - .splineToLinearHeading(new Pose2d(-60, 25, Math.toRadians(270)), Math.toRadians(270)) + .splineToLinearHeading(new Pose2d(-62, 34, Math.toRadians(180 + 40)), Math.toRadians(180 + 40)) .build(); - // Intake Pixels - intakingPixels = robot.drive.trajectoryBuilder(knockingPixelStack.end()) - .splineToLinearHeading(new Pose2d(-60, 24, Math.toRadians(180)), Math.toRadians(180)) - .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixelsSweep)) + // Intake Pixels (1) + intakingPixels1 = robot.drive.trajectoryBuilder(knockingPixelStack.end()) + .strafeTo(new Vector2d(-60, 36)) + //.splineTo(new Vector2d(-63, -36), Math.toRadians(120)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels2)) .build(); - // Intaking Pixels Sweep - intakingPixelsSweep = robot.drive.trajectoryBuilder(intakingPixels.end()) - .splineToLinearHeading(new Pose2d(-60, 25, Math.toRadians(270)), Math.toRadians(270)) +// // Intake Pixels (2) + intakingPixels2 = robot.drive.trajectoryBuilder(intakingPixels1.end()) + .strafeRight(2) +// .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels3)) .build(); - // Drive to Backboard 2 from Pixel Stack - toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(knockingPixelStack.end()) + // Intake Pixels (3) +// intakingPixels3 = robot.drive.trajectoryBuilder(intakingPixels2.end()) +// .strafeRight(5) +// .build(); + + // Drive to Backboard from Pixel Stack + toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) .setReversed(true) - .splineTo(new Vector2d(7, 9), Math.toRadians(0)) - .splineTo(new Vector2d(48, 35), Math.toRadians(0)) + .splineTo(new Vector2d(-12, 60), Math.toRadians(0)) + .splineTo(new Vector2d(46, 35), Math.toRadians(0)) .build(); - // Drive to Park from Backboard 2 - toParkFromBackboard2 = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) + // Drive to Park + toPark = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) .strafeTo(new Vector2d(48, 60)) .build(); + break; } @@ -337,9 +361,9 @@ public void loop() { } // ------ State Methods ------ // - private void drivingToDepositPurplePixel() { + private void drivingToDepositPurplePixel() { // ------ Select Trajectory ------ // - if(firstRun) { + if (firstRun) { firstRun = false; if (elementLocation == HWC.Location.CENTER) { activeTrajectory = "toDepositCenter"; @@ -358,15 +382,10 @@ private void drivingToDepositPurplePixel() { state = State.DEPOSITING_PURPLE_PIXEL; firstRun = true; } - } + } // Deposit Purple Pixel private void depositingPurplePixel() { - // ------ Run Intake Motor Backwards for 1.5 Seconds ------ // - robot.intakeMotor.setPower(0.8); - robot.elapsedTimeSleep(1500); - robot.intakeMotor.setPower(0); - // ------ Set Next State ------ // state = State.DRIVING_TO_BACKBOARD; } @@ -374,7 +393,7 @@ private void depositingPurplePixel() { // Drive to Backboard private void drivingToBackboard() { // ------ Select Trajectory ------ // - if(firstRun) { + if (firstRun) { firstRun = false; if (elementLocation == HWC.Location.CENTER) { activeTrajectory = "toBackboardFromInitial"; @@ -398,13 +417,14 @@ private void drivingToBackboard() { // Deposit Yellow Pixel private void depositingYellowPixel() { // ------ Move Slides, Passover & Wrist ------ // - deliver(); + deliver(-170); // ------ Wait for Passover to Move ------ // robot.elapsedTimeSleep(1000); // ------ Open Claw ------ // - robot.toggleClaw('L'); + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); // ------ Wait for Claw to Open ------ // robot.elapsedTimeSleep(1000); @@ -414,6 +434,7 @@ private void depositingYellowPixel() { // ------ Set Next State ------ // state = State.DRIVING_TO_PIXEL_STACK; + firstRun = true; } // Drive to Pixel Stack @@ -421,15 +442,15 @@ private void drivingToPixelStack() { // ------ Select Trajectory ------ // if (firstRun) { firstRun = false; - if (elementLocation == HWC.Location.RIGHT) { + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toPixelStackFromCenter"; + robot.drive.followTrajectoryAsync(toPixelStackFromCenter); + } else if (elementLocation == HWC.Location.RIGHT) { activeTrajectory = "toPixelStackFromRight"; robot.drive.followTrajectoryAsync(toPixelStackFromRight); - } else if (elementLocation == HWC.Location.LEFT) { + } else { activeTrajectory = "toPixelStackFromLeft"; robot.drive.followTrajectoryAsync(toPixelStackFromLeft); - } else { - activeTrajectory = "toPixelStackFromCenter"; - robot.drive.followTrajectoryAsync(toPixelStackFromCenter); } } @@ -442,6 +463,9 @@ private void drivingToPixelStack() { // Knock Pixel Stack private void knockingPixelStack() { + // ------ Run Intake ------ // + robot.intakeMotor.setPower(-1); + // ------ Start Trajectory ------ // if (firstRun) { firstRun = false; @@ -458,34 +482,33 @@ private void knockingPixelStack() { // Intake Pixels private void intakingPixels() { - // ----- Start Trajectory ------ // + // ------ Run Intake ------ // + robot.intakeMotor.setPower(-1); + + // ------ Start Trajectory ------ // if (firstRun) { firstRun = false; - robot.time.reset(); activeTrajectory = "intakingPixels"; - robot.drive.followTrajectoryAsync(intakingPixels); + robot.drive.followTrajectoryAsync(intakingPixels1); } - // ------ Run Intake Motor Forward------ // - robot.intakeMotor.setPower(-1); - - // ------ Close Claw on Color Sensor ------ // + // ------ Check Claws & Close ------ // checkClaws(); // ------ Set Next State ------ // - if(!robot.drive.isBusy() && robot.time.seconds() >= 5) { + if (!robot.drive.isBusy()) { state = State.DRIVING_TO_BACKBOARD_2; firstRun = true; } } - // Drive to Backboard 2 + // Drive to Backboard private void drivingToBackboard2() { - // ------ Start Trajectory ------ // + // ------ Select Trajectory ------ // if (firstRun) { firstRun = false; - activeTrajectory = "toBackboard2FromPixelStack"; - robot.drive.followTrajectorySequenceAsync(toBackboardFromPixelStack); + activeTrajectory = "toBackboardFromPixelStack"; + robot.drive.followTrajectorySequence(toBackboardFromPixelStack); } // ------ Check Claws While Driving ------ // @@ -500,17 +523,22 @@ private void drivingToBackboard2() { // Deliver Backboard private void deliveringBackboard() { + // ------ Close Claws (fully to extremes) if not closed already ------ // + robot.clawL.setPosition(0.5); + robot.clawR.setPosition(0.5); + // ------ If Intake is on Turn it Off ------ // robot.intakeMotor.setPower(0); // ------ Move Slides, Passover & Wrist ------ // - deliver(); + deliver(-1000); // ------ Wait for Passover to Move ------ // robot.elapsedTimeSleep(1000); // ------ Open Claw ------ // - robot.toggleClaw('L'); + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); // ------ Wait for Claw to Open ------ // robot.elapsedTimeSleep(1000); @@ -520,15 +548,16 @@ private void deliveringBackboard() { // ------ Set Next State ------ // state = State.PARK; + firstRun = true; } // Drive to Park private void drivingToPark() { // ------ Select Trajectory ------ // - if(firstRun) { + if (firstRun) { firstRun = false; - activeTrajectory = "toParkFromBackboard2"; - robot.drive.followTrajectoryAsync(toParkFromBackboard2); + activeTrajectory = "toPark"; + robot.drive.followTrajectoryAsync(toPark); } // ------ Set Next State ------ // @@ -538,13 +567,14 @@ private void drivingToPark() { } } + // ------ Helper Methods Used All Around ------ // // Method to move to Delivery Position - private void deliver() { + private void deliver(int slideHeight) { robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos); robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos); robot.wrist.setPosition(HWC.wristDeliveryPos); - robot.pulleyLComponent.setTarget(HWC.slidePositions[1] / 4.0); - robot.pulleyRComponent.setTarget(HWC.slidePositions[1] / 4.0); + robot.pulleyLComponent.setTarget(slideHeight); + robot.pulleyRComponent.setTarget(slideHeight); } // Method to move to Intake Position @@ -568,8 +598,17 @@ private void checkClaws() { } // If both Pixels are Detected, Stop Intake - if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1) { - robot.intakeMotor.setPower(0); + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1.5 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1.5) { + robot.intakeMotor.setPower(1); } } + + // Method to Reset Slide Encoders + private void resetSlideEncoders() { + robot.leftPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.rightPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.leftPulley.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.rightPulley.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/CenterAutonomousV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/CenterAutonomousV2.java new file mode 100644 index 0000000..2bf0dd2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/CenterAutonomousV2.java @@ -0,0 +1,617 @@ +package org.firstinspires.ftc.teamcode.auton; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; +import org.firstinspires.ftc.teamcode.subsystems.HWC; +import org.firstinspires.ftc.teamcode.subsystems.roadrunner.trajectorysequence.TrajectorySequence; + +@Autonomous(name = "CenterAutonomousV2") +public class CenterAutonomousV2 extends OpMode { + // ------ State Enum ------ // + private enum State { + DRIVING_TO_DEPOSIT_PURPLE_PIXEL, DEPOSITING_PURPLE_PIXEL, DRIVING_TO_BACKBOARD, DEPOSITING_YELLOW_PIXEL, DRIVING_TO_PIXEL_STACK, KNOCKING_PIXEL_STACK, INTAKING_PIXELS, DRIVING_TO_BACKBOARD_2, DELIVERING_BACKBOARD, PARK, STOP + } + + // ------ Declare Others ------ // + private HWC robot; + private State state = State.DRIVING_TO_DEPOSIT_PURPLE_PIXEL; + private AllianceColor allianceColor = AllianceColor.RED; + private HWC.Location elementLocation; + private String activeTrajectory = ""; + private boolean firstRun = true; + + // ------ Trajectories ------ // + private Trajectory toDepositCenter; + private Trajectory toDepositRight; + private Trajectory toDepositLeft; + private Trajectory toBackboardFromCenter; + private Trajectory toBackboardFromRight; + private Trajectory toBackboardFromLeft; + private Trajectory toPixelStackFromCenter; + private Trajectory toPixelStackFromRight; + private Trajectory toPixelStackFromLeft; + private Trajectory knockingPixelStack; + private Trajectory intakingPixels1; + private Trajectory intakingPixels2; + private Trajectory intakingPixels3; + private TrajectorySequence toBackboardFromPixelStack; + private Trajectory toPark; + + // ------ Starting Positions ------ // + private final Pose2d START_POSE_RED = new Pose2d(12, -60, Math.toRadians(90.00)); + private final Pose2d START_POSE_BLUE = new Pose2d(12, 60, Math.toRadians(270.00)); + + @Override + public void init() { + // ------ Telemetry ------ // + telemetry.addData("Status", "Initializing"); + telemetry.update(); + + // ------ Initialize Robot Hardware ------ // + robot = new HWC(hardwareMap, telemetry, true); + + // ------ Start FTC Dashboard Telemetry ------ // + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + // ------ Start Tensorflow ------ // + robot.initTFOD("fpaVision.tflite"); + + // ------ Reset Servos ------ // + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); + + // ------ Close Claw for Yellow Pixel ------ // + robot.clawL.setPosition(0.5); + + // ------ Reset Slide Encoders ------ // + resetSlideEncoders(); + + // ------ Telemetry ------ // + telemetry.addData("Status", "Initialized"); + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.update(); + } + + @Override + public void init_loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ Save Element Position ------ // + elementLocation = robot.detectElement(allianceColor); + + // ------ Alliance Color Selection ------ // + if (robot.currentGamepad1.a && !robot.previousGamepad1.a) { + allianceColor = allianceColor.equals(AllianceColor.RED) ? AllianceColor.BLUE : AllianceColor.RED; + } + + telemetry.addLine("Color Selected"); + telemetry.update(); + + // ------ Set Trajectories based on Alliance Color ------ // + switch (allianceColor) { + case RED: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_RED); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_RED) + .lineTo(new Vector2d(12.0, -34)) + .build(); + + // Drive to Right Line + toDepositRight = robot.drive.trajectoryBuilder(START_POSE_RED) + .splineToLinearHeading(new Pose2d(22, -43, Math.toRadians(180 + 45)), Math.toRadians(180 + 45)) + .build(); + + // Drive to Left Line + toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_RED) + .splineToLinearHeading(new Pose2d(7, -39, Math.toRadians(135)), Math.toRadians(135)) + .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(49, -34, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Right + toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(49, -40, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Left + toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(49, -30, Math.toRadians(180))) + .build(); + + // Drive to Pixel Stack from Center + toPixelStackFromCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .splineTo(new Vector2d(24, -12), Math.toRadians(180)) //new + .splineTo(new Vector2d(-12, -12), Math.toRadians(180)) + .splineTo(new Vector2d(-63, -8), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Right + // TODO: Will Likely Hit Purple Pixel, Adjust Trajectory after Testing + toPixelStackFromRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) + .splineTo(new Vector2d(24, -12), Math.toRadians(180)) //new + .splineTo(new Vector2d(-12, -12), Math.toRadians(180)) + .splineTo(new Vector2d(-63, -8), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Left + toPixelStackFromLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) + .splineTo(new Vector2d(24, -12), Math.toRadians(180)) //new + .splineTo(new Vector2d(-12, -12), Math.toRadians(180)) + .splineTo(new Vector2d(-63, -8), Math.toRadians(180)) + .build(); + + // Knock Pixel Stack + knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .splineToLinearHeading(new Pose2d(-62, -14, Math.toRadians(180 + 40)), Math.toRadians(180 + 40)) + .build(); + + // Intake Pixels (1) + intakingPixels1 = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .strafeLeft(2) + //.splineTo(new Vector2d(-63, -36), Math.toRadians(120)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels2)) + .build(); + +// // Intake Pixels (2) + intakingPixels2 = robot.drive.trajectoryBuilder(intakingPixels1.end()) + .strafeRight(2) +// .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels3)) + .build(); + + // Intake Pixels (3) +// intakingPixels3 = robot.drive.trajectoryBuilder(intakingPixels2.end()) +// .strafeRight(5) +// .build(); + + // Drive to Backboard from Pixel Stack + toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-12, -12), Math.toRadians(0)) + .splineTo(new Vector2d(24, -12), Math.toRadians(0)) + .splineTo(new Vector2d(45, -35), Math.toRadians(0)) + .build(); + + // Drive to Park + toPark = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) + .strafeTo(new Vector2d(48, -60)) + .build(); + + break; + case BLUE: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_BLUE); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .lineTo(new Vector2d(12.0, 34)) + .build(); + + // Drive to Right Line + toDepositRight = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .splineToLinearHeading(new Pose2d(7, 39, Math.toRadians(180 + 45)), Math.toRadians(180 + 45)) + .build(); + + // Drive to Left Line + toDepositLeft = robot.drive.trajectoryBuilder(START_POSE_BLUE) + .splineToLinearHeading(new Pose2d(22, 43, Math.toRadians(180 - 45)), Math.toRadians(180 - 45)) + .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(49, 34, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Right + toBackboardFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(49, 28, Math.toRadians(180))) + .build(); + + // Drive to Backboard from Left + toBackboardFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(49, 40, Math.toRadians(180))) + .build(); + + // Drive to Pixel Stack from Center + toPixelStackFromCenter = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .splineTo(new Vector2d(24, 12), Math.toRadians(180)) //new + .splineTo(new Vector2d(-12, 12), Math.toRadians(180)) + .splineTo(new Vector2d(-63, 8), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Right + // TODO: Will Likely Hit Purple Pixel, Adjust Trajectory after Testing + toPixelStackFromRight = robot.drive.trajectoryBuilder(toBackboardFromRight.end()) + .splineTo(new Vector2d(24, 12), Math.toRadians(180)) //new + .splineTo(new Vector2d(-12, 12), Math.toRadians(180)) + .splineTo(new Vector2d(-63, 8), Math.toRadians(180)) + .build(); + + // Drive to Pixel Stack from Left + toPixelStackFromLeft = robot.drive.trajectoryBuilder(toBackboardFromLeft.end()) + .splineTo(new Vector2d(24, 12), Math.toRadians(180)) //new + .splineTo(new Vector2d(-12, 12), Math.toRadians(180)) + .splineTo(new Vector2d(-63, 8), Math.toRadians(180)) //used to be 60? + .build(); + + // Knock Pixel Stack + knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .splineToLinearHeading(new Pose2d(-62, -14, Math.toRadians(180 - 40)), Math.toRadians(180 - 40)) + .build(); + + // Intake Pixels (1) + intakingPixels1 = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .strafeLeft(2) + //.splineTo(new Vector2d(-63, -36), Math.toRadians(120)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels2)) + .build(); + +// // Intake Pixels (2) + intakingPixels2 = robot.drive.trajectoryBuilder(intakingPixels1.end()) + .strafeRight(2) +// .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels3)) + .build(); + + // Intake Pixels (3) +// intakingPixels3 = robot.drive.trajectoryBuilder(intakingPixels2.end()) +// .strafeRight(5) +// .build(); + + // Drive to Backboard from Pixel Stack + toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-12, 12), Math.toRadians(0)) + .splineTo(new Vector2d(24, 12), Math.toRadians(0)) + .splineTo(new Vector2d(45, 35), Math.toRadians(0)) + .build(); + + // Drive to Park + toPark = robot.drive.trajectoryBuilder(toBackboardFromPixelStack.end()) + .strafeTo(new Vector2d(48, 60)) + .build(); + + break; + } + + // ------ Telemetry ------ // + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Selected Alliance Color", allianceColor); + telemetry.addData("Status", "Init Loop"); + telemetry.update(); + } + + @Override + public void loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ State Machine ------ // + switch (state) { + case DRIVING_TO_DEPOSIT_PURPLE_PIXEL: + drivingToDepositPurplePixel(); + break; + case DEPOSITING_PURPLE_PIXEL: + depositingPurplePixel(); + break; + case DRIVING_TO_BACKBOARD: + drivingToBackboard(); + break; + case DEPOSITING_YELLOW_PIXEL: + depositingYellowPixel(); + break; + case DRIVING_TO_PIXEL_STACK: + drivingToPixelStack(); + break; + case KNOCKING_PIXEL_STACK: + knockingPixelStack(); + break; + case INTAKING_PIXELS: + intakingPixels(); + break; + case DRIVING_TO_BACKBOARD_2: + drivingToBackboard2(); + break; + case DELIVERING_BACKBOARD: + deliveringBackboard(); + break; + case PARK: + drivingToPark(); + case STOP: + stop(); + break; + } + + // ------ Move Slides Using PID ------ // + robot.pulleyLComponent.moveUsingPID(); + robot.pulleyRComponent.moveUsingPID(); + + // ------ Move Robot ------ // + robot.drive.update(); + + // ------ Telemetry ------ // + telemetry.addData("Status", state); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Pose", robot.drive.getPoseEstimate()); + telemetry.addData("Active Trajectory", activeTrajectory); + telemetry.addLine(); + telemetry.addData("Hardware", robot); + telemetry.update(); + } + + // ------ State Methods ------ // + private void drivingToDepositPurplePixel() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toDepositCenter"; + robot.drive.followTrajectoryAsync(toDepositCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toDepositRight"; + robot.drive.followTrajectoryAsync(toDepositRight); + } else { + activeTrajectory = "toDepositLeft"; + robot.drive.followTrajectoryAsync(toDepositLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DEPOSITING_PURPLE_PIXEL; + firstRun = true; + } + } + + // Deposit Purple Pixel + private void depositingPurplePixel() { + // ------ Set Next State ------ // + state = State.DRIVING_TO_BACKBOARD; + } + + // Drive to Backboard + private void drivingToBackboard() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toBackboardFromInitial"; + robot.drive.followTrajectoryAsync(toBackboardFromCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toBackboardFromSecond"; + robot.drive.followTrajectoryAsync(toBackboardFromRight); + } else { + activeTrajectory = "toBackBoardFromLeft"; + robot.drive.followTrajectoryAsync(toBackboardFromLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DEPOSITING_YELLOW_PIXEL; + firstRun = true; + } + } + + // Deposit Yellow Pixel + private void depositingYellowPixel() { + // ------ Move Slides, Passover & Wrist ------ // + deliver(-170); + + // ------ Wait for Passover to Move ------ // + robot.elapsedTimeSleep(1000); + + // ------ Open Claw ------ // + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); + + // ------ Wait for Claw to Open ------ // + robot.elapsedTimeSleep(1000); + + // ------ Move Slides, Passover & Wrist ------ // + intake(); + + // ------ Set Next State ------ // + state = State.DRIVING_TO_PIXEL_STACK; + firstRun = true; + } + + // Drive to Pixel Stack + private void drivingToPixelStack() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toPixelStackFromCenter"; + robot.drive.followTrajectoryAsync(toPixelStackFromCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toPixelStackFromRight"; + robot.drive.followTrajectoryAsync(toPixelStackFromRight); + } else { + activeTrajectory = "toPixelStackFromLeft"; + robot.drive.followTrajectoryAsync(toPixelStackFromLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.KNOCKING_PIXEL_STACK; + firstRun = true; + } + } + + // Knock Pixel Stack + private void knockingPixelStack() { + // ------ Run Intake ------ // + robot.intakeMotor.setPower(-1); + + // ------ Start Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "knockingPixelStack"; + robot.drive.followTrajectoryAsync(knockingPixelStack); + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.INTAKING_PIXELS; + firstRun = true; + } + } + + // Intake Pixels + private void intakingPixels() { + // ------ Run Intake ------ // + robot.intakeMotor.setPower(-1); + + // ------ Start Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "intakingPixels"; + robot.drive.followTrajectoryAsync(intakingPixels1); + } + + // ------ Check Claws & Close ------ // + checkClaws(); + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DRIVING_TO_BACKBOARD_2; + firstRun = true; + } + } + + // Drive to Backboard + private void drivingToBackboard2() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "toBackboardFromPixelStack"; + robot.drive.followTrajectorySequence(toBackboardFromPixelStack); + } + + // ------ Check Claws While Driving ------ // + checkClaws(); + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DELIVERING_BACKBOARD; + firstRun = true; + } + } + + // Deliver Backboard + private void deliveringBackboard() { + // ------ Close Claws (fully to extremes) if not closed already ------ // + robot.clawL.setPosition(0); + robot.clawR.setPosition(1); + + // ------ If Intake is on Turn it Off ------ // + robot.intakeMotor.setPower(0); + + // ------ Move Slides, Passover & Wrist ------ // + deliver(HWC.slidePositions[2]); + + // ------ Wait for Passover to Move ------ // + robot.elapsedTimeSleep(1000); + + // ------ Open Claw ------ // + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); + + // ------ Wait for Claw to Open ------ // + robot.elapsedTimeSleep(1000); + + // ------ Move Slides, Passover & Wrist ------ // + intake(); + + // ------ Set Next State ------ // + state = State.PARK; + firstRun = true; + } + + // Drive to Park + private void drivingToPark() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "toPark"; + robot.drive.followTrajectoryAsync(toPark); + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.STOP; + firstRun = true; + } + } + + // ------ Helper Methods Used All Around ------ // + // Method to move to Delivery Position + private void deliver(int slideHeight) { + robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos); + robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos); + robot.wrist.setPosition(HWC.wristDeliveryPos); + robot.pulleyLComponent.setTarget(slideHeight); + robot.pulleyRComponent.setTarget(slideHeight); + } + + // Method to move to Intake Position + private void intake() { + robot.passoverArmLeft.setPosition(HWC.passoverIntakePos); + robot.passoverArmRight.setPosition(HWC.passoverIntakePos); + robot.wrist.setPosition(HWC.wristIntakePos); + robot.pulleyLComponent.setTarget(HWC.slidePositions[0]); + robot.pulleyRComponent.setTarget(HWC.slidePositions[0]); + } + + // Method to Check Claws & Close + private void checkClaws() { + // Close Claws when Pixel Detected + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 2) { + robot.clawL.setPosition(0.5); + } + + if (robot.colorRight.getDistance(DistanceUnit.CM) <= 2) { + robot.clawR.setPosition(0.5); + } + + // If both Pixels are Detected, Stop Intake + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1.5 && robot.colorRight.getDistance(DistanceUnit.CM) <= 1.5) { + robot.intakeMotor.setPower(0); + } + } + + // Method to Reset Slide Encoders + private void resetSlideEncoders() { + robot.leftPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.rightPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.leftPulley.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.rightPulley.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/FarAutonomousV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/FarAutonomousV2.java new file mode 100644 index 0000000..9d0a57f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/FarAutonomousV2.java @@ -0,0 +1,662 @@ +package org.firstinspires.ftc.teamcode.auton; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.geometry.Vector2d; +import com.acmerobotics.roadrunner.trajectory.Trajectory; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; +import org.firstinspires.ftc.teamcode.subsystems.HWC; +import org.firstinspires.ftc.teamcode.subsystems.roadrunner.trajectorysequence.TrajectorySequence; + +@Autonomous(name = "FarAutonomousV2") +public class FarAutonomousV2 extends OpMode { + // ------ State Enum ------ // + private enum State { + DRIVING_TO_DEPOSIT_PURPLE_PIXEL, DEPOSITING_PURPLE_PIXEL, DRIVING_TO_BACKBOARD, DEPOSITING_YELLOW_PIXEL, DRIVING_TO_PIXEL_STACK, KNOCKING_PIXEL_STACK, INTAKING_PIXELS, DRIVING_TO_BACKBOARD_2, DELIVERING_BACKBOARD, PARK, STOP + } + + // ------ Declare Others ------ // + private HWC robot; + private State state = State.DRIVING_TO_DEPOSIT_PURPLE_PIXEL; + private AllianceColor allianceColor = AllianceColor.RED; + private HWC.Location elementLocation; + private String activeTrajectory = ""; + private boolean firstRun = true; + + // ------ Trajectories ------ // + private TrajectorySequence toDepositCenter; + private TrajectorySequence toDepositRight; + private TrajectorySequence toDepositLeft; + private Trajectory toPixelStackFromCenter; + private Trajectory toPixelStackFromRight; + private Trajectory toPixelStackFromLeft; + private Trajectory knockingPixelStack; + private Trajectory intakingPixels1; + private Trajectory intakingPixels2; + private Trajectory intakingPixels3; + private TrajectorySequence toBackboardFromCenter; + private TrajectorySequence toBackboardFromRight; + private TrajectorySequence toBackboardFromLeft; + private TrajectorySequence toBackboardFromPixelStack; + private Trajectory toPark; + + // ------ Starting Positions ------ // + private final Pose2d START_POSE_RED = new Pose2d(-36, -60, Math.toRadians(90.00)); + private final Pose2d START_POSE_BLUE = new Pose2d(-36, 60, Math.toRadians(270.00)); + + @Override + public void init() { + // ------ Telemetry ------ // + telemetry.addData("Status", "Initializing"); + telemetry.update(); + + // ------ Initialize Robot Hardware ------ // + robot = new HWC(hardwareMap, telemetry, true); + + // ------ Start FTC Dashboard Telemetry ------ // + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + // ------ Start Tensorflow ------ // + robot.initTFOD("fpaVision.tflite"); + + // ------ Reset Servos ------ // + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); + + // ------ Close Claw for Yellow Pixel ------ // + robot.clawL.setPosition(0.5); + + // ------ Reset Slide Encoders ------ // + resetSlideEncoders(); + + // ------ Telemetry ------ // + telemetry.addData("Status", "Initialized"); + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.update(); + } + + @Override + public void init_loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ Save Element Position ------ // + elementLocation = robot.detectElement(allianceColor); + + // ------ Alliance Color Selection ------ // + if (robot.currentGamepad1.a && !robot.previousGamepad1.a) { + if (allianceColor.equals(AllianceColor.RED)) { + allianceColor = AllianceColor.BLUE; + } else { + allianceColor = AllianceColor.RED; + } + } + + // ------ Set Trajectories based on Alliance Color ------ // + switch (allianceColor) { + case RED: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_RED); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectorySequenceBuilder(START_POSE_RED) + .strafeTo(new Vector2d(12.0 - 48, -34)) + .setReversed(true) + .strafeTo(new Vector2d(12.0 - 48, -40)) + .setReversed(false) + //.strafeTo(new Vector2d(12.0-48, -34-5)) + .build(); + + // Drive to Right Line + toDepositRight = robot.drive.trajectorySequenceBuilder(START_POSE_RED) + .splineToLinearHeading(new Pose2d(18 - 48, -35, Math.toRadians(45)), Math.toRadians(45)) + .setReversed(true) + .splineToLinearHeading(new Pose2d(18 - 48 - 5, -40, Math.toRadians(45)), Math.toRadians(180)) + .setReversed(false) + //.back(5) + .build(); + + // Drive to Left Line + toDepositLeft = robot.drive.trajectorySequenceBuilder(START_POSE_RED) + .strafeTo(new Vector2d(7 - 48, -39)) + .setReversed(true) + .strafeTo(new Vector2d(12.0 - 48, -45)) + .setReversed(false) + //.strafeTo(new Vector2d(7.0-48, -39-5)) + .build(); + + // Drive to Pixel Stack from Center + toPixelStackFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(-57, -40, Math.toRadians(180 - 40))) + .build(); + + // Drive to Pixel Stack from Right + // TODO: Will Likely Hit Purple Pixel, Adjust Trajectory after Testing + toPixelStackFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(-57, -40, Math.toRadians(180 - 40))) + .build(); + + // Drive to Pixel Stack from Left + toPixelStackFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(-57, -40, Math.toRadians(180 - 40))) + .build(); + + // Knock Pixel Stack + knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .splineToLinearHeading(new Pose2d(-59, -36, Math.toRadians(180 - 40)), Math.toRadians(180 - 40)) + .splineToLinearHeading(new Pose2d(-57, -30, Math.toRadians(180 - 40)), Math.toRadians(180 - 0)) + .build(); + + // Intake Pixels (1) + intakingPixels1 = robot.drive.trajectoryBuilder(knockingPixelStack.end()) + .strafeRight(8) + //.splineTo(new Vector2d(-63, -36), Math.toRadians(120)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels2)) + .build(); + +// // Intake Pixels (2) + intakingPixels2 = robot.drive.trajectoryBuilder(intakingPixels1.end()) + .strafeLeft(2) +// .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels3)) + .build(); + + // Intake Pixels (3) +// intakingPixels3 = robot.drive.trajectoryBuilder(intakingPixels2.end()) +// .strafeRight(5) +// .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-50, -12), Math.toRadians(0)) + .splineTo(new Vector2d(-12, -8), Math.toRadians(0)) + .splineTo(new Vector2d(32, -12), Math.toRadians(0)) + .splineTo(new Vector2d(49, -34), Math.toRadians(0)) + .build(); + + // Drive to Backboard from Right + toBackboardFromRight = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-50, -12), Math.toRadians(0)) + .splineTo(new Vector2d(-12, -8), Math.toRadians(0)) + .splineTo(new Vector2d(32, -12), Math.toRadians(0)) + .splineTo(new Vector2d(49, -40), Math.toRadians(0)) + .build(); + + // Drive to Backboard from Left + toBackboardFromLeft = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-50, -12), Math.toRadians(0)) + .splineTo(new Vector2d(-12, -8), Math.toRadians(0)) + .splineTo(new Vector2d(32, -12), Math.toRadians(0)) + .splineTo(new Vector2d(49, -28), Math.toRadians(0)) + .build(); + + // Drive to Park + toPark = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .strafeTo(new Vector2d(48, -60)) + .build(); + +// // Drive to Backboard from Pixel Stack (DEPRECATED?) +// toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) +// .setReversed(true) +// .splineTo(new Vector2d(-12, -60), Math.toRadians(0)) +// .splineTo(new Vector2d(47, -35), Math.toRadians(0)) +// .build(); + + break; + case BLUE: + // ------ Set Robot Start Pose ------ // + robot.drive.setPoseEstimate(START_POSE_BLUE); + + // ------ Declare Trajectories ------ // + // Drive to Center Line + toDepositCenter = robot.drive.trajectorySequenceBuilder(START_POSE_BLUE) + .strafeTo(new Vector2d(12.0 - 48, 34)) + .setReversed(true) + .strafeTo(new Vector2d(12.0 - 48, 40)) + .setReversed(false) + //.strafeTo(new Vector2d(12.0-48, -34-5)) + .build(); + + // Drive to Right Line + toDepositRight = robot.drive.trajectorySequenceBuilder(START_POSE_BLUE) + .strafeTo(new Vector2d(7 - 48, 39)) + .setReversed(true) + .strafeTo(new Vector2d(7 - 48, 45)) + .setReversed(false) + + //.back(5) + .build(); + + // Drive to Left Line + toDepositLeft = robot.drive.trajectorySequenceBuilder(START_POSE_BLUE) + .splineToLinearHeading(new Pose2d(18 - 48, 35, Math.toRadians(360 - 45)), Math.toRadians(360 - 45)) + .setReversed(true) + .splineToLinearHeading(new Pose2d(18 - 48 - 5, 40, Math.toRadians(360 - 45)), Math.toRadians(360 - 45)) + .setReversed(false) + .build(); + + // Drive to Pixel Stack from Center + toPixelStackFromCenter = robot.drive.trajectoryBuilder(toDepositCenter.end()) + .lineToLinearHeading(new Pose2d(-57, 40, Math.toRadians(180 + 40))) + .build(); + + // Drive to Pixel Stack from Right + // TODO: Will Likely Hit Purple Pixel, Adjust Trajectory after Testing + toPixelStackFromRight = robot.drive.trajectoryBuilder(toDepositRight.end()) + .lineToLinearHeading(new Pose2d(-57, 40, Math.toRadians(180 + 40))) + .build(); + + // Drive to Pixel Stack from Left + toPixelStackFromLeft = robot.drive.trajectoryBuilder(toDepositLeft.end()) + .lineToLinearHeading(new Pose2d(-57, 40, Math.toRadians(180 + 40))) + .build(); + + // Knock Pixel Stack + knockingPixelStack = robot.drive.trajectoryBuilder(toPixelStackFromCenter.end()) + .splineToLinearHeading(new Pose2d(-59, 36, Math.toRadians(180 + 40)), Math.toRadians(180 + 40)) + .splineToLinearHeading(new Pose2d(-57, 30, Math.toRadians(180 + 40)), Math.toRadians(180 - 0)) + .build(); + + // Intake Pixels (1) + intakingPixels1 = robot.drive.trajectoryBuilder(knockingPixelStack.end()) + .strafeLeft(8) + //.splineTo(new Vector2d(-63, -36), Math.toRadians(120)) + .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels2)) + .build(); + +// // Intake Pixels (2) + intakingPixels2 = robot.drive.trajectoryBuilder(intakingPixels1.end()) + .strafeRight(2) +// .addDisplacementMarker(() -> robot.drive.followTrajectoryAsync(intakingPixels3)) + .build(); + + // Intake Pixels (3) +// intakingPixels3 = robot.drive.trajectoryBuilder(intakingPixels2.end()) +// .strafeRight(5) +// .build(); + + // Drive to Backboard from Center + toBackboardFromCenter = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-50, 12), Math.toRadians(0)) + .splineTo(new Vector2d(-12, 8), Math.toRadians(0)) + .splineTo(new Vector2d(32, 12), Math.toRadians(0)) + .splineTo(new Vector2d(49, 34), Math.toRadians(0)) + .build(); + + // Drive to Backboard from Right + toBackboardFromRight = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-50, 12), Math.toRadians(0)) + .splineTo(new Vector2d(-12, 8), Math.toRadians(0)) + .splineTo(new Vector2d(32, 12), Math.toRadians(0)) + .splineTo(new Vector2d(49, 28), Math.toRadians(0)) + .build(); + + // Drive to Backboard from Left + toBackboardFromLeft = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) + .setReversed(true) + .splineTo(new Vector2d(-50, 12), Math.toRadians(0)) + .splineTo(new Vector2d(-12, 8), Math.toRadians(0)) + .splineTo(new Vector2d(32, 12), Math.toRadians(0)) + .splineTo(new Vector2d(49, 40), Math.toRadians(0)) + .build(); + + // Drive to Park + toPark = robot.drive.trajectoryBuilder(toBackboardFromCenter.end()) + .strafeTo(new Vector2d(48, 60)) + .build(); + +// // Drive to Backboard from Pixel Stack (DEPRECATED?) +// toBackboardFromPixelStack = robot.drive.trajectorySequenceBuilder(intakingPixels2.end()) +// .setReversed(true) +// .splineTo(new Vector2d(-12, -60), Math.toRadians(0)) +// .splineTo(new Vector2d(47, -35), Math.toRadians(0)) +// .build(); + + break; + } + + // ------ Telemetry ------ // + telemetry.addData(">", "Yellow Pixel on Left Side"); + telemetry.addData(">", "Purple Pixel in intake"); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Selected Alliance Color", allianceColor); + telemetry.addData("Status", "Init Loop"); + telemetry.update(); + } + + @Override + public void loop() { + // ------ GamePad Updates ------ // + robot.previousGamepad1.copy(robot.currentGamepad1); + robot.previousGamepad2.copy(robot.currentGamepad2); + robot.currentGamepad1.copy(gamepad1); + robot.currentGamepad2.copy(gamepad2); + + // ------ State Machine ------ // + switch (state) { + case DRIVING_TO_DEPOSIT_PURPLE_PIXEL: + drivingToDepositPurplePixel(); + break; + case DEPOSITING_PURPLE_PIXEL: + depositingPurplePixel(); + break; + case DRIVING_TO_BACKBOARD: + drivingToBackboard(); + break; +// case DEPOSITING_YELLOW_PIXEL: +// depositingYellowPixel(); +// break; + case DRIVING_TO_PIXEL_STACK: + drivingToPixelStack(); + break; + case KNOCKING_PIXEL_STACK: + knockingPixelStack(); + break; + case INTAKING_PIXELS: + intakingPixels(); + break; +// case DRIVING_TO_BACKBOARD_2: +// drivingToBackboard2(); +// break; + case DELIVERING_BACKBOARD: + deliveringBackboard(); + break; + case PARK: + drivingToPark(); + case STOP: + stop(); + break; + } + + // ------ Move Slides Using PID ------ // + robot.pulleyLComponent.moveUsingPID(); + robot.pulleyRComponent.moveUsingPID(); + + // ------ Move Robot ------ // + robot.drive.update(); + + // ------ Telemetry ------ // + telemetry.addData("Status", state); + telemetry.addData("Element Location", elementLocation); + telemetry.addData("Pose", robot.drive.getPoseEstimate()); + telemetry.addData("Active Trajectory", activeTrajectory); + telemetry.addLine(); + telemetry.addData("Hardware", robot); + telemetry.update(); + } + + // ------ State Methods ------ // + private void drivingToDepositPurplePixel() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toDepositCenter"; + robot.drive.followTrajectorySequenceAsync(toDepositCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toDepositRight"; + robot.drive.followTrajectorySequenceAsync(toDepositRight); + } else { + activeTrajectory = "toDepositLeft"; + robot.drive.followTrajectorySequenceAsync(toDepositLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DEPOSITING_PURPLE_PIXEL; + firstRun = true; + } + } + + // Deposit Purple Pixel + private void depositingPurplePixel() { + // ------ Set Next State ------ // + state = State.DRIVING_TO_PIXEL_STACK; + } + + // Drive to Pixel Stack + private void drivingToPixelStack() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toPixelStackFromCenter"; + robot.drive.followTrajectoryAsync(toPixelStackFromCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toPixelStackFromRight"; + robot.drive.followTrajectoryAsync(toPixelStackFromRight); + } else { + activeTrajectory = "toPixelStackFromLeft"; + robot.drive.followTrajectoryAsync(toPixelStackFromLeft); + } + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.KNOCKING_PIXEL_STACK; + firstRun = true; + } + } + + // Knock Pixel Stack + private void knockingPixelStack() { + // ------ Run Intake ------ // + robot.intakeMotor.setPower(-1); + + // ------ Start Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "knockingPixelStack"; + robot.drive.followTrajectoryAsync(knockingPixelStack); + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.INTAKING_PIXELS; + firstRun = true; + } + } + + // Intake Pixels + private void intakingPixels() { + + // ------ Run Intake ------ // + robot.intakeMotor.setPower(-1); + + // ------ Start Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "intakingPixels"; + robot.drive.followTrajectoryAsync(intakingPixels1); + } + + // ------ Check Claws & Close ------ // + checkClaws(); + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DRIVING_TO_BACKBOARD; + firstRun = true; + } + } + + // Drive to Backboard + private void drivingToBackboard() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + if (elementLocation == HWC.Location.CENTER) { + activeTrajectory = "toBackboardFromInitial"; + robot.drive.followTrajectorySequenceAsync(toBackboardFromCenter); + } else if (elementLocation == HWC.Location.RIGHT) { + activeTrajectory = "toBackboardFromSecond"; + robot.drive.followTrajectorySequenceAsync(toBackboardFromRight); + } else { + activeTrajectory = "toBackBoardFromLeft"; + robot.drive.followTrajectorySequenceAsync(toBackboardFromLeft); + } + } + + // ------ Check Claws Again & Close ------ // + checkClaws(); + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.DELIVERING_BACKBOARD; + firstRun = true; + } + } + + // Deliver Backboard + private void deliveringBackboard() { + // ------ Close Claws if not closed already ------ // + robot.clawL.setPosition(0.5); + robot.clawR.setPosition(0.5); + + // ------ If Intake is on Turn it Off ------ // + robot.intakeMotor.setPower(0); + + // ------ Move Slides, Passover & Wrist ------ // + deliver(HWC.slidePositions[2]); + + // ------ Wait for Passover to Move ------ // + robot.elapsedTimeSleep(1000); + + // ------ Open Claw ------ // + robot.clawL.setPosition(1); + robot.clawR.setPosition(0); + + // ------ Wait for Claw to Open ------ // + robot.elapsedTimeSleep(1000); + + // ------ Move Slides, Passover & Wrist ------ // + intake(); + + // ------ Set Next State ------ // + state = State.PARK; + firstRun = true; + } + + // Drive to Park + private void drivingToPark() { + // ------ Select Trajectory ------ // + if (firstRun) { + firstRun = false; + activeTrajectory = "toPark"; + robot.drive.followTrajectoryAsync(toPark); + } + + // ------ Set Next State ------ // + if (!robot.drive.isBusy()) { + state = State.STOP; + firstRun = true; + } + } + + // Drive to Backboard +// private void drivingToBackboard2() { +// // ------ Select Trajectory ------ // +// if(firstRun) { +// firstRun = false; +// activeTrajectory = "toBackboardFromPixelStack"; +// robot.drive.followTrajectorySequence(toBackboardFromPixelStack); +// } +// +// // ------ Check Claws While Driving ------ // +// checkClaws(); +// +// // ------ Set Next State ------ // +// if (!robot.drive.isBusy()) { +// state = State.DELIVERING_BACKBOARD; +// firstRun = true; +// } +// } +// +// // Deposit Yellow Pixel +// private void depositingYellowPixel() { +// // ------ Move Slides, Passover & Wrist ------ // +// deliver(-170); +// +// // ------ Wait for Passover to Move ------ // +// robot.elapsedTimeSleep(1000); +// +// // ------ Open Claw ------ // +// robot.clawL.setPosition(1); +// robot.clawR.setPosition(0); +// +// // ------ Wait for Claw to Open ------ // +// robot.elapsedTimeSleep(1000); +// +// // ------ Move Slides, Passover & Wrist ------ // +// intake(); +// +// // ------ Set Next State ------ // +// state = State.DRIVING_TO_PIXEL_STACK; +// firstRun = true; +// } + + // ------ Helper Methods Used All Around ------ // + // Method to move to Delivery Position + private void deliver(int slideHeight) { + robot.passoverArmLeft.setPosition(HWC.passoverDeliveryPos); + robot.passoverArmRight.setPosition(HWC.passoverDeliveryPos); + robot.wrist.setPosition(HWC.wristDeliveryPos); + robot.pulleyLComponent.setTarget(slideHeight); + robot.pulleyRComponent.setTarget(slideHeight); + } + + // Method to move to Intake Position + private void intake() { + robot.passoverArmLeft.setPosition(HWC.passoverIntakePos); + robot.passoverArmRight.setPosition(HWC.passoverIntakePos); + robot.wrist.setPosition(HWC.wristIntakePos); + robot.pulleyLComponent.setTarget(HWC.slidePositions[0]); + robot.pulleyRComponent.setTarget(HWC.slidePositions[0]); + } + + // Method to Check Claws & Close + private void checkClaws() { + // Close Claws when Pixel Detected + boolean leftFull = false; + boolean rightFull = false; + if (robot.colorLeft.getDistance(DistanceUnit.CM) <= 1.25) { + robot.clawL.setPosition(0.5); + leftFull = true; + } + + if (robot.colorRight.getDistance(DistanceUnit.CM) <= 2) { + robot.clawR.setPosition(0.5); + rightFull = true; + } + + // If both Pixels are Detected, Reverse Intake + if (leftFull && rightFull) { + robot.intakeMotor.setPower(1); + } + } + + // Method to Reset Slide Encoders + private void resetSlideEncoders() { + robot.leftPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.rightPulley.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.leftPulley.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.rightPulley.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SimpleParkAuton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SimpleParkAuton.java deleted file mode 100644 index b241200..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/SimpleParkAuton.java +++ /dev/null @@ -1,80 +0,0 @@ -package org.firstinspires.ftc.teamcode.auton; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; - -import org.firstinspires.ftc.teamcode.subsystems.HWC; - -/** - * Main Autonomous OpMode for the robot - */ -@Autonomous(name = "Sleepy") -public class SimpleParkAuton extends OpMode { - HWC robot; - String color = "red"; - - @Override - public void init() { - // ------ Telemetry ------ // - telemetry.addData("Status", "Initializing"); - telemetry.update(); - - // ------ Initialize Robot Hardware ------ // - robot = new HWC(hardwareMap, telemetry, false); - - // ------ Reset Servos ------ // - robot.clawL.setPosition(1); - robot.clawR.setPosition(0); - - // ------ Telemetry ------ // - telemetry.addData("Status", "Initialized"); - telemetry.update(); - } - - @Override - public void init_loop() { - if (robot.currentGamepad1.a && !robot.previousGamepad1.a) { - if (color.equals("red")) { - color = "blue"; - } else if (color.equals("blue")) { - color = "red"; - } - } - - telemetry.addData("Status", "Initialized"); - telemetry.addData("Color", color); - telemetry.update(); - } - - @Override - public void start() { - robot.elapsedTimeSleep(10000); - - if (color.equals("red")) { - robot.leftFront.setPower(-2); - robot.leftRear.setPower(2); - robot.rightFront.setPower(-2); - robot.rightRear.setPower(2); - robot.elapsedTimeSleep(1500); - robot.leftFront.setPower(0); - robot.leftRear.setPower(0); - robot.rightFront.setPower(0); - robot.rightRear.setPower(0); - } else if (color.equals("blue")) { - robot.leftFront.setPower(2); - robot.leftRear.setPower(-2); - robot.rightFront.setPower(2); - robot.rightRear.setPower(-2); - robot.elapsedTimeSleep(1500); - robot.leftFront.setPower(0); - robot.leftRear.setPower(0); - robot.rightFront.setPower(0); - robot.rightRear.setPower(0); - } - } - - @Override - public void loop() { - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java index fe7a671..b3d0ba6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java @@ -15,7 +15,6 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.tfod.Recognition; import org.firstinspires.ftc.teamcode.auton.enums.AllianceColor; import org.firstinspires.ftc.teamcode.subsystems.pid.RobotComponents; @@ -40,7 +39,7 @@ public enum Location { } // ------ Declare Slide Positions ------ // - public static int[] slidePositions = { 0, -200, -482, -2110, -3460, -4250}; + public static int[] slidePositions = {0, -200, -482, -2110, -3460, -4250}; // ------ Declare Servo Positions ------ // public static double passoverDeliveryPos = 0.2; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java index bb08f43..2945693 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java @@ -59,10 +59,10 @@ public void loop() { kit.currentGamepad2.copy(gamepad2); // ------ Set PID ------ // - controller.setPID(p, i ,d); + controller.setPID(p, i, d); // ------ Get Motor Position ------ // - motorPos = (slideLeft.getCurrentPosition() + slideRight.getCurrentPosition())/ 2.0; + motorPos = (slideLeft.getCurrentPosition() + slideRight.getCurrentPosition()) / 2.0; // ------ Calculate PID ------ // pid = controller.calculate(motorPos, target); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java index 889d62a..e672471 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/RobotComponents.java @@ -1,8 +1,6 @@ package org.firstinspires.ftc.teamcode.subsystems.pid; import com.arcrobotics.ftclib.controller.PIDController; -import com.qualcomm.robotcore.hardware.AnalogInput; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorEx; /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ColorTest.java deleted file mode 100644 index 87f7362..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/ColorTest.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; - -import org.firstinspires.ftc.teamcode.subsystems.HWC; - -/** - * TeleOp OpMode for simply driving with strafing wheels - */ -@TeleOp(name = "Color Sensor Test", group = "Testing") -public class ColorTest extends OpMode { - HWC robot; - - @Override - public void init() { - // ------ Telemetry ------ // - telemetry.addData("Status", "Initializing"); - telemetry.update(); - - // ------ Initialize Robot Hardware ------ // - robot = new HWC(hardwareMap, telemetry, false); - } - - @Override - public void init_loop() { - } - - @Override - public void start() { - } - - @Override - public void loop() { - if (gamepad1.a) { - telemetry.addData("Color", robot.colorLeft.argb()); - } else if (gamepad1.b) { - - telemetry.addData("Color", robot.colorRight.argb()); - } else if (gamepad1.y) { - telemetry.addData("Color", "Not Detecting"); - } - - telemetry.update(); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/VisionTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/VisionTest.java deleted file mode 100644 index 191f205..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/VisionTest.java +++ /dev/null @@ -1,54 +0,0 @@ -package org.firstinspires.ftc.teamcode.teleop; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.teamcode.subsystems.vision.PixelCameraTest; -import org.openftc.easyopencv.OpenCvCamera; -import org.openftc.easyopencv.OpenCvCameraFactory; -import org.openftc.easyopencv.OpenCvCameraRotation; - -@Autonomous(name = "Vision Test", group = "Testing") -public class VisionTest extends LinearOpMode { - OpenCvCamera camera; - String webcamName = "Webcam 1"; - - @Override - public void runOpMode() throws InterruptedException { - int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); - camera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, webcamName), cameraMonitorViewId); - PixelCameraTest detector = new PixelCameraTest(telemetry); - camera.setPipeline(detector); - camera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() { - @Override - public void onOpened() { - camera.startStreaming(320, 240, OpenCvCameraRotation.SIDEWAYS_LEFT); - } - - @Override - public void onError(int errorCode) { - } - }); - - - waitForStart(); - switch (detector.getLocation()) { - case LEFT: - telemetry.addData("Object Position", "Left"); - break; - case RIGHT: - telemetry.addData("Object Position", "Right"); - break; - case MIDDLE: - telemetry.addData("Object Position", "Middle"); - break; - case NOT_FOUND: - telemetry.addData("Object Position", "Unknown!!!"); - - } - telemetry.update(); - - - } -} diff --git a/build.gradle b/build.gradle index bedf99f..5619ec1 100644 --- a/build.gradle +++ b/build.gradle @@ -12,7 +12,7 @@ buildscript { dependencies { // Note for FTC Teams: Do not modify this yourself. // Note for 19922: You can do whatever, Milo will make sure things don't break. - classpath 'com.android.tools.build:gradle:8.1.1' + classpath 'com.android.tools.build:gradle:8.2.2' } } diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties index da1db5f..15de902 100644 --- a/gradle/wrapper/gradle-wrapper.properties +++ b/gradle/wrapper/gradle-wrapper.properties @@ -1,5 +1,5 @@ distributionBase=GRADLE_USER_HOME distributionPath=wrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-8.0-bin.zip +distributionUrl=https\://services.gradle.org/distributions/gradle-8.2-bin.zip zipStoreBase=GRADLE_USER_HOME zipStorePath=wrapper/dists