Merge pull request #120 from strykeforce/frost-hotfix

Frost hotfix

Author: mwitcpalek

src/main/deploy/paths/pieceTwoToDockPath.toml

@@ -1,7 +1,7 @@
 max_velocity = 1.5 #m/s
 max_acceleration = 2.0 #m/s
 start_velocity = 0.0 #m/s
-end_velocity = 1.0 #m/s
+end_velocity = 1.1 #m/s
 is_reversed = false
 target_yaw = 0.0 #degrees
 

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

@@ -229,7 +229,7 @@ public static Translation2d[] getWheelLocationMeters() {
 
     public static final double kAutoBalanceSlowDriveVel = 0.35; // 0.6
     public static final double kAutoBalanceRecoveryDriveVel = 0.25;
-    public static final double kAutoBalanceFinalDriveVel = 1.0; // 0.5 0.75
+    public static final double kAutoBalanceFinalDriveVel = 1.1; // 0.5 0.75
     public static final double kAutoBalanceSlowdownTimeSec = 1.0; // 1.3-> 1.15
     public static final double kAutoBalanceStopThresholdDegrees = 1.5; // 1 0.6 1.5
     public static final double kAutoBalanceEdgeTriggerThreshold = 3; // 5

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

@@ -74,7 +74,7 @@ public void disabledPeriodic() {
     if (!mappedDriveJoystick) {
       String joystick = DriverStation.getJoystickName(0);
       if (lastJoystick != joystick) {
-        System.out.println("Joystick Name: " + joystick);
+        // System.out.println("Joystick Name: " + joystick);
         mappedDriveJoystick = m_robotContainer.configureDriverButtonBindings();
       }
       lastJoystick = joystick;

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

@@ -58,6 +58,7 @@
 import frc.robot.commands.robotState.ShuffleBoardHealthCheckIntakeCommandGroup;
 import frc.robot.commands.robotState.StowRobotCommand;
 import frc.robot.commands.robotState.ToggleIntakeCommand;
+import frc.robot.commands.robotState.ToggleVirtualSwitchCommand;
 import frc.robot.commands.shoulder.ShoulderSpeedCommand;
 import frc.robot.commands.shoulder.StopTwistCommand;
 import frc.robot.commands.shoulder.TwistShoulderCommand;
@@ -277,7 +278,7 @@ public boolean configureDriverButtonBindings() {
         success = true;
         break;
       default:
-        logger.info("No joystick type {} defined", joystick);
+        // logger.info("No joystick type {} defined", joystick);
         break;
     }
     return success;
@@ -516,7 +517,6 @@ private void configureMatchDashboard() {
         .addBoolean("Update With Vision True", () -> driveSubsystem.visionUpdates)
         .withSize(1, 1)
         .withPosition(4, 0);
-
     Shuffleboard.getTab("Match")
         .addBoolean("IsRight", () -> robotStateSubsystem.getTargetCol() == TargetCol.RIGHT)
         .withSize(1, 1)
@@ -541,6 +541,22 @@ private void configureMatchDashboard() {
         .addBoolean("Is Navx Connected", () -> driveSubsystem.isNavxWorking())
         .withSize(1, 1)
         .withPosition(8, 1);
+    Shuffleboard.getTab("Match")
+        .addString("AutoSwitchPos", () -> autoSwitch.getSwitchPos())
+        .withSize(1, 1)
+        .withPosition(0, 2);
+    Shuffleboard.getTab("Match")
+        .add("VirtualAutonSwitch", autoSwitch.getSendableChooser())
+        .withSize(1, 1)
+        .withPosition(1, 2);
+    Shuffleboard.getTab("Match")
+        .add("ToggleVirtualSwitch", new ToggleVirtualSwitchCommand(autoSwitch))
+        .withSize(1, 1)
+        .withPosition(2, 2);
+    Shuffleboard.getTab("Match")
+        .addBoolean("VirtualSwitchUsed?", () -> autoSwitch.isUseVirtualSwitch())
+        .withSize(1, 1)
+        .withPosition(3, 2);
   }
 
   private void configurePitDashboard() {

src/main/java/frc/robot/commands/robotState/ToggleVirtualSwitchCommand.java

@@ -0,0 +1,32 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.AutoSwitch;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+public class ToggleVirtualSwitchCommand extends InstantCommand {
+  private AutoSwitch autoSwitch;
+  private static Logger logger;
+
+  public ToggleVirtualSwitchCommand(AutoSwitch autoSwitch) {
+    this.autoSwitch = autoSwitch;
+    logger = LoggerFactory.getLogger(ToggleVirtualSwitchCommand.class);
+  }
+
+  @Override
+  public void initialize() {
+    autoSwitch.toggleVirtualSwitch();
+    logger.info("toggledSwitch:command");
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+}

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

@@ -2,6 +2,8 @@
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.AutonConstants;
 import frc.robot.commands.auto.AutoCommandInterface;
 import frc.robot.commands.auto.DefaultAutoCommand;
@@ -19,17 +21,21 @@
 import frc.robot.commands.auto.TwoPieceWithDockAutoCommandGroup;
 import frc.robot.commands.auto.TwoPieceWithDockAutoMidCommandGroup;
 import java.util.ArrayList;
+import java.util.Set;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
 import org.strykeforce.thirdcoast.util.AutonSwitch;
 
-public class AutoSwitch {
+public class AutoSwitch extends MeasurableSubsystem {
   private final AutonSwitch autoSwitch;
   private ArrayList<DigitalInput> switchInputs = new ArrayList<>();
   private int currAutoSwitchPos = -1;
   private int newAutoSwitchPos;
   private int autoSwitchStableCounts = 0;
   private Logger logger = LoggerFactory.getLogger(AutoSwitch.class);
+  private static SendableChooser<Integer> sendableChooser = new SendableChooser<>();
 
   private AutoCommandInterface autoCommand;
   private final RobotStateSubsystem robotStateSubsystem;
@@ -43,6 +49,7 @@ public class AutoSwitch {
   private final VisionSubsystem visionSubsystem;
   private final RGBlightsSubsystem rgBlightsSubsystem;
   AutoCommandInterface defaultCommand;
+  private boolean useVirtualSwitch = false;
 
   public AutoSwitch(
       RobotStateSubsystem robotStateSubsystem,
@@ -66,6 +73,21 @@ public AutoSwitch(
     this.visionSubsystem = visionSubsystem;
     this.rgBlightsSubsystem = rgBlightsSubsystem;
 
+    sendableChooser.addOption("00  Cone Lvl 3, Cube Lvl 3, Auto Balance", 0x00);
+    sendableChooser.setDefaultOption("01  Cone Lvl 3, Cube Lvl 3", 0x01);
+    sendableChooser.setDefaultOption("02  Same as #1 but scores cone mid", 0x02);
+    sendableChooser.setDefaultOption("03  Cone lvl 3, Cube lvl 3, Cube lvl 2", 0x03);
+    sendableChooser.setDefaultOption("10  Cone lvl 3, cube lvl 3, balance", 0x10);
+    sendableChooser.setDefaultOption("11  Middle to dock", 0x11);
+    sendableChooser.setDefaultOption("12  Middle to dock with mobility", 0x12);
+    sendableChooser.setDefaultOption("20  Cone Lvl 3, Cube Lvl 3", 0x20);
+    sendableChooser.setDefaultOption("21  FALLBACK - Cone Lvl 3, cube lvl 2 and 3", 0x21);
+    sendableChooser.setDefaultOption("22  Cone Lvl3, Cube Lvl3", 0x22);
+    sendableChooser.setDefaultOption("23  Cone Lvl 3, cube lvl 2 and 3", 0x23);
+    sendableChooser.setDefaultOption("24  Cone Lvl 1, Cube Lvl 3 and 2", 0x24);
+    sendableChooser.setDefaultOption("30  Do nothing", 0x30);
+    SmartDashboard.putData("Auto Mode", sendableChooser);
+
     defaultCommand =
         new DefaultAutoCommand(
             driveSubsystem, robotStateSubsystem, elevatorSubsystem, handSubsystem, armSubsystem);
@@ -100,7 +122,7 @@ public AutoCommandInterface getAutoCommand() {
 
   private boolean hasSwitchChanged() {
     boolean changed = false;
-    int switchPos = autoSwitch.position();
+    int switchPos = useVirtualSwitch ? sendableChooser.getSelected() : autoSwitch.position();
 
     if (switchPos != newAutoSwitchPos) {
       autoSwitchStableCounts = 0;
@@ -116,6 +138,28 @@ private boolean hasSwitchChanged() {
     return changed;
   }
 
+  public void toggleVirtualSwitch() {
+    logger.info("toggledSwitch:function");
+    if (useVirtualSwitch) {
+      useVirtualSwitch = false;
+    } else {
+      useVirtualSwitch = true;
+    }
+    // useVirtualSwitch = useVirtualSwitch ? false : true;
+  }
+
+  public String getSwitchPos() {
+    return Integer.toHexString(currAutoSwitchPos);
+  }
+
+  public boolean isUseVirtualSwitch() {
+    return useVirtualSwitch;
+  }
+
+  public SendableChooser<Integer> getSendableChooser() {
+    return sendableChooser;
+  }
+
   private AutoCommandInterface getAutoCommand(int switchPos) {
     switch (switchPos) {
         // Non-Bump Side
@@ -292,4 +336,11 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             driveSubsystem, robotStateSubsystem, elevatorSubsystem, handSubsystem, armSubsystem);
     }
   }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of(
+        new Measure("AutoSwitch", () -> currAutoSwitchPos),
+        new Measure("usingVirtualSwitch", () -> this.useVirtualSwitch ? 1.0 : 0.0));
+  }
 }

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

@@ -736,8 +736,19 @@ public void periodic() {
           } else {
             drive(0, 0, 0);
             xLock();
-            logger.info("{} -> AUTO_BALANCE_FINISHED", currDriveState);
-            currDriveState = DriveStates.AUTO_BALANCE_FINISHED;
+
+            logger.info(
+                "AutoBalance Stop: Gyro Roll: {}, trigger Difference: {}",
+                getGyroPitch(),
+                Math.abs(avgStartingRoll) - Math.abs(getGyroPitch()));
+            logger.info("{} -> AUTO_BALANCE_RECOVERY", currDriveState);
+            currDriveState = DriveStates.AUTO_BALANCE_RECOVERY;
+
+            autoBalanceRecoveryTimer.reset();
+            autoBalanceRecoveryTimer.start();
+            recoveryStartingPitchSet = false;
+            // logger.info("{} -> AUTO_BALANCE_FINISHED", currDriveState);
+            // currDriveState = DriveStates.AUTO_BALANCE_FINISHED;
           }
         }
         break;