Expose joysticks for live data collection

src/main/java/frc/lib/SendableXBoxController.java

@@ -0,0 +1,41 @@
+package frc.lib;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class SendableXBoxController extends XboxController implements Sendable {
+
+  public SendableXBoxController(int port) {
+    super(port);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("XBox");
+
+    builder.addDoubleProperty("LeftXAxis", () -> getLeftX(), null);
+    builder.addDoubleProperty("LeftYAxis", () -> getLeftY(), null);
+    builder.addDoubleProperty("RightXAxis", () -> getRightX(), null);
+    builder.addDoubleProperty("RightYAxis", () -> getRightY(), null);
+
+    builder.addDoubleProperty("LeftTriggerAxis", () -> getLeftTriggerAxis(), null);
+    builder.addDoubleProperty("RightTriggerAxis", () -> getRightTriggerAxis(), null);
+
+    builder.addBooleanProperty("A", () -> getAButton(), null);
+    builder.addBooleanProperty("B", () -> getBButton(), null);
+    builder.addBooleanProperty("X", () -> getXButton(), null);
+    builder.addBooleanProperty("Y", () -> getYButton(), null);
+
+    builder.addDoubleProperty("POVAngle", () -> getPOV(), null);
+
+    builder.addBooleanProperty("BackButton", () -> getBackButton(), null);
+    builder.addBooleanProperty("StartButton", () -> getStartButton(), null);
+
+    builder.addBooleanProperty("LeftStickButton", () -> getLeftStickButton(), null);
+    builder.addBooleanProperty("RightStickButton", () -> getRightStickButton(), null);
+
+    builder.addBooleanProperty("LeftBumper", () -> getLeftBumper(), null);
+    builder.addBooleanProperty("RightBumper", () -> getRightBumper(), null);
+  }
+}

src/main/java/frc/lib/SendableZorroController.java

@@ -0,0 +1,47 @@
+package frc.lib;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.Joystick;
+import frc.robot.Constants.OIConstants.Zorro;
+
+public class SendableZorroController extends Joystick implements Sendable {
+
+  public SendableZorroController(int port) {
+    super(port);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Zorro");
+
+    builder.addDoubleProperty("LeftXAxis", () -> getRawAxis(Zorro.kLeftXAxis), null);
+    builder.addDoubleProperty("LeftYAxis", () -> getRawAxis(Zorro.kLeftYAxis), null);
+    builder.addDoubleProperty("LeftDial", () -> getRawAxis(Zorro.kLeftDial), null);
+    builder.addDoubleProperty("RightDial", () -> getRawAxis(Zorro.kRightDial), null);
+    builder.addDoubleProperty("RightXAxis", () -> getRawAxis(Zorro.kRightXAxis), null);
+    builder.addDoubleProperty("RightYAxis", () -> getRawAxis(Zorro.kRightYAxis), null);
+
+    builder.addBooleanProperty("AIn", () -> getRawButton(Zorro.kAIn), null);
+
+    builder.addBooleanProperty("BDown", () -> getRawButton(Zorro.kBDown), null);
+    builder.addBooleanProperty("BMid", () -> getRawButton(Zorro.kBMid), null);
+    builder.addBooleanProperty("BUp", () -> getRawButton(Zorro.kBUp), null);
+
+    builder.addBooleanProperty("CDown", () -> getRawButton(Zorro.kCDown), null);
+    builder.addBooleanProperty("CMid", () -> getRawButton(Zorro.kCMid), null);
+    builder.addBooleanProperty("CUp", () -> getRawButton(Zorro.kCUp), null);
+
+    builder.addBooleanProperty("DIn", () -> getRawButton(Zorro.kDIn), null);
+
+    builder.addBooleanProperty("EDown", () -> getRawButton(Zorro.kEDown), null);
+    builder.addBooleanProperty("EUp", () -> getRawButton(Zorro.kEUp), null);
+
+    builder.addBooleanProperty("FDown", () -> getRawButton(Zorro.kFDown), null);
+    builder.addBooleanProperty("FUp", () -> getRawButton(Zorro.kFUp), null);
+
+    builder.addBooleanProperty("GIn", () -> getRawButton(Zorro.kGIn), null);
+
+    builder.addBooleanProperty("HIn", () -> getRawButton(Zorro.kHIn), null);
+  }
+}

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

@@ -155,35 +155,39 @@ public static final class OIConstants {
     public static final int kDefaultDriverControllerPort = 0;
     public static final int kDefaultOperatorControllerPort = 1;
 
-    // RadioMaster Zorro joystick axis
-    public static int kZorroLeftXAxis = 0;
-    public static int kZorroLeftYAxis = 1;
-    public static int kZorroLeftDial = 2;
-    public static int kZorroRightDial = 3;
-    public static int kZorroRightXAxis = 4;
-    public static int kZorroRightYAxis = 5;
-
-    // RadioMaster Zorro buttons
-    public static int kZorroBDown = 1;
-    public static int kZorroBMid = 2;
-    public static int kZorroBUp = 3;
-    public static int kZorroEDown = 4;
-    public static int kZorroEUp = 5;
-    public static int kZorroAIn = 6;
-    public static int kZorroGIn = 7;
-    public static int kZorroCDown = 8;
-    public static int kZorroCMid = 9;
-    public static int kZorroCUp = 10;
-    public static int kZorroFDown = 11;
-    public static int kZorroFUp = 12;
-    public static int kZorroDIn = 13;
-    public static int kZorroHIn = 14;
-
-    // XBox Controller D-Pad Constants
-    public static int kUp = 0;
-    public static int kRight = 90;
-    public static int kDown = 180;
-    public static int kLeft = 270;
+    public static final class Zorro {
+      // RadioMaster Zorro joystick axis
+      public static int kLeftXAxis = 0;
+      public static int kLeftYAxis = 1;
+      public static int kLeftDial = 2;
+      public static int kRightDial = 3;
+      public static int kRightXAxis = 4;
+      public static int kRightYAxis = 5;
+
+      // RadioMaster Zorro buttons
+      public static int kBDown = 1;
+      public static int kBMid = 2;
+      public static int kBUp = 3;
+      public static int kEDown = 4;
+      public static int kEUp = 5;
+      public static int kAIn = 6;
+      public static int kGIn = 7;
+      public static int kCDown = 8;
+      public static int kCMid = 9;
+      public static int kCUp = 10;
+      public static int kFDown = 11;
+      public static int kFUp = 12;
+      public static int kDIn = 13;
+      public static int kHIn = 14;
+    }
+
+    public static final class XBox {
+      // XBox Controller D-Pad Constants
+      public static int kUp = 0;
+      public static int kRight = 90;
+      public static int kDown = 180;
+      public static int kLeft = 270;
+    }
   }
 
   public static final class IntakeConstants {

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

@@ -5,11 +5,9 @@
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.RobotState;
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Axis;
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj.event.BooleanEvent;
@@ -25,13 +23,16 @@
 import edu.wpi.first.wpilibj2.command.button.POVButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.lib.ControllerPatroller;
+import frc.lib.SendableXBoxController;
+import frc.lib.SendableZorroController;
 import frc.robot.Constants.ArmConstants.ArmState;
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.ClimberConstants;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.Constants.IntakeConstants.IntakeState;
-import frc.robot.Constants.OIConstants;
+import frc.robot.Constants.OIConstants.XBox;
+import frc.robot.Constants.OIConstants.Zorro;
 import frc.robot.LEDs.LEDs;
 import frc.robot.arm.Arm;
 import frc.robot.climber.Climber;
@@ -62,8 +63,8 @@ public static RobotContainer getRobotContainer() {
   private final LEDs m_LEDs = new LEDs();
 
   private final EventLoop m_loop = new EventLoop();
-  private Joystick m_driver;
-  private XboxController m_operator;
+  private SendableZorroController m_driver;
+  private SendableXBoxController m_operator;
 
   // digital inputs for autonomous selection
   private final DigitalInput[] autonomousModes =
@@ -99,8 +100,8 @@ public void configureButtonBindings() {
 
     // We use two different types of controllers - Joystick & XboxController.
     // Create objects of the specific types.
-    m_driver = new Joystick(cp.findDriverPort());
-    m_operator = new XboxController(cp.findOperatorPort());
+    m_driver = new SendableZorroController(cp.findDriverPort());
+    m_operator = new SendableXBoxController(cp.findOperatorPort());
 
     configureDriverButtonBindings();
     configureOperatorButtonBindings();
@@ -146,9 +147,9 @@ public void periodic() {
       SmartDashboard.putString("Auto", "Null");
     }
 
-    SmartDashboard.putNumber("PDHVoltage", m_PowerDistribution.getVoltage());
-    SmartDashboard.putNumber("PDHTotalCurrent", m_PowerDistribution.getTotalCurrent());
-    SmartDashboard.putNumber("RightBumperXBox", Button.kRightBumper.value);
+    SmartDashboard.putData(m_driver);
+    SmartDashboard.putData(m_operator);
+    SmartDashboard.putData(m_PowerDistribution);
   }
 
   private class Autonomous {
@@ -274,16 +275,16 @@ private void setDefaultCommands() {
   private void configureDriverButtonBindings() {
 
     // Reset heading
-    new JoystickButton(m_driver, OIConstants.kZorroHIn)
+    new JoystickButton(m_driver, Zorro.kHIn)
         .onTrue(new InstantCommand(() -> m_swerve.resetHeading())
         .ignoringDisable(true));
 
-    new JoystickButton(m_driver,OIConstants.kZorroAIn)
+    new JoystickButton(m_driver, Zorro.kAIn)
     .whileTrue((new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematicsDriveFromArm, m_driver)));
 
 
     Trigger armDeployed = new Trigger(m_arm.stateChecker(ArmState.DEPLOYED));
-    JoystickButton D_Button = new JoystickButton(m_driver, OIConstants.kZorroDIn);
+    JoystickButton D_Button = new JoystickButton(m_driver, Zorro.kDIn);
 
     // Reverse intake to outake or reject intaking Note
     D_Button.and(armDeployed.negate())
@@ -359,16 +360,15 @@ private void configureOperatorButtonBindings() {
     // Raise and lower arm
     new JoystickButton(m_operator, Button.kA.value).onTrue(m_arm.createStowCommand());
     new JoystickButton(m_operator, Button.kY.value).onTrue(m_arm.createDeployCommand());
-
 
     // Deploy flap
-    new POVButton(m_operator, OIConstants.kUp)
+    new POVButton(m_operator, XBox.kUp)
         .onTrue(m_arm.createFlapDeployCommand()
         .andThen(() -> m_PowerDistribution.setSwitchableChannel(false)));
     // only while arm is raised
 
     // Stow flap
-    new POVButton(m_operator, OIConstants.kDown)
+    new POVButton(m_operator, XBox.kDown)
         .onTrue(m_arm.createFlapRetractCommand()
         .andThen(() -> m_PowerDistribution.setSwitchableChannel(true)));
     // only while arm is raised

src/main/java/frc/robot/drivetrain/commands/ZorroDriveCommand.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.wpilibj.Joystick;
-import frc.robot.Constants.OIConstants;
+import frc.robot.Constants.OIConstants.Zorro;
 import frc.robot.drivetrain.Drivetrain;
 
 public class ZorroDriveCommand extends DriveCommand {
@@ -20,21 +20,21 @@ public ZorroDriveCommand(
 
   @Override
   public double getX() {
-    return -MathUtil.applyDeadband(m_controller.getRawAxis(OIConstants.kZorroRightYAxis), 0.05);
+    return -MathUtil.applyDeadband(m_controller.getRawAxis(Zorro.kRightYAxis), 0.05);
   }
 
   @Override
   public double getY() {
-    return -MathUtil.applyDeadband(m_controller.getRawAxis(OIConstants.kZorroRightXAxis), 0.05);
+    return -MathUtil.applyDeadband(m_controller.getRawAxis(Zorro.kRightXAxis), 0.05);
   }
 
   @Override
   public double getTheta() {
-    return -MathUtil.applyDeadband(m_controller.getRawAxis(OIConstants.kZorroLeftXAxis), 0.05);
+    return -MathUtil.applyDeadband(m_controller.getRawAxis(Zorro.kLeftXAxis), 0.05);
   }
 
   @Override
   public boolean fieldRelative() {
-    return m_controller.getRawButton(OIConstants.kZorroEUp);
+    return m_controller.getRawButton(Zorro.kEUp);
   }
 }