Drive practice changes

src/main/deploy/choreo/CTofetchPSlow.traj

@@ -16,7 +16,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"C.x", "val":3.6132737518457456}, "y":{"exp":"C.y", "val":3.1185381922643174}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.9328655004501343 m", "val":1.9328655004501345}, "y":{"exp":"1.1461 m", "val":1.1461}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[

src/main/deploy/choreo/DTofetchPSlow.traj

@@ -16,7 +16,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"D.x", "val":3.89906213509461}, "y":{"exp":"D.y", "val":2.953538192264317}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.9099534749984741 m", "val":1.909953474998474}, "y":{"exp":"1.1158 m", "val":1.1158}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[

src/main/deploy/choreo/KTofetchSlow.traj

@@ -16,7 +16,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":33, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.9099534749984741 m", "val":1.909953474998474}, "y":{"exp":"6.936121463775635 m", "val":6.936121463775635}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":11, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.6957741975784302 m", "val":1.6957741975784302}, "y":{"exp":"7.228199481964111 m", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[

src/main/deploy/choreo/LTofetchSlow.traj

@@ -16,7 +16,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.9328655004501343 m", "val":1.9328655004501345}, "y":{"exp":"6.905690670013428 m", "val":6.905690670013428}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":12, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[

src/main/deploy/choreo/fetchPToCSlow.traj

@@ -19,7 +19,7 @@
   "waypoints":[
     {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"3.2043471336364746 m", "val":3.2043471336364746}, "y":{"exp":"2.569765567779541 m", "val":2.569765567779541}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":41, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"C.x", "val":3.6132737518457456}, "y":{"exp":"C.y", "val":3.1185381922643174}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"C.x", "val":3.5561852518457457}, "y":{"exp":"C.y", "val":3.0196580097364216}, "heading":{"exp":"C.heading", "val":1.0471975511965976}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

src/main/deploy/choreo/fetchPToDSlow.traj

@@ -19,7 +19,7 @@
   "waypoints":[
     {"x":{"exp":"fetchP.x", "val":1.69577419757843}, "y":{"exp":"fetchP.y", "val":0.8236}, "heading":{"exp":"fetchP.heading", "val":0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"3.427126884460449 m", "val":3.427126884460449}, "y":{"exp":"2.2866694927215576 m", "val":2.2866694927215576}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":44, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"D.x", "val":3.89906213509461}, "y":{"exp":"D.y", "val":2.953538192264317}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"D.x", "val":3.84197363509461}, "y":{"exp":"D.y", "val":2.854658009736421}, "heading":{"exp":"D.heading", "val":1.0471975511965976}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

src/main/deploy/choreo/fetchToKSlow.traj

@@ -19,7 +19,7 @@
   "waypoints":[
     {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":36, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"3.6935606002807617 m", "val":3.6935606002807617}, "y":{"exp":"5.774015426635742 m", "val":5.774015426635742}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":42, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"K.x", "val":4.141549248154255}, "y":{"exp":"K.y", "val":5.238261807735684}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"K.x", "val":4.084460748154255}, "y":{"exp":"K.y", "val":5.3371419902635795}, "heading":{"exp":"K.heading", "val":5.235987755982989}, "intervals":27, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

src/main/deploy/choreo/fetchToLSlow.traj

@@ -19,7 +19,7 @@
   "waypoints":[
     {"x":{"exp":"fetch.x", "val":1.69577419757843}, "y":{"exp":"fetch.y", "val":7.228199481964111}, "heading":{"exp":"fetch.heading", "val":-0.942477796076938}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"3.410693407058716 m", "val":3.410693407058716}, "y":{"exp":"5.76054573059082 m", "val":5.76054573059082}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":45, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false},
-    {"x":{"exp":"L.x", "val":3.85576086490539}, "y":{"exp":"L.y", "val":5.073261807735684}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"L.x", "val":3.798672364905389}, "y":{"exp":"L.y", "val":5.1721419902635795}, "heading":{"exp":"L.heading", "val":5.235987755982989}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

src/main/deploy/choreo/startPToESlow.traj

@@ -19,7 +19,7 @@
   "waypoints":[
     {"x":{"exp":"startP.x", "val":7.1}, "y":{"exp":"startP.y", "val":2.97}, "heading":{"exp":"startP.heading", "val":3.141592653589793}, "intervals":32, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"5.436488628387451 m", "val":5.436488628387451}, "y":{"exp":"2.3468501567840576 m", "val":2.3468501567840576}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"E.x", "val":4.837096751845746}, "y":{"exp":"E.y", "val":2.813538192264317}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"E.x", "val":4.894185251845}, "y":{"exp":"E.y", "val":2.7146580097364}, "heading":{"exp":"E.heading", "val":2.0943951023931953}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

src/main/deploy/choreo/startToJSlow.traj

@@ -19,7 +19,7 @@
   "waypoints":[
     {"x":{"exp":"start.x", "val":7.1008875}, "y":{"exp":"start.y", "val":5.0756788}, "heading":{"exp":"start.heading", "val":3.141592653589793}, "intervals":29, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"5.7971601486206055 m", "val":5.7971601486206055}, "y":{"exp":"5.54304313659668 m", "val":5.54304313659668}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
-    {"x":{"exp":"J.x", "val":5.079583864905389}, "y":{"exp":"J.y", "val":5.098261807735684}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+    {"x":{"exp":"J.x", "val":5.1366723649053885}, "y":{"exp":"J.y", "val":5.197141990263579}, "heading":{"exp":"J.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[
     {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
     {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},

src/main/java/frc/robot/Robot.java

@@ -96,17 +96,16 @@ public void teleopInit() {
       m_autonomousCommand.cancel();
     }
     if (!m_robotContainer.hasElevatorZeroed()) m_robotContainer.zeroElevator();
+    else if (m_robotContainer.wasScoringCoral()) {
+      m_robotContainer.finishAuto();
+    } else {
+      m_robotContainer.stow();
+    }
 
     m_robotContainer.setIsAuto(false);
     m_robotContainer.setIsAutoPlacing(true);
     m_robotContainer.setScoringSide(ScoreSide.LEFT);
     m_robotContainer.disableNoMotionCal();
-
-    if (m_robotContainer.wasScoringCoral()) {
-      m_robotContainer.finishAuto();
-    } else {
-      m_robotContainer.stow();
-    }
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -424,30 +424,6 @@ private void configureOperatorBindings() {
         .onTrue(
             new ClimbPrepCommand(
                 robotStateSubsystem, climbSubsystem, elevatorSubsystem, biscuitSubsystem));
-
-    // Move biscuit
-    new Trigger((() -> xboxController.getRightY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-    new Trigger((() -> xboxController.getRightY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogBiscuitCommand(
-                biscuitSubsystem, Angle.ofBaseUnits(BiscuitConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldBiscuitCommand(biscuitSubsystem));
-
-    // Move elevator
-    new Trigger((() -> xboxController.getLeftY() < -RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountUp, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
-    new Trigger((() -> xboxController.getLeftY() > RobotConstants.kTestingDeadband))
-        .onTrue(
-            new JogElevatorCommand(
-                elevatorSubsystem, Angle.ofBaseUnits(ElevatorConstants.kJogAmountDown, Rotations)))
-        .onFalse(new HoldElevatorCommand(elevatorSubsystem));
   }
 
   private void configureTestOperatorBindings() {
@@ -621,6 +597,17 @@ private void configureMatchDashboard() {
         .withSize(1, 1)
         .withPosition(9, 1);
 
+    Shuffleboard.getTab("Match")
+        .add(
+            "Clear Coral",
+            new InstantCommand(
+                () -> {
+                  funnelSubsystem.clearCoral();
+                  coralSubsystem.setState(CoralSubsystem.CoralState.EMPTY);
+                }))
+        .withSize(1, 1)
+        .withPosition(9, 0);
+
     // Shuffleboard.getTab("Match")
     // .addBoolean(
     // "Cams Connected",

src/main/java/frc/robot/commands/auton/NonProcessorDeepAutonCommand.java

@@ -31,6 +31,7 @@ public class NonProcessorDeepAutonCommand extends SequentialCommandGroup
   private List<Character> NodeNames;
   private List<Integer> NodeLevels;
   private char startNode;
+  private boolean lastAlgae;
 
   public NonProcessorDeepAutonCommand(
       DriveSubsystem driveSubsystem,
@@ -46,7 +47,8 @@ public NonProcessorDeepAutonCommand(
       List<Character> NodeNames,
       List<Integer> NodeLevels,
       char startNode,
-      Boolean startScoreLeft,
+      boolean startScoreLeft,
+      boolean lastAlgae,
       Pose2d startPose) {
     addRequirements(
         driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
@@ -59,6 +61,7 @@ public NonProcessorDeepAutonCommand(
     this.NodeNames = NodeNames;
     this.NodeLevels = NodeLevels;
     this.startNode = startNode;
+    this.lastAlgae = lastAlgae;
 
     startPath =
         new DriveAutonServoCommand(
@@ -102,5 +105,6 @@ public void reassignAlliance() {
     pathHandler.setNodeNames(NodeNames);
     pathHandler.setNodeLevels(NodeLevels);
     pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(lastAlgae);
   }
 }

src/main/java/frc/robot/commands/auton/NonProcessorShallowAutonCommand.java

@@ -31,6 +31,7 @@ public class NonProcessorShallowAutonCommand extends SequentialCommandGroup
   private List<Character> NodeNames;
   private List<Integer> NodeLevels;
   private char startNode;
+  private boolean lastAlgae;
 
   public NonProcessorShallowAutonCommand(
       DriveSubsystem driveSubsystem,
@@ -46,7 +47,8 @@ public NonProcessorShallowAutonCommand(
       List<Character> NodeNames,
       List<Integer> NodeLevels,
       char startNode,
-      Boolean startScoreLeft,
+      boolean startScoreLeft,
+      boolean lastAlgae,
       Pose2d startPose) {
     addRequirements(
         driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
@@ -59,6 +61,7 @@ public NonProcessorShallowAutonCommand(
     this.NodeNames = NodeNames;
     this.NodeLevels = NodeLevels;
     this.startNode = startNode;
+    this.lastAlgae = lastAlgae;
 
     startPath =
         new DriveAutonServoCommand(
@@ -116,6 +119,7 @@ public void reassignAlliance() {
     pathHandler.setNodeNames(NodeNames);
     pathHandler.setNodeLevels(NodeLevels);
     pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(lastAlgae);
     // pathHandler.reassignAlliance();
   }
 }

src/main/java/frc/robot/commands/auton/NonProcessorShallowSlowAutonCommand.java

@@ -119,6 +119,7 @@ public void reassignAlliance() {
     pathHandler.setNodeNames(NodeNames);
     pathHandler.setNodeLevels(NodeLevels);
     pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(false);
     // pathHandler.reassignAlliance();
   }
 }

src/main/java/frc/robot/commands/auton/ProcessorShallowAutonCommand.java

@@ -31,6 +31,7 @@ public class ProcessorShallowAutonCommand extends SequentialCommandGroup
   private List<Character> NodeNames;
   private List<Integer> NodeLevels;
   private char startNode;
+  private boolean lastAlgae;
 
   public ProcessorShallowAutonCommand(
       DriveSubsystem driveSubsystem,
@@ -47,6 +48,7 @@ public ProcessorShallowAutonCommand(
       List<Integer> NodeLevels,
       char startNode,
       boolean startScoreLeft,
+      boolean lastAlgae,
       Pose2d startPose) {
     addRequirements(
         driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
@@ -59,6 +61,7 @@ public ProcessorShallowAutonCommand(
     this.NodeNames = NodeNames;
     this.NodeLevels = NodeLevels;
     this.startNode = startNode;
+    this.lastAlgae = lastAlgae;
 
     startPath =
         new DriveAutonServoCommand(
@@ -116,6 +119,7 @@ public void reassignAlliance() {
     pathHandler.setNodeNames(NodeNames);
     pathHandler.setNodeLevels(NodeLevels);
     pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(lastAlgae);
     // pathHandler.reassignAlliance();
   }
 }

src/main/java/frc/robot/commands/auton/ProcessorShallowSlowAutonCommand.java

@@ -116,6 +116,7 @@ public void reassignAlliance() {
     pathHandler.setNodeNames(NodeNames);
     pathHandler.setNodeLevels(NodeLevels);
     pathHandler.setStartNode(startNode);
+    pathHandler.setGetAlgaeLast(false);
     // pathHandler.reassignAlliance();
   }
 }

src/main/java/frc/robot/constants/AlgaeConstants.java

@@ -32,7 +32,7 @@ public class AlgaeConstants {
   public static final double kIntakingSpeed = -1;
 
   public static final double kHasAlgaeVelThreshold = 10;
-  public static final double kSuperCycleHasAlgaeVelThres = 50;
+  public static final double kSuperCycleHasAlgaeVelThres = 45;
   public static final double kHasAlgaeCounts = 2;
 
   // Example Talon FX Config

src/main/java/frc/robot/constants/BiscuitConstants.java

@@ -4,9 +4,10 @@ public class BiscuitConstants {
 
   // public static final double kZero = .37;
   public static final int talonID = 25;
-  public static final double kCloseEnough = 0.05;
-  public static final double kRezeroVelocityCloseEnough = 0.1; // FIXME: find a real value
-  public static final double kRezeroErrorCloseEnough = 0.1; // FIXME: find a real value
+  public static final double kCloseEnough = 1.3; // was 0.05
+  public static final double kInFunnel = 0.94; // TODO Put real numbers in here
+  public static final double kTooLow = 0.217;
+  public static final double kZeroVelThresh = 2;
   // public static final double kSafeToStowUpper = 40;
   // public static final double kSafeToStowLower = -5;