Combine all controls into one unified using suppliers

shuffleboard.json

@@ -103,17 +103,126 @@
               "Colors/Color when false": "#8B0000FF"
             }
           },
-          "9,2": {
+          "7,1": {
             "size": [
-              1,
+              2,
               1
             ],
             "content": {
               "_type": "Text View",
               "_source0": "network_table:///SmartDashboard/Bot Name",
               "_title": "Bot Name",
-              "_glyph": 148,
-              "_showGlyph": false
+              "_glyph": 474,
+              "_showGlyph": true
+            }
+          },
+          "1,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/PoseX",
+              "_title": "PoseX",
+              "_glyph": 114,
+              "_showGlyph": true
+            }
+          },
+          "0,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/PoseY",
+              "_title": "PoseY",
+              "_glyph": 115,
+              "_showGlyph": true
+            }
+          },
+          "2,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/PoseDegrees",
+              "_title": "PoseDegrees",
+              "_glyph": 419,
+              "_showGlyph": true
+            }
+          },
+          "7,0": {
+            "size": [
+              2,
+              1
+            ],
+            "content": {
+              "_type": "ComboBox Chooser",
+              "_source0": "network_table:///SmartDashboard/Auto Chooser",
+              "_title": "Auto Chooser",
+              "_glyph": 167,
+              "_showGlyph": true
+            }
+          },
+          "5,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Number Bar",
+              "_source0": "network_table:///SmartDashboard/SpeedX",
+              "_title": "SpeedX",
+              "_glyph": 114,
+              "_showGlyph": true,
+              "Range/Min": -1.0,
+              "Range/Max": 1.0,
+              "Range/Center": 0.0,
+              "Visuals/Num tick marks": 5,
+              "Visuals/Show text": true,
+              "Visuals/Orientation": "HORIZONTAL"
+            }
+          },
+          "4,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Number Bar",
+              "_source0": "network_table:///SmartDashboard/SpeedY",
+              "_title": "SpeedY",
+              "_glyph": 115,
+              "_showGlyph": true,
+              "Range/Min": -1.0,
+              "Range/Max": 1.0,
+              "Range/Center": 0.0,
+              "Visuals/Num tick marks": 5,
+              "Visuals/Show text": true,
+              "Visuals/Orientation": "HORIZONTAL"
+            }
+          },
+          "6,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Number Bar",
+              "_source0": "network_table:///SmartDashboard/Speed",
+              "_title": "Speed",
+              "_glyph": 419,
+              "_showGlyph": true,
+              "Range/Min": -1.0,
+              "Range/Max": 1.0,
+              "Range/Center": 0.0,
+              "Visuals/Num tick marks": 5,
+              "Visuals/Show text": true,
+              "Visuals/Orientation": "HORIZONTAL"
             }
           }
         }

simgui.json

@@ -2,6 +2,7 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto Chooser": "String Chooser",
       "/SmartDashboard/SendableChooser[0]": "String Chooser"
     }
   },

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

@@ -31,7 +31,7 @@ public static enum Bot {
 	 * @author Aceius E.
 	 */
 	static {
-		serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "-default-";
+		serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "Simulation";
 
 		switch (serialNumber) {
 			case "03282B00": // Wood Bot Serial Number
@@ -42,7 +42,6 @@ public static enum Bot {
 				currentBot = Bot.PRACTICE_BOT;
 				break;
 
-			case "-default-": // Competition Bot Serial Number
 			default: // Also use competition bot as default
 				currentBot = Bot.COMPETITION_BOT;
 				break;
@@ -52,9 +51,7 @@ public static enum Bot {
 	public static class DriverConstants {
 		public static final int DRIVER_JOYSTICK_PORT = 0;
 
-		public static final double JOYSTICK_DEAD_ZONE = 0.25;
-		public static final double XBOX_DEAD_ZONE = 0.10;
-		public static final double PS4_DEAD_ZONE = 0.12;
+		public static final double DEAD_ZONE = 0.25;
 
 		// Names of options for displaying
 		public static final String[] maxSpeedOptionsNames = { "Precise", "Normal", "Boost" };

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

@@ -4,11 +4,12 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.commands.Autos;
-import frc.robot.commands.SwerveRemoteOperation.BaseControl;
-import frc.robot.commands.SwerveRemoteOperation.JoystickControl;
-import frc.robot.commands.SwerveRemoteOperation.DriveXboxControl;
+import frc.robot.commands.DriverControl;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
+import frc.robot.utils.ChassisDriveInputs;
+import frc.robot.utils.OptionButton;
+import frc.robot.utils.OptionButton.ActivationMode;
 
 import com.kauailabs.navx.frc.AHRS;
 
@@ -74,7 +75,7 @@ public class RobotContainer {
 	 */
 	public RobotContainer() {
 		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
-		SmartDashboard.putData(autoChooser);
+		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
 
@@ -87,16 +88,32 @@ public void setUpDriveController() {
 		final HIDType genericHIDType = genericHID.getType();
 
 		SmartDashboard.putString("Drive Controller", genericHIDType.toString());
-		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " " + Constants.serialNumber);
+		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
 
 		drivetrain.removeDefaultCommand();
 
-		BaseControl control;
+		DriverControl control;
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
-			control = new JoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
+			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
+			control = new DriverControl(drivetrain,
+				new ChassisDriveInputs(
+					joystick::getX, joystick::getY, joystick::getTwist,
+					-1, -1, Constants.DriverConstants.DEAD_ZONE),
+				new OptionButton(joystick, 2, ActivationMode.TOGGLE),
+				new OptionButton(joystick, 1, ActivationMode.HOLD),
+				new OptionButton(joystick, 3, ActivationMode.TOGGLE)
+			);
 		} else {
-			control = new DriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
+			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
+			control = new DriverControl(drivetrain,
+				new ChassisDriveInputs(
+					xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
+					+1, -1, Constants.DriverConstants.DEAD_ZONE),
+				new OptionButton(xbox::b, ActivationMode.TOGGLE),
+				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
+				new OptionButton(xbox::povUp, ActivationMode.TOGGLE)
+			);
 		}
 
 		drivetrain.setDefaultCommand(control);

src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java → src/main/java/frc/robot/commands/DriverControl.java

@@ -1,30 +1,42 @@
-package frc.robot.commands.SwerveRemoteOperation;
+package frc.robot.commands;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
+import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.utils.ChassisDriveInputs;
+import frc.robot.utils.OptionButton;
 
 /**
  * This can be the default command for the drivetrain, allowing for remote
  * operation with a controller
  */
-public abstract class BaseControl extends Command {
+public class DriverControl extends Command {
 	protected final SwerveDrivetrain drivetrain;
-	protected final CommandGenericHID controller;
+
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+	private final OptionButton fieldRelativeButton;
+
+	private final ChassisDriveInputs chassisDriveInputs;
 
 	/**
 	 * Creates a new SwerveDriveBaseControl Command.
 	 *
 	 * @param drivetrain       The drivetrain of the robot
 	 * @param driverController The device used to control drivetrain
 	 */
-	public BaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
+	public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
+			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton) {
+
+		this.chassisDriveInputs = chassisDriveInputs;
+
+		this.preciseModeButton = preciseModeButton;
+		this.boostModeButton = boostModeButton;
+		this.fieldRelativeButton = fieldRelativeButton;
 
-		// save parameters
-		this.controller = driverController;
 		this.drivetrain = drivetrain;
 
 		// Tell the command schedular we are using the drivetrain
@@ -50,15 +62,41 @@ public void initialize() {
 	 */
 	@Override
 	public void execute() {
+		final double speedX = chassisDriveInputs.getX();
+		final double speedY = chassisDriveInputs.getY();
+
+		final double speedRotation = chassisDriveInputs.getRotation();
+
+		final boolean isFieldRelative = fieldRelativeButton.getState();
+
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedRotation * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
+
+		SmartDashboard.putNumber("SpeedX", speedX);
+		SmartDashboard.putNumber("SpeedY", speedY);
+		SmartDashboard.putNumber("Speed", speedRotation);
+
+		drivetrain.setDesiredState(speeds, isFieldRelative, true);
+
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
 		// Position display
 		final Pose2d robotPosition = drivetrain.getPosition();
 
 		SmartDashboard.putNumber("PoseX", robotPosition.getX());
-		SmartDashboard.putNumber("PoseY", robotPosition.getX());
+		SmartDashboard.putNumber("PoseY", robotPosition.getY());
 		SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
 
-		// Speed degrees
+		// Speed and Heading
 		final ChassisSpeeds currentSpeeds = drivetrain.getState();
 		final double speedMetersPerSecond = Math
 				.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
@@ -89,24 +127,4 @@ public void end(boolean interrupted) {
 
 		SmartDashboard.putBoolean("ControlActive", false);
 	}
-
-	// --- Util ---
-
-	/**
-	 * Utility method. Apply a deadzone to the joystick output to account for stick
-	 * drift and small bumps.
-	 * 
-	 * @param joystickValue Value in [-1, 1] from joystick axis
-	 * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the
-	 *         {@code joystickValue} scaled to the new control area
-	 */
-	public static double applyJoystickDeadzone(double joystickValue, double deadzone) {
-		if (Math.abs(joystickValue) <= deadzone) {
-			// If the joystick |value| is in the deadzone than zero it out
-			return 0;
-		}
-
-		// scale value from the range [0, 1] to (deadzone, 1]
-		return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
-	}
 }