From 8366470c8f374b7fad43bd668b5cb7d431f7ca09 Mon Sep 17 00:00:00 2001 From: kuek64 Date: Thu, 21 Nov 2024 21:27:55 -0600 Subject: [PATCH] Cause I'm hopeful, yes I am, hopeful for today, Take this music and use it Let it take you away, And be hopeful, hopeful and he'll make a way I know it ain't easy but that's okay. --- .../indubitables/config/runmodes/Auto.java | 37 ++--- .../config/util/FieldConstants.java | 12 +- .../indubitables/opmode/BlueObservation.java | 133 ++++++++---------- 3 files changed, 78 insertions(+), 104 deletions(-) diff --git a/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java b/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java index 3d7a222..c4edd85 100644 --- a/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java +++ b/TeamCode/src/main/java/indubitables/config/runmodes/Auto.java @@ -103,7 +103,7 @@ public void update() { telemetryUpdate(); } - public void createPoses() { + public void createPoses() { //Able to be cut switch (startLocation) { case BLUE_BUCKET: startPose = blueBucketStartPose; @@ -151,7 +151,6 @@ public void createPoses() { } public void buildPaths() { - if((startLocation == RobotStart.BLUE_BUCKET) || (startLocation == RobotStart.RED_BUCKET)) { preload = follower.pathBuilder() .addPath(new BezierLine(new Point(startPose), new Point(preloadPose))) @@ -186,7 +185,7 @@ public void buildPaths() { preload = follower.pathBuilder() .addPath(new BezierLine(new Point(startPose), new Point(preloadPose))) .setLinearHeadingInterpolation(startPose.getHeading(), preloadPose.getHeading()) - .setZeroPowerAccelerationMultiplier(1) + .setZeroPowerAccelerationMultiplier(0.25) .build(); pushSamples = follower.pathBuilder() @@ -210,7 +209,7 @@ public void buildPaths() { grab1 = follower.pathBuilder() .addPath(new BezierLine(new Point(specimenSetPose), new Point(grab1Pose))) .setLinearHeadingInterpolation(specimenSetPose.getHeading(), grab1Pose.getHeading()) - //.setZeroPowerAccelerationMultiplier(0.25) + .setZeroPowerAccelerationMultiplier(0.25) .build(); specimen1 = follower.pathBuilder() @@ -222,7 +221,7 @@ public void buildPaths() { grab2 = follower.pathBuilder() .addPath(new BezierLine(new Point(specimen1Pose), new Point(grab2Pose))) .setLinearHeadingInterpolation(specimen1Pose.getHeading(), grab2Pose.getHeading()) - //.setZeroPowerAccelerationMultiplier(1) + .setZeroPowerAccelerationMultiplier(0.25) .build(); specimen2 = follower.pathBuilder() @@ -234,7 +233,7 @@ public void buildPaths() { grab3 = follower.pathBuilder() .addPath(new BezierLine(new Point(specimen2Pose), new Point(grab3Pose))) .setLinearHeadingInterpolation(specimen2Pose.getHeading(), grab3Pose.getHeading()) - //.setZeroPowerAccelerationMultiplier(1) + .setZeroPowerAccelerationMultiplier(0.25) .build(); specimen3 = follower.pathBuilder() @@ -246,7 +245,7 @@ public void buildPaths() { grab4 = follower.pathBuilder() .addPath(new BezierLine(new Point(specimen3Pose), new Point(grab4Pose))) .setLinearHeadingInterpolation(specimen3Pose.getHeading(), grab4Pose.getHeading()) - //.setZeroPowerAccelerationMultiplier(1) + .setZeroPowerAccelerationMultiplier(0.25) .build(); specimen4 = follower.pathBuilder() @@ -258,7 +257,7 @@ public void buildPaths() { park = follower.pathBuilder() .addPath(new BezierLine(new Point(specimen4Pose), new Point(parkPose))) .setLinearHeadingInterpolation(specimen4Pose.getHeading(), parkPose.getHeading()) - //.setZeroPowerAccelerationMultiplier(0.5) + //.setZeroPowerAccelerationMultiplier(0.25) .build(); } @@ -357,7 +356,7 @@ public void chamber() { actionBusy = true; arm.specimenScore(); claw.close(); - claw.score(); + claw.specimenScore(); extend.toZero(); chamberTimer.resetTimer(); setChamberState(2); @@ -369,21 +368,11 @@ public void chamber() { } break; case 3: - if(chamberTimer.getElapsedTimeSeconds() > 0.35) { - claw.specimenScore(); - arm.score(); - chamberTimer.resetTimer(); - setChamberState(4); - } - break; - case 4: if(chamberTimer.getElapsedTimeSeconds() > 0.25) { - claw.score(); - claw.open(); - arm.score(); actionBusy = false; setChamberState(-1); } + break; } } @@ -391,7 +380,7 @@ public void setChamberState(int x) { chamberState = x; } - public void startChamber2() { + public void startChamber() { if(actionNotBusy()) { setChamberState(1); } @@ -524,9 +513,9 @@ public void telemetryUpdate() { telemetry.addData("Y: ", follower.getPose().getY()); telemetry.addData("Heading: ", follower.getPose().getHeading()); telemetry.addData("Action Busy?: ", actionBusy); - telemetry.addData("Lift Pos", lift.getPos()); - telemetry.addData("Extend Pos", extend.leftExtend.getPosition()); - telemetry.addData("Extend Limit", extend.extendLimit); + //telemetry.addData("Lift Pos", lift.getPos()); + //telemetry.addData("Extend Pos", extend.leftExtend.getPosition()); + //telemetry.addData("Extend Limit", extend.extendLimit); telemetry.addData("Claw Grab State", claw.grabState); telemetry.addData("Claw Pivot State", claw.pivotState); // telemetry.addData("Intake Spin State", intakeSpinState); diff --git a/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java b/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java index 78569bd..3880502 100644 --- a/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java +++ b/TeamCode/src/main/java/indubitables/config/util/FieldConstants.java @@ -19,9 +19,9 @@ public enum RobotStart { public static final Pose redBucketStartPose = new Pose(144-blueBucketStartPose.getX(), blueBucketStartPose.getY(), 0); public static final Pose redObservationStartPose = new Pose(144-blueObservationStartPose.getX(), blueObservationStartPose.getY(), 0); - // Preload Poses+ + // Preload Poses public static final Pose blueBucketPreloadPose = new Pose(29.25, 78.375, Math.toRadians(180)); - public static final Pose blueObservationPreloadPose = new Pose(30.75, 60.625, Math.toRadians(180)); + public static final Pose blueObservationPreloadPose = new Pose(32, 60.625, Math.toRadians(180)); public static final Pose redBucketPreloadPose = new Pose(144-blueBucketPreloadPose.getX(), blueBucketPreloadPose.getY(), 0); public static final Pose redObservationPreloadPose = new Pose(144-blueObservationPreloadPose.getX(), blueObservationPreloadPose.getY(), 0); @@ -40,10 +40,10 @@ public enum RobotStart { public static final Pose blueObservationSpecimenPickup2Pose = new Pose(7, 35, Math.toRadians(180)); public static final Pose blueObservationSpecimenPickup3Pose = new Pose(7, 35, Math.toRadians(180)); public static final Pose blueObservationSpecimenPickup4Pose = new Pose(7, 35, Math.toRadians(180)); - public static final Pose blueObservationSpecimen1Pose = new Pose(30.75, 66.25, Math.toRadians(180)); - public static final Pose blueObservationSpecimen2Pose = new Pose(30.75, 70.75, Math.toRadians(180)); - public static final Pose blueObservationSpecimen3Pose = new Pose(30.75, 74.25, Math.toRadians(180)); - public static final Pose blueObservationSpecimen4Pose = new Pose(30.75, 78.25, Math.toRadians(180)); + public static final Pose blueObservationSpecimen1Pose = new Pose(32, 66.25, Math.toRadians(180)); + public static final Pose blueObservationSpecimen2Pose = new Pose(32, 70.75, Math.toRadians(180)); + public static final Pose blueObservationSpecimen3Pose = new Pose(32, 74.25, Math.toRadians(180)); + public static final Pose blueObservationSpecimen4Pose = new Pose(32, 78.25, Math.toRadians(180)); public static final Pose blueBucketParkPose = new Pose(62, 97.75, Math.toRadians(90)); diff --git a/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java b/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java index 3b2a06b..db7dd06 100644 --- a/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java +++ b/TeamCode/src/main/java/indubitables/opmode/BlueObservation.java @@ -2,12 +2,9 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.DcMotor; import indubitables.pedroPathing.follower.Follower; import indubitables.config.runmodes.Auto; -import indubitables.config.util.action.Actions; -import indubitables.config.util.action.SequentialAction; import indubitables.pedroPathing.util.Timer; @Autonomous(name="BlueObservation", group="A") @@ -41,182 +38,170 @@ public void loop() { public void pathUpdate() { switch (pathState) { case 0: //Runs to the position of the preload and holds it's point at 0.5 power + auto.liftPIDF = false; auto.extend.toZero(); - auto.startChamber2(); - auto.follower.setMaxPower(0.7); + auto.startChamber(); + auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.preload, true); setPathState(1); break; - case 1: //Waits until follower reaches it's position then begins the Chamber State Machine - if(auto.actionNotBusy()) { - setPathState(2); - } - break; - case 2: //Once Chamber State Machine Machine finishes, begins Pathchain to push elements to the submersible + case 1: //Once Chamber State Machine finishes, begins Pathchain to push elements to the submersible if(auto.actionNotBusy()) { auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.pushSamples, true); - setPathState(3); + setPathState(2); } break; - case 3: //Waits until follower reaches it's position then begins the Specimen State Machine + case 2: //Once the Pathchain finishes, begins the Specimen State Machine if(!auto.follower.isBusy()) { - //auto.liftPIDF = false; - //auto.lift.manual(-0.4); - setPathState(4); - } - break; - case 4: - if(pathTimer.getElapsedTimeSeconds() > 0) { - //auto.lift.manual(0); auto.startSpecimen(); - setPathState(5); + setPathState(3); } break; - case 5: //Runs to the position of the grab1 and holds it's point at full power - if(auto.actionNotBusy() && !auto.follower.isBusy()) { + case 3: //Once the Specimen State Machine finishes, begins the grab path + if(auto.actionNotBusy()) { auto.follower.setMaxPower(0.8); auto.follower.followPath(auto.grab1, false); - setPathState(6); + setPathState(4); } break; - case 6: //Closes the claw when the follower reaches the grab1 position + case 4: //Closes the claw when the follower reaches the grab1 position if(pathTimer.getElapsedTimeSeconds() > 0.5) { auto.claw.close(); - setPathState(7); + setPathState(5); } break; - case 7: //Sets the arm to a neutral position and puts lifts to zero; + case 5: //Sets the arm to a neutral position and puts lifts to zero; if(pathTimer.getElapsedTimeSeconds() > 0.25) { auto.init(); auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.specimen1, true); - setPathState(8); + setPathState(6); } break; - case 8: //Waits until follower reaches it's position then begins the Chamber State Machine + case 6: //Waits until follower reaches it's position then begins the Chamber State Machine if(pathTimer.getElapsedTimeSeconds() > 0) { - auto.startChamber2(); - setPathState(9); + auto.startChamber(); + setPathState(7); } break; - case 9: //Starts the Specimen State Machine + case 7: //Starts the Specimen State Machine if(auto.actionNotBusy()) { auto.startSpecimen(); - setPathState(10); + setPathState(8); } break; - case 10: //Begins the path for grab 2 & closes the claw once it reaches position and passes 0.75 seconds + case 8: //Begins the path for grab 2 & closes the claw once it reaches position and passes 0.75 seconds if(pathTimer.getElapsedTimeSeconds() > 0) { auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.grab2, true); - setPathState(11); + setPathState(9); } break; - case 11: + case 9: if(pathTimer.getElapsedTimeSeconds() > 2) { auto.claw.close(); - setPathState(12); + setPathState(10); } break; - case 12: //Waits 0.25 seconds and puts robot in neutral position + case 10: //Waits 0.25 seconds and puts robot in neutral position if(pathTimer.getElapsedTimeSeconds() > 0.25) { auto.init(); - setPathState(13); + setPathState(11); } break; - case 13: //Drives to chamber once action finishes + case 11: //Drives to chamber once action finishes auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.specimen2, true); - setPathState(14); + setPathState(12); break; - case 14: //Starts the Chamber State Machine + case 12: //Starts the Chamber State Machine if(pathTimer.getElapsedTimeSeconds() > 0) { - auto.startChamber2(); - setPathState(15); + auto.startChamber(); + setPathState(13); } break; - case 15: //Starts the Specimen State Machine + case 13: //Starts the Specimen State Machine if(auto.actionNotBusy()) { auto.startSpecimen(); - setPathState(16); + setPathState(14); } break; - case 16: //Begins the path for grab 2 & closes the claw once it reaches position and passes 0.75 seconds + case 14: //Begins the path for grab 2 & closes the claw once it reaches position and passes 0.75 seconds if(pathTimer.getElapsedTimeSeconds() > 0) { auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.grab3, true); - setPathState(17); + setPathState(15); } break; - case 17: + case 15: if(pathTimer.getElapsedTimeSeconds() > 2) { auto.claw.close(); - setPathState(18); + setPathState(16); } break; - case 18: //Waits 0.25 seconds and puts robot in neutral position + case 16: //Waits 0.25 seconds and puts robot in neutral position if(pathTimer.getElapsedTimeSeconds() > 0.25) { auto.init(); - setPathState(19); + setPathState(17); } break; - case 19: //Drives to chamber once action finishes + case 17: //Drives to chamber once action finishes auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.specimen3, true); - setPathState(20); + setPathState(18); break; - case 20: //Starts the Chamber State Machine + case 18: //Starts the Chamber State Machine if(pathTimer.getElapsedTimeSeconds() > 0) { - auto.startChamber2(); - setPathState(21); + auto.startChamber(); + setPathState(19); } break; - case 21: //Starts the Specimen State Machine + case 19: //Starts the Specimen State Machine if(auto.actionNotBusy()) { auto.startSpecimen(); - setPathState(22); + setPathState(20); } break; - case 22: //Begins the path for grab 2 & closes the claw once it reaches position and passes 0.75 seconds + case 20: //Begins the path for grab 2 & closes the claw once it reaches position and passes 0.75 seconds if(pathTimer.getElapsedTimeSeconds() > 0) { auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.grab4, true); - setPathState(23); + setPathState(21); } break; - case 23: + case 21: if(pathTimer.getElapsedTimeSeconds() > 2) { auto.claw.close(); - setPathState(24); + setPathState(22); } break; - case 24: //Waits 0.25 seconds and puts robot in neutral position + case 22: //Waits 0.25 seconds and puts robot in neutral position if(pathTimer.getElapsedTimeSeconds() > 0.25) { auto.init(); - setPathState(25); + setPathState(23); } break; - case 25: //Drives to chamber once action finishes + case 23: //Drives to chamber once action finishes auto.follower.setMaxPower(0.9); auto.follower.followPath(auto.specimen4, true); - setPathState(26); + setPathState(24); break; - case 26: //Starts the Chamber State Machine + case 24: //Starts the Chamber State Machine if(pathTimer.getElapsedTimeSeconds() > 0) { - auto.startChamber2(); - setPathState(27); + auto.startChamber(); + setPathState(25); } break; - case 27: //Park and End the autonomous + case 25: //Park and End the autonomous if(auto.actionNotBusy()) { auto.follower.setMaxPower(1); auto.follower.followPath(auto.park, true); auto.extend.toFull(); - setPathState(28); + setPathState(26); } break; - case 28: + case 26: if(pathTimer.getElapsedTimeSeconds() > 0.5) { auto.intake.pivotGround(); auto.intake.spinIn();