Merge pull request #123 from strykeforce/drivePracticeTweaks

Drive Practice Tweaks

Author: mwitcpalek

src/main/java/frc/robot/Constants.java

@@ -226,6 +226,7 @@ public static Translation2d[] getWheelLocationMeters() {
     public static final double kAutoBalanceStableCount = 10;
     public static final double kAutoBalanceStartTimerThresholdDeg = 5;
     public static final double kAutoBalanceEnableGyroThresholdDegrees = 8;
+    public static final int kMaxAutoBalanceRetries = 3;
 
     public static final double kAutoBalanceSlowDriveVel = 0.35; // 0.6
     public static final double kAutoBalanceRecoveryDriveVel = 0.25;

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

@@ -73,6 +73,7 @@
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.AutoSwitch;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.DriveSubsystem.DriveStates;
 import frc.robot.subsystems.ElbowSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.HandSubsystem;
@@ -203,6 +204,10 @@ public void autoStowTele() {
     }
   }
 
+  public void driveSubsystemTele() {
+    driveSubsystem.setDriveState(DriveStates.IDLE);
+  }
+
   public void setDisabled(boolean isDisabled) {
     robotStateSubsystem.setDisabled(isDisabled);
   }

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -73,6 +73,7 @@ public class DriveSubsystem extends MeasurableSubsystem {
   private double yCalc;
   private double omegaCalc;
   private RobotStateSubsystem robotStateSubsystem;
+  private int numAutoBalanceRetries = 0;
 
   // HEALTHCHECK FIX
   @HealthCheck
@@ -498,6 +499,7 @@ public void setAutoDriving(boolean autoDrive) {
 
   public void autoBalance(boolean isOnAllianceSide) {
     // On alliance side of charge station, drive Positive X
+    numAutoBalanceRetries = 0;
     autoBalanceAvgCount = 0;
     sumRoll = 0;
     avgStartingRoll = 0;
@@ -702,6 +704,8 @@ public void periodic() {
           if (!recoveryStartingPitchSet) {
             recoveryStartingPitch = getGyroPitch();
             recoveryStartingPitchSet = true;
+            logger.info("Recovery Start Pitch: {}", recoveryStartingPitch);
+            numAutoBalanceRetries++;
           }
           // if (isOnAllianceSide)
           if ((Math.abs(tempRoll - getGyroPitch()) > 2)
@@ -741,8 +745,13 @@ public void periodic() {
                 "AutoBalance Stop: Gyro Roll: {}, trigger Difference: {}",
                 getGyroPitch(),
                 Math.abs(avgStartingRoll) - Math.abs(getGyroPitch()));
-            logger.info("{} -> AUTO_BALANCE_RECOVERY", currDriveState);
-            currDriveState = DriveStates.AUTO_BALANCE_RECOVERY;
+            if (numAutoBalanceRetries < DriveConstants.kMaxAutoBalanceRetries) {
+              logger.info("{} -> AUTO_BALANCE_RECOVERY", currDriveState);
+              currDriveState = DriveStates.AUTO_BALANCE_RECOVERY;
+            } else {
+              logger.info("{} -> AUTO_BALANCE_FINISHED", currDriveState);
+              currDriveState = DriveStates.AUTO_BALANCE_FINISHED;
+            }
 
             autoBalanceRecoveryTimer.reset();
             autoBalanceRecoveryTimer.start();

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -47,6 +47,7 @@ public class VisionSubsystem extends MeasurableSubsystem {
   private DigitalInput[][] dios = {diosHigh, diosLow};
   private WallEyeResult[][] results = {{}, {}};
   private Pose2d[] camPoses = {new Pose2d(), new Pose2d()};
+  private Pose2d[] currCamPoses = {new Pose2d(), new Pose2d()};
   private int[] updates = {0, 0};
   private int[] numTags = {0, 0};
   private double[] camDelay = {0, 0};
@@ -128,7 +129,8 @@ public void periodic() {
         for (int j = 0; j < numCams[i]; ++j) {
           numTags[i] = results[i][j].getNumTags();
 
-          camPoses[i] =
+          // Save current camera pose to temp variable until check that it's valid
+          currCamPoses[i] =
               new Pose2d(
                   new Translation2d(
                           results[i][j].getCameraPose().getX(),
@@ -176,7 +178,9 @@ private void handleCameraFilter(
       for (WallEyeResult res : results) {
 
         if (res.getCameraPose().getX() > 1000.0) continue;
-
+        camPoses =
+            currCamPoses; // If current camera pose = valid then overwrite last valid cam pose with
+        // it
         Pose2d camPose =
             new Pose2d(
                 new Translation2d(res.getCameraPose().getX(), res.getCameraPose().getY())
@@ -242,7 +246,7 @@ private void handleCameraFilter(
   public void handleVision(WallEyeResult res, int camNum, Pose2d camPose) {
     numUpdateForReset++;
     suppliedCamPose[camNum] = camPose;
-    System.out.println(suppliedCamPose[camNum]);
+    // System.out.println(suppliedCamPose[camNum]);
 
     driveSubsystem.updateOdometryWithVision(camPose, res.getTimeStamp() / 1000000);
   }
@@ -318,7 +322,8 @@ public void resetOdometry(Pose2d errorCheckPose, boolean isBlueAlliance) {
           new Pose2d(
               new Translation2d(camPoses[0].getX(), camPoses[0].getY()),
               driveSubsystem.getGyroRotation2d()));
-    } else {
+    } else if (!((camPoses[0].getX() == 0 && camPoses[0].getY() == 0)
+        || (camPoses[0].getX() == 2767 && camPoses[0].getY() == 2767))) {
       // double tempX = (driveSubsystem.getPoseMeters().getX() + cameraPose.getX()) / 2;
       double tempY = (driveSubsystem.getPoseMeters().getY() + camPoses[0].getY()) / 2;
       // logger.info("TempX: {}, TempY: {}", tempX, tempY);
@@ -335,6 +340,8 @@ public void resetOdometry(Pose2d errorCheckPose, boolean isBlueAlliance) {
           tempPose,
           driveSubsystem.getPoseMeters());
       driveSubsystem.resetOdometry(tempPose);
+    } else {
+      logger.info("Not resetting odometry, last cam pose: {}", camPoses[0]);
     }
   }