Merge pull request #70 from strykeforce/barge-buffer-zone

Opponent Steal and Auto Barge Improvements

Author: mwitcpalek

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

@@ -5,7 +5,7 @@ public class BiscuitConstants {
   // public static final double kZero = .37;
   public static final int talonID = 25;
   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 kInFunnel = 0.94;
   public static final double kTooLow = 0.217;
   public static final double kZeroVelThresh = 2;
   // public static final double kSafeToStowUpper = 40;

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -119,13 +119,13 @@ public RobotStateSubsystem(
     this.tagAlignSubsystem = tagAlignSubsystem;
     this.visionSubsystem = visionSubsystem;
     this.bargeAlignSubsystem = bargeAlignSubsystem;
-    this.canBus = new CANBus();
+    this.canBus = new CANBus("CAN FD 25-1");
 
     ledSubsystem.setState(LEDStates.NORMAL);
   }
 
   public boolean isCANivoreConnected() {
-    CANBusStatus status = canBus.getStatus("CAN FD 25-1");
+    CANBusStatus status = canBus.getStatus();
     return status.Status.isOK();
   }
 
@@ -344,6 +344,14 @@ private boolean needSafeAlgaeTransfer(RobotStates nextState) {
 
     return false;
   }
+  // Function made so I don't cram everything into a function call in toReefAlign
+  private boolean justAlgae() {
+    boolean blueSide = driveSubsystem.getPoseMeters().getX() < DriveConstants.kFieldMaxX / 2;
+
+    return !hasCoral()
+        || (blueSide && allianceColor == Alliance.Red)
+        || (!blueSide && allianceColor == Alliance.Blue);
+  }
 
   public void toStow() {
     visionSubsystem.setYawUpdateCamera(-1);
@@ -501,7 +509,11 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
       return;
     }
 
+    boolean ignoreCoralScoring = isAutoPlacing && getAlgaeOnCycle && scoreSide == ScoreSide.RIGHT;
+
     if (drive) {
+      // TODO Test without a coral in the robot and we don't want algae
+      tagAlignSubsystem.setJustAlgae(justAlgae());
       tagAlignSubsystem.start(
           allianceColor,
           scoringLevel,
@@ -516,8 +528,6 @@ private void toReefAlign(boolean getAlgae, boolean drive) {
         tagAlignSubsystem.getCurRadius() > TagServoingConstants.kAlgaeStopXDriveRadius
             || tagAlignSubsystem.getState() != TagAlignStates.DRIVE;
 
-    boolean ignoreCoralScoring = isAutoPlacing && getAlgaeOnCycle && scoreSide == ScoreSide.RIGHT;
-
     if (wantAlgae && !algaeSafe) {
       algaeSubsystem.intakeAlgae();
       setBiscuitTransfer(RobotConstants.kPrestageAlgaeSetpoint, true);
@@ -693,6 +703,7 @@ public void toScoreAlgae() {
         if (needSafeAlgaeTransfer(RobotStates.BARGE_ALGAE)) {
           return;
         }
+
         double poseX = driveSubsystem.getPoseMeters().getX();
         isBargeSafe =
             poseX > RobotStateConstants.kRedBargeSafeX

src/main/java/frc/robot/subsystems/tagAlign/BargeAlignSubsystem.java

@@ -145,6 +145,17 @@ public void periodic() {
           terminate();
         }
       }
+      case REVERSE -> {
+        if (isOnBlueSide) {
+          driveSubsystem.move(
+              -vX, getYStickReading(), BargeAlignConstants.kBlueRaiseElevatorX, true);
+          if (shouldRaiseElevator()) {
+            setState(BargeAlignStates.RAISE_ELEV);
+          }
+        } else {
+          driveSubsystem.move(vX, getYStickReading(), BargeAlignConstants.kRedRaiseElevatorX, true);
+        }
+      }
       case FINISHED -> {}
     }
   }
@@ -158,6 +169,7 @@ public enum BargeAlignStates {
     // INIT,
     DRIVE,
     RAISE_ELEV,
+    REVERSE,
     FINISHED
   }
 }

src/main/java/frc/robot/subsystems/tagAlign/TagAlignSubsystem.java

@@ -51,6 +51,7 @@ public class TagAlignSubsystem extends MeasurableSubsystem {
   private double coralOffset = 0;
   private double noProgressCounts = 0;
   private double yAutoOffset = 0;
+  private boolean justAlgae = false;
 
   public TagAlignSubsystem(DriveSubsystem driveSubsystem, VisionSubsystem visionSubsystem) {
     this.driveSubsystem = driveSubsystem;
@@ -80,6 +81,10 @@ public void setProceedToAlign(boolean proceed) {
     this.proceedToAlign = proceed;
   }
 
+  public void setJustAlgae(boolean justAlgae) {
+    this.justAlgae = justAlgae;
+  }
+
   public boolean yErrorSmall() {
     return curState == TagAlignStates.DONE
         || finalDrive
@@ -285,6 +290,7 @@ public void startAuto(
     isAuto = true;
     this.yAutoOffset = yAutoOffset;
     setup(alliance, level, scoreLeft, algae);
+    setJustAlgae(false);
     tagAlign();
   }
 
@@ -406,10 +412,14 @@ public void periodic() {
               }
             }
             case TAG_ALIGN -> {
-              if ((
-                  /*isAuto ? true :*/ FastMath.abs(alignX.getError()) < driveXCloseEnough)
-                  && FastMath.abs(alignY.getError()) < driveYCloseEnough) {
+              if (justAlgae && (FastMath.abs(alignY.getError()) < driveYCloseEnough)) {
                 finalDrive = true;
+              } else {
+                if ((
+                    /*isAuto ? true :*/ FastMath.abs(alignX.getError()) < driveXCloseEnough)
+                    && FastMath.abs(alignY.getError()) < driveYCloseEnough) {
+                  finalDrive = true;
+                }
               }
               if (finalDrive
                   && driveSubsystem.getAvgDriveCurrent()
@@ -430,9 +440,11 @@ public void periodic() {
             }
           }
         }
-
+        // TODO Find Proper Number
         if (finalDrive) {
-          if (isAuto) {
+          if (justAlgae) {
+            vX = (vX > 0.35) ? vX : 0.35;
+          } else if (isAuto) {
             if (scoreLeft) {
               vX = 0.25;
             } else {