Yet another shot retuning session

notes/canIDs.md

@@ -44,7 +44,7 @@ Spindexer Agitator | 9 | X60-046
 Shooter Hood | 11 | X44-008
 Shooter Flywheel Leader (Left) | 12 | X60-048
 Shooter Flywheel Follower (Right) | 13 | X60-050
-intake Pivot | 14 | X44-017
+Intake Pivot | 14 | X44-017
 Turret Pivot | 15 | X44-001
 Climber | 16 | X60-049
 2nd Kicker Roller | 17 | X60-051

src/main/deploy/choreo/LBumptoDepot.traj

@@ -16,7 +16,7 @@
  },
  "params":{
   "waypoints":[
-    {"x":{"exp":"LBump.x", "val":3.631879568099976}, "y":{"exp":"LBump.y", "val":5.975247383117676}, "heading":{"exp":"LBump.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"StartingLBump.x", "val":3.631879568099976}, "y":{"exp":"StartingLBump.y", "val":5.975247383117676}, "heading":{"exp":"StartingLBump.heading", "val":1.5707963267948966}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"1.7998199462890625 m", "val":1.7998199462890625}, "y":{"exp":"Depot.y", "val":5.975247383117676}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
     {"x":{"exp":"Depot.x - 6 in", "val":0.551599767303467}, "y":{"exp":"Depot.y", "val":5.975247383117676}, "heading":{"exp":"Depot.heading", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
   "constraints":[

src/main/deploy/choreo/rebuiltChoreo.chor

@@ -113,16 +113,16 @@
    },
    "LBump":{
     "x":{
-     "exp":"3.6318795680999756 m",
-     "val":3.631879568099976
+     "exp":"2.8895323276519775 m",
+     "val":2.8895323276519775
     },
     "y":{
-     "exp":"Depot.y",
-     "val":5.975247383117676
+     "exp":"5.703888893127441 m",
+     "val":5.703888893127441
     },
     "heading":{
-     "exp":"90 deg",
-     "val":1.5707963267948966
+     "exp":"0 rad",
+     "val":0.0
     }
    },
    "LCleanup":{
@@ -405,6 +405,20 @@
      "val":1.5707963192945
     }
    },
+   "StartingLBump":{
+    "x":{
+     "exp":"3.6318795680999756 m",
+     "val":3.631879568099976
+    },
+    "y":{
+     "exp":"Depot.y",
+     "val":5.975247383117676
+    },
+    "heading":{
+     "exp":"90 deg",
+     "val":1.5707963267948966
+    }
+   },
    "StartingLTrench":{
     "x":{
      "exp":"4.40537166595459 m",

src/main/java/frc/robot/Autos.java

@@ -116,6 +116,10 @@ public enum Path {
     StartingRTrenchtoDisrupt("StartingRTrenchtoDisruptR", Action.INTAKE),
     StartingLTrenchtoDisrupt("StartingLTrenchtoDisruptL", Action.INTAKE),
 
+    LNeutralToLBump("LNeutralToLBump", Action.SCORE_AT_END),
+    LBumptoLCleanup("LBumpToLCleanup", Action.INTAKE),
+    LCleanupToLBump("LCleanupToLBump", Action.SCORE_AT_END),
+
     HubtoDepot("HubtoDepot", Action.DEPOT),
 
     PreDepottoDepot("PreDepottoDepot", Action.DEPOT),
@@ -754,6 +758,19 @@ public Command getLeftNeutralScoreTwice() {
         setLeftClimb());
   }
 
+  public Command getLeftBumpDoubleDipAuto() {
+    return createAuto(
+        "Left Double Dip Bump",
+        new Path[] {
+          Path.StartingLTrenchtoLNeutral,
+          Path.LNeutralToLBump,
+          Path.LBumptoLCleanup,
+          // Path.LCleanupToLBump
+          Path.EndWScoreLCleanuptoLPreTrench
+        },
+        Commands.none());
+  }
+
   public Command getTestAuto() {
     final AutoRoutine routine = factory.newRoutine("test auto");
     Path[] paths = {Path.RUNtoTEST, Path.RUNtoTEST, Path.RUNtoTEST, Path.RUNtoTEST};

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

@@ -673,6 +673,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
         .onTrue(
             Commands.runOnce(
                 () ->
+                    // swerve.setGyroYaw(
                     swerve.setYaw(
                         DriverStation.getAlliance().orElse(Alliance.Blue).equals(Alliance.Blue)
                             ? Rotation2d.kZero
@@ -731,11 +732,7 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
 
     driver
         .leftBumper()
-        .onTrue(
-            Commands.runOnce(
-                () ->
-                    shooter
-                        .resetTurretToCalculatedPosition())); // , intake.zeroPivotOffCancoder()));
+        .onTrue(shooter.resetTurretToCalculatedPosition()); // , intake.zeroPivotOffCancoder()));
 
     operator
         .leftBumper()
@@ -748,11 +745,9 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
     operator
         .rightStick()
         .onTrue(
-            Commands.runOnce(
-                () ->
-                    shooter
-                        .resetTurretToPosition(shooter::getCalculatedTurretRotations)
-                        .ignoringDisable(true)));
+            shooter
+                .resetTurretToPosition(shooter::getCalculatedTurretRotations)
+                .ignoringDisable(true));
 
     driver
         .rightBumper()
@@ -772,40 +767,40 @@ private void addControllerBindings(Indexer indexer, Shooter shooter, Intake inta
     // driver
     //     .leftBumper()
     //     .and(
-    new Trigger(AutoAim::targetInTurretDeadzone)
-        .and(() -> Superstructure.getState().isAScoreState())
-        .and(() -> !Superstructure.getState().isAFlowState())
-        .and(() -> !Superstructure.getPoseOverride())
-        .and(() -> superstructure.inScoringArea())
-        .whileTrue(
-            swerve.faceHubComp(
-                () ->
-                    -1
-                        * modifyJoystick(driver.getLeftY())
-                        * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
-                () ->
-                    -1
-                        * modifyJoystick(driver.getLeftX())
-                        * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
-                shooter::getTurretPosition));
-
-    new Trigger(AutoAim::targetInTurretDeadzone)
-        .and(() -> Superstructure.getState().isAFeedState())
-        .and(() -> !Superstructure.getState().isAFlowState())
-        .and(() -> !Superstructure.getPoseOverride())
-        .and(() -> !superstructure.inScoringArea())
-        .whileTrue(
-            swerve.faceFeedComp(
-                () ->
-                    -1
-                        * modifyJoystick(driver.getLeftY())
-                        * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
-                () ->
-                    -1
-                        * modifyJoystick(driver.getLeftX())
-                        * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
-                shooter::getTurretPosition,
-                () -> Superstructure.getFeedTarget()));
+    // new Trigger(AutoAim::targetInTurretDeadzone)
+    //     .and(() -> Superstructure.getState().isAScoreState())
+    //     .and(() -> !Superstructure.getState().isAFlowState())
+    //     .and(() -> !Superstructure.getPoseOverride())
+    //     .and(() -> superstructure.inScoringArea())
+    //     .whileTrue(
+    //         swerve.faceHubComp(
+    //             () ->
+    //                 -1
+    //                     * modifyJoystick(driver.getLeftY())
+    //                     * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
+    //             () ->
+    //                 -1
+    //                     * modifyJoystick(driver.getLeftX())
+    //                     * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
+    //             shooter::getTurretPosition));
+
+    // new Trigger(AutoAim::targetInTurretDeadzone)
+    //     .and(() -> Superstructure.getState().isAFeedState())
+    //     .and(() -> !Superstructure.getState().isAFlowState())
+    //     .and(() -> !Superstructure.getPoseOverride())
+    //     .and(() -> !superstructure.inScoringArea())
+    //     .whileTrue(
+    //         swerve.faceFeedComp(
+    //             () ->
+    //                 -1
+    //                     * modifyJoystick(driver.getLeftY())
+    //                     * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
+    //             () ->
+    //                 -1
+    //                     * modifyJoystick(driver.getLeftX())
+    //                     * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(),
+    //             shooter::getTurretPosition,
+    //             () -> Superstructure.getFeedTarget()));
 
     operator.povRight().onTrue(Commands.runOnce(() -> AutoAim.incrementFudgeFactor()));
     operator.povLeft().onTrue(Commands.runOnce(() -> AutoAim.decrementFudgeFactor()));
@@ -859,6 +854,9 @@ private void addAutos() {
     autoChooser.addOption("X44 Sysid", indexer.runX44Sysid());
 
     autoChooser.addOption("Right Neutral Outpost Score", autos.getRightNeutralOutpostScore());
+    autoChooser.addOption("Left Double Dip Bump", autos.getLeftBumpDoubleDipAuto());
+
+    autoChooser.addOption("spin", spinTest());
 
     haveAutosGenerated = true;
     System.out.println("Done generating autos");
@@ -950,6 +948,8 @@ public void robotPeriodic() {
             .getDistance(FieldUtils.getCurrentHubTranslation()));
     Logger.recordOutput(
         "AutoAim/Feed Target", FeedTargets.getFeedTarget(Superstructure.getFeedTarget()).getPose());
+
+    Logger.recordOutput("Wrapped gyro yaw", swerve.getRotation());
   }
 
   public void updateAlerts() {
@@ -1088,4 +1088,24 @@ public void testPeriodic() {}
 
   @Override
   public void testExit() {}
+
+  public Command spinTest() {
+    return Commands.sequence(
+        Commands.runOnce(() -> swerve.setGyroYaw(Rotation2d.kZero)),
+        swerve
+            .driveOpenLoopFieldRelative(
+                () ->
+                    new ChassisSpeeds(
+                        0, 0, SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed() / 4.0))
+            .withTimeout(20)
+        // .until()
+        //     ,
+        // // swerve
+        // //     .driveOpenLoopFieldRelative(
+        // //         () ->
+        // //             new ChassisSpeeds(0, 0,
+        // -SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()))
+        // //     .withTimeout(10)
+        );
+  }
 }