added arm contingency in case of broken dispenser wheel

team5115 · Dec 4, 2024 · ba96c7c · ba96c7c
1 parent 94446cd
commit ba96c7c
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 9 deletions.
diff --git a/src/main/java/frc/team5115/Constants.java b/src/main/java/frc/team5115/Constants.java
@@ -78,11 +78,11 @@ public static class SwerveConstants {
     public static class VisionConstants {
         public static final String cameraName = "FOV_120_Degrees";
         public static final double camYaw = Math.toRadians(+180.0);
-        public static final double camPitch = Math.toRadians(-37.0);
-        public static final double camRoll = Math.toRadians(0);
-        public static final double camZ = +0.57;
-        public static final double camX = -0.35;
-        public static final double camY = -0.06;
+        public static final double camPitch = Math.toRadians(-59.0);
+        public static final double camRoll = Math.toRadians(+180.0);
+        public static final double camZ = +0.10;
+        public static final double camX = +0.19;
+        public static final double camY = -0.19;
         public static final Transform3d robotToCam =
                 new Transform3d(camX, camY, camZ, new Rotation3d(camRoll, camPitch, camYaw));
     }

diff --git a/src/main/java/frc/team5115/commands/DriveCommands.java b/src/main/java/frc/team5115/commands/DriveCommands.java
@@ -36,9 +36,7 @@ public static Command quickScore(Dispenser dispenser, Arm arm, Drivetrain drivet
                         arm.prepareDispense(),
                         arm.waitForSetpoint(3.0),
                         drivetrain.alignPoseB(),
-                        dispenser.dispense(),
-                        dispenser.waitTillNoCanister(),
-                        dispenser.stop(),
+                        arm.dispenseContingency(),
                         drivetrain.alignPoseA(),
                         arm.stow())
                 .withInterruptBehavior(InterruptionBehavior.kCancelIncoming);

diff --git a/src/main/java/frc/team5115/subsystems/arm/Arm.java b/src/main/java/frc/team5115/subsystems/arm/Arm.java
@@ -32,7 +32,8 @@ public enum State {
         STOWED(+66.0),
         INTAKE(-22.0),
         DISPENSE(+135.0),
-        STACK(+180.0); // TODO add accurate stack position
+        STACK(+180.0), // TODO add accurate stack position
+        CONTINGENCY(+150.0); // TODO make sure angle is correct
 
         private final double position;
 
@@ -128,6 +129,10 @@ public Command intake() {
         return setState(State.INTAKE);
     }
 
+    public Command dispenseContingency() {
+        return setState(State.CONTINGENCY);
+    }
+
     // manipulate arm with joystic
     public void goAtVoltage(double speed) {
         io.setArmVoltage(speed * maxVolts);