Minor changes to Drivetrain to grant the ability to flip between field and robot relative controls. Plus a small amount of Shuffleboard stuff

.OutlineViewer/outlineviewer.json

@@ -13,5 +13,18 @@
       "open": true
     },
     "open": true
+  },
+  "retained": {
+    "Shuffleboard": {
+      "open": true
+    }
+  },
+  "transitory": {
+    "photonvision": {
+      "c922_Pro_Stream_Webcam": {
+        "open": true
+      },
+      "open": true
+    }
   }
 }

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

@@ -12,18 +12,28 @@ import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
 
 public class RobotContainer {
+  private static final String kAutoTabName = "Autonomous";
+  private static final String kTeleopTabName = "Teloperated";
+
   private Drivetrain drivetrain;
 
   private CommandXboxController primary;
   private CommandXboxController secondary;
 
+  private Trigger isTeleopTrigger;
+
   private SendableChooser<Command> autoChooser;
 
   private int ampTagID;
@@ -50,6 +60,8 @@ public class RobotContainer {
     primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
     secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
 
+    isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
+
     Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
 
     if (alliance.isEmpty() || alliance.get() == DriverStation.Alliance.Red) {
@@ -63,14 +75,18 @@ public class RobotContainer {
     }
 
     configureBindings();
+    shuffleboardSetup();
   }
 
   // The objective should be to have the subsystems expose methods that return commands
   // that can be bound to the triggers provided by the CommandXboxController class.
   // This mindset should help keep RobotContainer a little cleaner this year. 
   // This is not to say you can't trigger or command chain here (see driveCardnial drivetrain example),
-  // but generally avoid any situation where the keyword "new" is involved
+  // but generally reduce/avoid any situation where the keyword "new" is involved if you're working 
+  // with a subsystem
   private void configureBindings() {
+    isTeleopTrigger.onTrue(new InstantCommand(() -> Shuffleboard.selectTab(kTeleopTabName)));
+
     drivetrain.setDefaultCommand(
       drivetrain.teleopCommand(
         primary::getLeftX, 
@@ -131,6 +147,34 @@ public class RobotContainer {
 
     // This was originally a run while held, not sure that's really necessary, change it if need be
     primary.rightBumper().onTrue(drivetrain.setXCommand());
+
+    /*
+     * This has been added because interest has been expressed in trying field relative vs
+     * robot relative control. This should <i>default</i> to field relative, but give the option
+     * for the person practicing driving to push start on the driver's controller to quickly switch to
+     * the other style. 
+     * 
+     * If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
+     * mechanism will need to be devised, this will work for now. 
+     */
+    primary.start().onTrue(drivetrain.toggleFieldRelativeControl());
+  }
+
+  private void shuffleboardSetup() {
+    ShuffleboardTab autoTab = Shuffleboard.getTab(kAutoTabName);
+    autoTab.add("Autonomous Selector", autoChooser)
+      .withPosition(0, 0)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kComboBoxChooser);
+
+    //Always select the auto tab on startup
+    Shuffleboard.selectTab(kAutoTabName);
+
+    ShuffleboardTab teleopTab = Shuffleboard.getTab(kTeleopTabName);
+    teleopTab.addBoolean("Field Relative Controls Enabled?", drivetrain::isFieldRelativeControl)
+      .withPosition(0, 0)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kBooleanBox);
   }
 
   public Command getAutonomousCommand() {

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

@@ -49,5 +49,6 @@ public final class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
+    // TODO Is this offset going to continue to be correct for this year
     public static final double kRobotStartOffset = 180;
 }

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

@@ -14,6 +14,7 @@ import edu.wpi.first.util.WPIUtilJNI;
 
 import java.util.Optional;
 import java.util.OptionalDouble;
+import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
 import com.kauailabs.navx.frc.AHRS;
@@ -53,6 +54,8 @@ public class Drivetrain extends SubsystemBase {
     // Odometry class for tracking robot pose
     private final SwerveDrivePoseEstimator m_poseEstimator;
 
+    private boolean fieldRelativeControl;
+
     // Slew rate filter variables for controlling lateral acceleration
     private double m_currentRotation;
     private double m_currentTranslationDir;
@@ -107,6 +110,8 @@ public class Drivetrain extends SubsystemBase {
 
         m_visualPoseProvider = visualPoseProvider;
 
+        fieldRelativeControl = true;
+
         m_currentRotation = 0.0;
         m_currentTranslationDir = 0.0;
         m_currentTranslationMag = 0.0;
@@ -151,6 +156,16 @@ public class Drivetrain extends SubsystemBase {
         }
     }
 
+    public boolean isFieldRelativeControl() {
+        return fieldRelativeControl;
+    }
+
+    public Command toggleFieldRelativeControl() {
+        return runOnce(() -> {
+            fieldRelativeControl = !fieldRelativeControl;
+        });
+    }
+
     /**
      * Returns the currently-estimated pose of the robot.
      *
@@ -205,7 +220,7 @@ public class Drivetrain extends SubsystemBase {
      *                      field.
      * @param rateLimit     Whether to enable rate limiting for smoother control.
      */
-    public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) {
+    public void drive(double xSpeed, double ySpeed, double rot, BooleanSupplier fieldRelative, boolean rateLimit) {
         double xSpeedCommanded;
         double ySpeedCommanded;
 
@@ -259,7 +274,7 @@ public class Drivetrain extends SubsystemBase {
         double rotDelivered = m_currentRotation * DrivetrainConstants.kMaxAngularSpeed;
 
         var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
-            fieldRelative
+            fieldRelative.getAsBoolean()
                 ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle()))
                 : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
         SwerveDriveKinematics.desaturateWheelSpeeds(
@@ -285,7 +300,7 @@ public class Drivetrain extends SubsystemBase {
                 -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
                 -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
                 -MathUtil.applyDeadband(rotation.getAsDouble(), deadband), 
-                true, 
+                () -> fieldRelativeControl, 
                 true
             );
         });
@@ -308,7 +323,7 @@ public class Drivetrain extends SubsystemBase {
                     -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
                     -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
                     output, 
-                    true, 
+                    () -> fieldRelativeControl, 
                     true
                 );
             }, 
@@ -340,12 +355,12 @@ public class Drivetrain extends SubsystemBase {
             }, 
             () -> { return 0; }, 
             (output) -> {
-                // TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field. 
+                // TODO Field relative or no? What makes sense here, since the intent is to orient to the tag, not the field, it shouldn't be affected by the drivers mode choice I don't think. 
                 this.drive(
                     -MathUtil.applyDeadband(xSpeed.getAsDouble(), deadband), 
                     -MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband), 
                     output, 
-                    true, 
+                    () -> true, 
                     true
                 );
             },