Merge pull request #109 from strykeforce/statefixes

mwitcpalek · web-flow · commit 68f6dab2b362 · 2023-04-10T19:36:52.000-04:00
State Champs Fixes
diff --git a/src/main/deploy/paths/pieceOneFetchBumpCubeTwo.toml b/src/main/deploy/paths/pieceOneFetchBumpCubeTwo.toml
@@ -12,7 +12,7 @@ target_yaw = 0.0 #degrees
 
 [end_pose]
     x = 6.77 #7.00
-    y = 2.3 #2.34 red:2.29 blue:2.34
+    y = 2.34 #2.3
     angle = 0.0
 
 [[internal_points]]
diff --git a/src/main/deploy/paths/pieceOneFetchCubeThreePath.toml b/src/main/deploy/paths/pieceOneFetchCubeThreePath.toml
@@ -11,7 +11,7 @@ target_yaw = 0.0 #degrees
     angle = 0.0
 
 [end_pose]
-    x = 6.3 # 7
+    x = 6.4 # 6.3
     y = 2.96 # cube 3
     angle = 0.0
 
diff --git a/src/main/deploy/paths/pieceOneFetchPathBump.toml b/src/main/deploy/paths/pieceOneFetchPathBump.toml
@@ -11,7 +11,7 @@ target_yaw = 0.0 #degrees
     angle = 0.0
 
 [end_pose]
-    x = 6.8 
+    x = 7.0 ##6.8
     y = 0.92 #0.92->1.02
     angle = 0.0
 
diff --git a/src/main/deploy/paths/piecePlaceOverLinePath.toml b/src/main/deploy/paths/piecePlaceOverLinePath.toml
@@ -11,7 +11,7 @@ target_yaw = 0.0 #degrees
     angle = 0.0
 
 [end_pose]
-    x = 6.5 #5.35 #test
+    x = 6.6 #6.5 #test
     y = 2.18
     angle = 0.0
 
diff --git a/src/main/deploy/paths/pieceTwoFetchCubeFourPath.toml b/src/main/deploy/paths/pieceTwoFetchCubeFourPath.toml
@@ -16,5 +16,5 @@ target_yaw = 0.0 #degrees
     angle = 0.0
 
 [[internal_points]]
-    x = 4.5
-    y = 4.36
+    x = 2.75
+    y = 4.5 #4.36
diff --git a/src/main/deploy/paths/pieceTwoPlaceCubeFourPath.toml b/src/main/deploy/paths/pieceTwoPlaceCubeFourPath.toml
@@ -7,14 +7,14 @@ target_yaw = 0.0 #degrees
 
 [start_pose] 
     x = 6.90 # 7
-    y = 4.58 # cube 4
+    y = 4.58 # cube 4 #4.58
     angle = 180.0
 
 [end_pose]
     x = 2.12 # 7
-    y = 4.42 # cube 4 place mid  3.82
+    y = 4.42 # cube 4 place mid  3.82 #4.42
     angle = 180.0
 
 [[internal_points]]
     x = 4.5
-    y = 4.85
+    y = 4.85 #4.85
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -82,7 +82,7 @@ public static final class RobotStateConstants {
     public static final Pose2d kCubeThreeAutoPickup =
         new Pose2d(new Translation2d(7.07, 3.36), new Rotation2d());
     public static final Pose2d kCubeTwoAutoPickup =
-        new Pose2d(new Translation2d(6.9, 2.14), new Rotation2d());
+        new Pose2d(new Translation2d(6.9, 2.18), new Rotation2d());
     public static final Pose2d kCubeOneAutoPickup =
         new Pose2d(new Translation2d(6.9, 0.92), new Rotation2d());
 
@@ -860,18 +860,19 @@ public static class CompConstants {
     public static final double kWheelDiameterInches = 3.0 * (496 / 500.0);
 
     // Elbow
-    public static final int kElbowZeroTicks = 700; // 730
+    public static final int kElbowZeroTicks = 705; // 700
 
     // Shoulder
     public static final double kShoulderMainZeroTicks = 2050;
     public static final double kShoulderFollowerZeroTicks = 2057;
 
     // Intake
-    public static final int kIntakeZeroTicks = 2790; // 2440 ->2540
+    public static final int kIntakeZeroTicks = 3_600; // 2440 ->2540
     public static final double kExtendPosTicks = -2_100;
 
     // Hand
-    public static final double kHandZeroTicks = 1230; // 686 ->976
+    // 273 for a tooth on the hand
+    public static final double kHandZeroTicks = 684; // 686 ->976
   }
 
   public static class ProtoConstants {
diff --git a/src/main/java/frc/robot/commands/auto/MiddleToDockWithMobility.java b/src/main/java/frc/robot/commands/auto/MiddleToDockWithMobility.java
@@ -65,7 +65,7 @@ public MiddleToDockWithMobility(
         new ReleaseGamepieceCommand(handSubsystem, robotStateSubsystem),
         new WaitCommand(1.0),
         firstPath,
-        new WaitCommand(0.25),
+        new WaitCommand(0.75),
         new ParallelRaceGroup(
             new AutoWaitForMatchTimeCommand(0.1),
             new SequentialCommandGroup(
diff --git a/src/main/java/frc/robot/commands/auto/ThreePieceBumpAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/ThreePieceBumpAutoCommandGroup.java
@@ -83,7 +83,7 @@ public ThreePieceBumpAutoCommandGroup(
                     robotStateSubsystem,
                     RobotStateConstants.kCubeTwoAutoPickup,
                     visionSubsystem)
-                .withTimeout(0.9)),
+                .withTimeout(0.75)),
         new ParallelDeadlineGroup(
             secondPath,
             new SetGamePieceCommand(robotStateSubsystem, GamePiece.CUBE),
@@ -101,7 +101,7 @@ public ThreePieceBumpAutoCommandGroup(
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
             new SetVisionUpdateCommand(driveSubsystem, false),
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem)),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true)),
         // Cube 2
         new ParallelDeadlineGroup(
             thirdPath,
@@ -115,7 +115,7 @@ public ThreePieceBumpAutoCommandGroup(
                     robotStateSubsystem,
                     RobotStateConstants.kCubeOneAutoPickup,
                     visionSubsystem)
-                .withTimeout(0.9)),
+                .withTimeout(0.75)),
         new ParallelDeadlineGroup(
             fourthPath,
             new SetGamePieceCommand(robotStateSubsystem, GamePiece.CUBE),
@@ -132,7 +132,7 @@ public ThreePieceBumpAutoCommandGroup(
                     .withTimeout(0.70),
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, false),
             new ClearGamePieceCommand(robotStateSubsystem))
         /*,new ParallelCommandGroup(
             fourthPath,
diff --git a/src/main/java/frc/robot/commands/auto/ThreePieceBumpFallbackAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/ThreePieceBumpFallbackAutoCommandGroup.java
@@ -89,7 +89,7 @@ public ThreePieceBumpFallbackAutoCommandGroup(
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
             new SetVisionUpdateCommand(driveSubsystem, false),
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem)),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true)),
         // Cube 2
         new ParallelDeadlineGroup(
             thirdPath,
@@ -110,7 +110,7 @@ public ThreePieceBumpFallbackAutoCommandGroup(
                     .withTimeout(0.70),
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, false),
             new ClearGamePieceCommand(robotStateSubsystem))
         /*,new ParallelCommandGroup(
             fourthPath,
diff --git a/src/main/java/frc/robot/commands/auto/ThreePieceBumpLowAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/ThreePieceBumpLowAutoCommandGroup.java
@@ -101,7 +101,7 @@ public ThreePieceBumpLowAutoCommandGroup(
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
             new SetVisionUpdateCommand(driveSubsystem, false),
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem)),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true)),
         // Cube 2
         new ParallelDeadlineGroup(
             thirdPath,
@@ -132,7 +132,7 @@ public ThreePieceBumpLowAutoCommandGroup(
                     .withTimeout(0.70),
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, false),
             new ClearGamePieceCommand(robotStateSubsystem))
         /*,new ParallelCommandGroup(
             fourthPath,
diff --git a/src/main/java/frc/robot/commands/auto/ThreePieceSmoothAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/ThreePieceSmoothAutoCommandGroup.java
@@ -83,7 +83,7 @@ public ThreePieceSmoothAutoCommandGroup(
                     robotStateSubsystem,
                     RobotStateConstants.kCubeThreeAutoPickup,
                     visionSubsystem)
-                .withTimeout(0.9)),
+                .withTimeout(0.75)),
         new ParallelDeadlineGroup(
             secondPath,
             new SetGamePieceCommand(robotStateSubsystem, GamePiece.CUBE),
@@ -101,7 +101,7 @@ public ThreePieceSmoothAutoCommandGroup(
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
             new SetVisionUpdateCommand(driveSubsystem, false),
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem)),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true)),
         // Cube 2
         new ParallelDeadlineGroup(
             thirdPath,
@@ -115,7 +115,7 @@ public ThreePieceSmoothAutoCommandGroup(
                     robotStateSubsystem,
                     RobotStateConstants.kCubeFourAutoPickup,
                     visionSubsystem)
-                .withTimeout(0.9)),
+                .withTimeout(0.75)),
         new ParallelDeadlineGroup(
             fourthPath,
             new SetGamePieceCommand(robotStateSubsystem, GamePiece.CUBE),
@@ -132,7 +132,7 @@ public ThreePieceSmoothAutoCommandGroup(
                     .withTimeout(0.70),
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, false),
             new ClearGamePieceCommand(robotStateSubsystem)));
   }
 
diff --git a/src/main/java/frc/robot/commands/auto/TwoPieceBumpWithDockAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/TwoPieceBumpWithDockAutoCommandGroup.java
@@ -88,7 +88,7 @@ public TwoPieceBumpWithDockAutoCommandGroup(
                 () -> !visionSubsystem.isCameraWorking())),
         new ParallelCommandGroup(
             new SetVisionUpdateCommand(driveSubsystem, false),
-            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem)),
+            new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true)),
         new ParallelRaceGroup(
             new AutoWaitForMatchTimeCommand(0.1),
             new SequentialCommandGroup(
diff --git a/src/main/java/frc/robot/commands/auto/TwoPieceLvl3AutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/TwoPieceLvl3AutoCommandGroup.java
@@ -66,7 +66,7 @@ public TwoPieceLvl3AutoCommandGroup(
                 new PastXPositionCommand(
                     robotStateSubsystem, driveSubsystem, Constants.AutonConstants.kPastXPosition),
                 new ManualScoreCommand(robotStateSubsystem, armSubsystem, handSubsystem))),
-        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true),
         new ParallelCommandGroup(
             new ClearGamePieceCommand(robotStateSubsystem),
             new SetVisionUpdateCommand(driveSubsystem, true)));
diff --git a/src/main/java/frc/robot/commands/auto/TwoPieceLvl3BumpAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/TwoPieceLvl3BumpAutoCommandGroup.java
@@ -69,7 +69,7 @@ public TwoPieceLvl3BumpAutoCommandGroup(
                 new PastXPositionCommand(
                     robotStateSubsystem, driveSubsystem, Constants.AutonConstants.kPastXPosition),
                 new ManualScoreCommand(robotStateSubsystem, armSubsystem, handSubsystem))),
-        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true),
         new ParallelCommandGroup(
             new ClearGamePieceCommand(robotStateSubsystem),
             new SetVisionUpdateCommand(driveSubsystem, true)));
diff --git a/src/main/java/frc/robot/commands/auto/TwoPieceMiddleBalanceAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/TwoPieceMiddleBalanceAutoCommandGroup.java
@@ -79,7 +79,7 @@ public TwoPieceMiddleBalanceAutoCommandGroup(
             thirdPath,
             new InterruptDriveCommand(driveSubsystem),
             () -> driveSubsystem.getDriveState() == DriveStates.AUTO_DRIVE_FAILED),
-        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true),
         new ParallelRaceGroup(
             new AutoWaitForMatchTimeCommand(0.1),
             new SequentialCommandGroup(
diff --git a/src/main/java/frc/robot/commands/auto/TwoPieceWithDockAutoCommandGroup.java b/src/main/java/frc/robot/commands/auto/TwoPieceWithDockAutoCommandGroup.java
@@ -73,7 +73,7 @@ public TwoPieceWithDockAutoCommandGroup(
                 new PastXPositionCommand(
                     robotStateSubsystem, driveSubsystem, Constants.AutonConstants.kPastXPosition),
                 new ManualScoreCommand(robotStateSubsystem, armSubsystem, handSubsystem))),
-        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem),
+        new ShootGamepieceCommand(handSubsystem, robotStateSubsystem, true),
         new ParallelRaceGroup(
             new AutoWaitForMatchTimeCommand(0.1),
             new SequentialCommandGroup(
diff --git a/src/main/java/frc/robot/commands/drive/AutoPickupCommand.java b/src/main/java/frc/robot/commands/drive/AutoPickupCommand.java
@@ -129,9 +129,10 @@ public boolean isFinished() {
   public void end(boolean interrupted) {
     driveSubsystem.drive(0, 0, 0);
     logger.info(
-        "Done AutoPickup y error {} | x error {} | END POSE {}",
+        "Done AutoPickup y error {} | x error {} | END POSE {} | interrupted: {}",
         Math.abs(endPose.getY() - driveSubsystem.getPoseMeters().getY()),
         Math.abs(endPose.getX() - driveSubsystem.getPoseMeters().getX()),
-        driveSubsystem.getPoseMeters());
+        driveSubsystem.getPoseMeters(),
+        interrupted);
   }
 }
diff --git a/src/main/java/frc/robot/commands/robotState/ShootGamepieceCommand.java b/src/main/java/frc/robot/commands/robotState/ShootGamepieceCommand.java
@@ -8,17 +8,19 @@
 public class ShootGamepieceCommand extends CommandBase {
   private RobotStateSubsystem robotStateSubsystem;
   private HandSubsystem handSubsystem;
+  private boolean doStow = false;
 
   public ShootGamepieceCommand(
-      HandSubsystem handSubsystem, RobotStateSubsystem robotStateSubsystem) {
+      HandSubsystem handSubsystem, RobotStateSubsystem robotStateSubsystem, boolean doStow) {
     this.robotStateSubsystem = robotStateSubsystem;
     this.handSubsystem = handSubsystem;
+    this.doStow = doStow;
     addRequirements(handSubsystem);
   }
 
   @Override
   public void initialize() {
-    robotStateSubsystem.toShootCube();
+    robotStateSubsystem.toShootCube(doStow);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -147,16 +147,16 @@ public void retractToPickupFromIntake() {
     retractClosedLoop(IntakeConstants.kPickupPosTicks);
   }
 
-  public void startIntaking(boolean doHolding) {
+  public void startIntaking(boolean isAuton) {
     logger.info("Start intaking");
-    this.doHolding = doHolding;
+    // this.doHolding = isAuton;
     currIntakeState = IntakeState.INTAKING;
-    intakeOpenLoop(IntakeConstants.kIntakeSpeed);
+    if (isAuton) intakeOpenLoop(IntakeConstants.kIntakeSpeed);
     extendClosedLoop();
   }
 
   public void startIntaking() {
-    startIntaking(false);
+    startIntaking(true);
   }
 
   public void startEjecting() {
diff --git a/src/main/java/frc/robot/subsystems/RobotStateSubsystem.java b/src/main/java/frc/robot/subsystems/RobotStateSubsystem.java
@@ -46,6 +46,7 @@ public class RobotStateSubsystem extends MeasurableSubsystem {
   private Timer intakeTimerOffset = new Timer();
   private boolean isReleaseDelayTimerRunning = false;
   private double currPoseX;
+  private boolean doStow = false;
   private double desiredPoseX;
   private boolean isAutoStageFinished = false;
   private boolean isAutoPlacing = false;
@@ -167,7 +168,7 @@ public void toIntake() {
     logger.info("{} -->  TO_INTAKE_STAGE", currRobotState);
     currRobotState = RobotState.TO_INTAKE_STAGE;
     currentAxis = CurrentAxis.INTAKE;
-    intakeSubsystem.startIntaking();
+    intakeSubsystem.startIntaking(isAuto);
     if (armSubsystem.getCurrState() != ArmState.STOW && isAuto) armSubsystem.toStowPos();
     // handSubsystem.stowHand(HandConstants.kCubeGrabbingPosition);
   }
@@ -275,6 +276,7 @@ public void toManualShelf() {
   public void toReleaseGamepiece() {
     logger.info("{} -> RELEASE_GAME_PIECE", currRobotState);
     currRobotState = RobotState.RELEASE_GAME_PIECE;
+    doStow = true;
     handSubsystem.runRollers(HandConstants.kRollerDrop);
     handSubsystem.open();
     rgbLightsSubsystem.setOff();
@@ -286,7 +288,8 @@ public void toReleaseGamepiece() {
     logger.info("Score Pos X: {}", scorePosXIntial);
   }
 
-  public void toShootCube() {
+  public void toShootCube(boolean doStow) {
+    this.doStow = doStow;
     logger.info("{} -> RELEASE_GAME_PIECE", currRobotState);
     currRobotState = RobotState.RELEASE_GAME_PIECE;
     handSubsystem.runRollers(HandConstants.kRollerShoot);
@@ -558,6 +561,7 @@ public void periodic() {
               hasIntakeDelayPassed = false;
               currentAxis = CurrentAxis.HAND;
               if (isAuto) armSubsystem.setArmFastStow(false);
+              intakeSubsystem.intakeOpenLoop(IntakeConstants.kIntakeSpeed);
               handSubsystem.openIntake();
               break;
             }
@@ -820,8 +824,9 @@ public void periodic() {
           isReleaseDelayTimerRunning = true;
           logger.info("Started release timer");
         } else if ((isReleaseDelayTimerRunning
-                && releaseDelayTimer.hasElapsed(RobotStateConstants.kReleaseDelayTime))
-            || isAuto) {
+                && releaseDelayTimer.hasElapsed(RobotStateConstants.kReleaseDelayTime)
+                && doStow)
+            || isAuto && doStow) {
           isReleaseDelayTimerRunning = false;
           releaseDelayTimer.stop();
           releaseDelayTimer.reset();
