Merge pull request #66 from strykeforce/statesDrivePractice

States drive practice

Author: mwitcpalek

.vscode/settings.json

@@ -57,5 +57,5 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable"
 }

src/main/deploy/choreo/CTofetchPSlow.traj

@@ -71,6 +71,8 @@ public void disabledPeriodic() {
     // if (!m_robotContainer.hasSwerveZeroed()) m_robotContainer.zeroSwerve();
 
     m_robotContainer.getAutoSwitch().checkSwitch();
+    m_robotContainer.updateCanivoreStatus();
+    m_robotContainer.updateCANErrorCount();
   }
 
   @Override

src/main/deploy/choreo/DTofetchPSlow.traj

@@ -10,6 +10,7 @@
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.SuppliedValueWidget;
@@ -43,14 +44,14 @@
 import frc.robot.commands.elevator.ZeroElevatorCommand;
 import frc.robot.commands.robotState.AutoReefCycleCommand;
 import frc.robot.commands.robotState.DepthChargeHealthCheckCommand;
-import frc.robot.commands.robotState.FloorAlgaeCommand;
+import frc.robot.commands.robotState.ForceBargeCommand;
+import frc.robot.commands.robotState.ForceLowFloorAlgaeCommand;
 import frc.robot.commands.robotState.ForceProcessorCommand;
 import frc.robot.commands.robotState.HPAlgaeCommand;
 import frc.robot.commands.robotState.InterruptAutoCommand;
 import frc.robot.commands.robotState.LockWheelsCommand;
 import frc.robot.commands.robotState.OperatorRumbleCommand;
 import frc.robot.commands.robotState.ReefCycleCommand;
-import frc.robot.commands.robotState.ScoreAlgaeCommand;
 import frc.robot.commands.robotState.SetScoreSideCommand;
 import frc.robot.commands.robotState.SetScoreSideRightCommand;
 import frc.robot.commands.robotState.SetScoringLevelCommand;
@@ -70,6 +71,7 @@
 import frc.robot.subsystems.algae.AlgaeIOFX;
 import frc.robot.subsystems.algae.AlgaeSubsystem;
 import frc.robot.subsystems.auto.AutoSwitch;
+import frc.robot.subsystems.battMon.BattMonHardware;
 import frc.robot.subsystems.battMon.BattMonSubsystem;
 import frc.robot.subsystems.biscuit.BiscuitIOFXS;
 import frc.robot.subsystems.biscuit.BiscuitSubsystem;
@@ -108,6 +110,7 @@ public class RobotContainer {
   private final AlgaeIOFX algaeIO;
   private final AlgaeSubsystem algaeSubsystem;
 
+  private final BattMonHardware batMonIO;
   private final BattMonSubsystem battMonSubsystem;
 
   private final BiscuitIOFXS biscuitIO;
@@ -142,6 +145,10 @@ public class RobotContainer {
 
   private final AutoSwitch autoSwitch;
 
+  private boolean canivoreStatus = false;
+  private int rCanErrors = RobotController.getCANStatus().receiveErrorCount;
+  private int tCanErrors = RobotController.getCANStatus().transmitErrorCount;
+
   private final XboxController xboxController = new XboxController(1);
   private final Joystick driveJoystick = new Joystick(0);
   private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
@@ -161,7 +168,8 @@ public RobotContainer() {
     algaeIO = new AlgaeIOFX();
     algaeSubsystem = new AlgaeSubsystem(algaeIO);
 
-    battMonSubsystem = new BattMonSubsystem();
+    batMonIO = new BattMonHardware();
+    battMonSubsystem = new BattMonSubsystem(batMonIO);
 
     biscuitIO = new BiscuitIOFXS();
     biscuitSubsystem = new BiscuitSubsystem(biscuitIO);
@@ -347,26 +355,29 @@ private void configureDriverBindings() {
                     coralSubsystem,
                     biscuitSubsystem,
                     algaeSubsystem),
-                () -> robotStateSubsystem.getIsAutoPlacing()));
+                () ->
+                    (robotStateSubsystem.getIsAutoPlacing()
+                        && robotStateSubsystem.getCoralLevel() != ScoringLevel.L1)));
 
     new JoystickButton(driveJoystick, Button.M_SWE.id)
         .onTrue(
-            new ScoreAlgaeCommand(
+            new ForceBargeCommand(
                 robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+    // new ForceBargeCommand(
+    //     robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem)); // for easy
+    // swapping
+    Command floorAlgaeCommand =
+        // new FloorAlgaeCommand(
+        //     robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem);
+        new ForceLowFloorAlgaeCommand(
+            robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem); // for easy
+    // swapping
     new JoystickButton(driveJoystick, Button.SWF_UP.id)
-        .onTrue(
-            new FloorAlgaeCommand(
-                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
-        .onFalse(
-            new FloorAlgaeCommand(
-                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+        .onTrue(floorAlgaeCommand)
+        .onFalse(floorAlgaeCommand);
     new JoystickButton(driveJoystick, Button.SWF_DWN.id)
-        .onTrue(
-            new FloorAlgaeCommand(
-                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
-        .onFalse(
-            new FloorAlgaeCommand(
-                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+        .onTrue(floorAlgaeCommand)
+        .onFalse(floorAlgaeCommand);
 
     // climb
     new JoystickButton(driveJoystick, Button.SWA.id)
@@ -636,6 +647,21 @@ private void configureMatchDashboard() {
         .withSize(1, 1)
         .withPosition(9, 0);
 
+    Shuffleboard.getTab("Match")
+        .addBoolean("CANivore Connected", () -> canivoreStatus)
+        .withSize(1, 1)
+        .withPosition(2, 2);
+
+    Shuffleboard.getTab("Match")
+        .addInteger("CAN Recieve Errors", () -> rCanErrors)
+        .withSize(1, 1)
+        .withPosition(3, 2);
+
+    Shuffleboard.getTab("Match")
+        .addInteger("CAN Transmit Errors", () -> tCanErrors)
+        .withSize(1, 1)
+        .withPosition(4, 2);
+
     // Shuffleboard.getTab("Match")
     // .addBoolean(
     // "Cams Connected",
@@ -799,6 +825,15 @@ public AutoSwitch getAutoSwitch() {
     return this.autoSwitch;
   }
 
+  public void updateCanivoreStatus() {
+    this.canivoreStatus = robotStateSubsystem.isCANivoreConnected();
+  }
+
+  public void updateCANErrorCount() {
+    rCanErrors = RobotController.getCANStatus().receiveErrorCount;
+    tCanErrors = RobotController.getCANStatus().transmitErrorCount;
+  }
+
   public void disableNoMotionCal() {
     if (RobotConstants.isComp) {
       swerve.disableNoMotionCal();