Implement OI class into 2022 code

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

@@ -242,48 +242,7 @@ public String[] getPaths() {
 
   public static final double reverseStopperWheelSpeed = -0.125;
 
-  // spotless:off
-  // Controller Constants {
-  public static final int portNumber0 = 0;
-  public static final int portNumber1 = 1;
-
-  // Buttons not in use
-  public static final int XButton = 1;
-  public static final int AButton = 2;
-  public static final int BButton = 3;
-  public static final int YButton = 4;
-
-  // Intake Subsystem
-  public static final int LBumper = 5; // Intake forward
-  public static final int RBumper = 6; // Intake reverse
-
-  // CDS Subsystem
-  public static final int LTriggerButton = 7; // CDS forward
-  public static final int RTriggerButton = 8; // CDS reverse
-
-  // Shooter Prime/LimeLight
-  public static final int backButton = 9; // Button starts ShooterPrime
-  public static final int startButton = 10; // Button to align LimeLight
-
-  // Shooter Subsystem mode change
-  public static final int LJoystickButton = 11; // Button for Shooter mode
-  public static final int RJoystickButton = 12; // BUtton for Shooter mode
-
-  // POV's not in use
-  public static final int POVup = 0;
-  public static final int POVdown = 180;
-  public static final int POVright = 90;
-  public static final int POVleft = 270;
-
-  // Joystick
-
-  // Unused but will easily be accidentally activated if used
-  public static final int leftJoystickX = 0;
-  public static final int leftJoystickY = 1; // arcade forward / tank left turning
-  public static final int rightJoystickX = 2; // arcade turning
-  public static final int rightJoystickY = 3; // tank right turning
-
-  // Shooter Constants
+  // ShooterConstants
   public static final class Shooter {
     // Motor IDs
     public static final int shooterID = 10; // ID of the shooter
@@ -365,11 +324,6 @@ public static final class Shooter {
 
   public static final double climbServoSetPoint = 0.5;
 
-  public static final double driveSpeedHigh = 1;
-  public static final double driveSpeedLow = 0.6;
-
-  public static final double controllerDeadZone = 0.1;
-
   public static final double climbSequence1Timeout = 20.0;
 
   public static final double armUpSpeed = -1;

src/main/java/frc/robot/OI.java

@@ -0,0 +1,121 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import java.util.function.Supplier;
+
+public class OI {
+  // Operator Interface (OI) class containing all control information
+  private static final int kDriverJoystickPort = 0;
+  private static final int kOperatorJoystickPort = 1;
+
+  public static final class Driver {
+    private static final Joystick kJoystick = new Joystick(OI.kDriverJoystickPort);
+
+    private static final int kIntakeButtonID = 8; // Run the CDS and intake forwards
+    private static final int kOuttakeButtonID = 6; // Run the CDS and intake backwards
+    private static final int kShootButtonID = 7; // Fire the shooter
+
+    private static final int kDriveSpeedAxis = 1; // This axis is inverted
+    private static final int kDriveRotationAxis = 2;
+
+    private static final ControlCurve kDriveSpeedCurve = new ControlCurve(0.9, 0, 0, 0);
+    private static final ControlCurve kDriveRotationCurve = new ControlCurve(0.85, 0, 0, 0);
+
+    public static Supplier<Double> getDriveSpeedSupplier() {
+      return () -> kDriveSpeedCurve.calculate(-kJoystick.getRawAxis(kDriveSpeedAxis));
+    }
+
+    public static Supplier<Double> getDriveRotationSupplier() {
+      return () -> kDriveRotationCurve.calculate(kJoystick.getRawAxis(kDriveRotationAxis));
+    }
+
+    public static JoystickButton getIntakeButton() {
+      return new JoystickButton(kJoystick, kIntakeButtonID);
+    }
+
+    public static JoystickButton getOuttakeButton() {
+      return new JoystickButton(kJoystick, kOuttakeButtonID);
+    }
+
+    public static JoystickButton getShootButton() {
+      return new JoystickButton(kJoystick, kShootButtonID);
+    }
+  }
+
+  public static final class Operator {
+    private static final Joystick kJoystick = new Joystick(OI.kOperatorJoystickPort);
+
+    private static final int kEnableClimbButtonID =
+        10; // Toggle the ability to manipulate climb arms
+    private static final int kAutoClimbButtonID = 1; // Automatically deploy climb arms
+    private static final int kCDSForwardButtonID = 8; // Run the CDS forward
+    private static final int kOuttakeButtonID = 6; // Run the CDS and intake backwards
+
+    private static final int kClimbArmAxis = 1; // This axis is inverted
+    private static final int kClimbPoleAxis = 3; // This axis is inverted
+
+    private static final ControlCurve kClimbArmCurve = new ControlCurve(1, 0, 0, 0.1);
+    private static final ControlCurve kClimbPoleCurve = new ControlCurve(1, 0, 0, 0.1);
+
+    public static Supplier<Double> getClimbArmSupplier() {
+      return () -> kClimbArmCurve.calculate(-kJoystick.getRawAxis(kClimbArmAxis));
+    }
+
+    public static Supplier<Double> getClimbPoleSupplier() {
+      return () -> kClimbPoleCurve.calculate(-kJoystick.getRawAxis(kClimbPoleAxis));
+    }
+
+    public static JoystickButton getEnableClimbButton() {
+      return new JoystickButton(kJoystick, kEnableClimbButtonID);
+    }
+
+    public static JoystickButton getAutoClimbButton() {
+      return new JoystickButton(kJoystick, kAutoClimbButtonID);
+    }
+
+    public static JoystickButton getCDSForwardButton() {
+      return new JoystickButton(kJoystick, kCDSForwardButtonID);
+    }
+
+    public static JoystickButton getOuttakeButton() {
+      return new JoystickButton(kJoystick, kOuttakeButtonID);
+    }
+  }
+
+  public static class ControlCurve {
+    private double ySaturation; // Maximum output, in percentage of possible output
+    private double yIntercept; // Minimum output, in percentage of saturation
+    private double curvature; // Curvature shift between linear and cubic
+    private double deadzone; // Range of input that will always return zero output
+
+    public ControlCurve(double ySaturation, double yIntercept, double curvature, double deadzone) {
+      this.ySaturation = ySaturation;
+      this.yIntercept = yIntercept;
+      this.curvature = curvature;
+      this.deadzone = deadzone;
+    }
+
+    public double calculate(double input) {
+      /* Two equations, separated by a ternary
+      The first is the deadzone
+      y = 0 {|x| < d}
+      The second is the curve
+      y = a(sign(x) * b + (1 - b) * (c * x^3 + (1 - c) * x)) {|x| >= d}
+      Where
+      x = input
+      y = output
+      a = ySaturation
+      b = yIntercept
+      c = curvature
+      d = deadzone
+      and 0 <= a,b,c,d < 1
+      */
+      return Math.abs(input) < deadzone
+          ? 0
+          : ySaturation
+              * (Math.signum(input) * yIntercept
+                  + (1 - yIntercept) * (curvature * Math.pow(input, 3) + (1 - curvature) * input));
+    }
+  }
+}

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

@@ -5,18 +5,15 @@
 package frc.robot;
 
 import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc.robot.Constants.Auton;
 import frc.robot.commands.AutonModes;
 import frc.robot.commands.CDSBallManagementCommand;
 import frc.robot.commands.CDSForwardCommand;
-import frc.robot.commands.ClimbEnable;
 import frc.robot.commands.ClimbPeriodic;
 import frc.robot.commands.ClimbSequence1;
 import frc.robot.commands.CombinedIntakeCDSForwardCommand;
@@ -46,10 +43,6 @@ public class RobotContainer {
   public static ShuffleboardTab controllerDetection;
 
   // The robot's subsystems and commands are defined here...
-  private static final Joystick driverJoystick = new Joystick(Constants.portNumber0);
-  private static final Joystick operatorJoystick = new Joystick(Constants.portNumber1);
-  private JoystickButton[] buttons = new JoystickButton[13];
-  private JoystickButton[] buttons2 = new JoystickButton[13];
 
   // subsystems
   private static ClimbSubsystem climbSubsystem;
@@ -71,9 +64,9 @@ public class RobotContainer {
   private OuttakeCommand outtakeCommand;
   private LimelightAlign limelightAlign;
   // ----------climb---------
-  private ClimbEnable climbEnabling;
+  private InstantCommand climbEnable;
   private ClimbSequence1 climbSequence1;
-  private ClimbPeriodic ClimbPeriodic;
+  private ClimbPeriodic climbPeriodic;
   private Command HaDeploy;
   private Command hookUnlock;
   private Command hookLock;
@@ -106,35 +99,12 @@ public RobotContainer() {
     initSubsystems();
     initCommands();
 
-    // initialize the button bindings
-    for (int i = 1; i < buttons.length; i++) {
-      buttons[i] = new JoystickButton(driverJoystick, i);
-      buttons2[i] = new JoystickButton(operatorJoystick, i);
-    }
     configureButtonBindings();
   }
 
-  private void controllerCheck() {
-    axisCount0 = DriverStation.getStickAxisCount(Constants.portNumber0);
-    buttonCount0 = DriverStation.getStickButtonCount(Constants.portNumber0);
-    sbaxisCount0.setDouble(axisCount0);
-    sbbuttonCount0.setDouble(buttonCount0);
-
-    axisCount1 = DriverStation.getStickAxisCount(Constants.portNumber1);
-    buttonCount1 = DriverStation.getStickButtonCount(Constants.portNumber1);
-    sbaxisCount1.setDouble(axisCount1);
-    sbbuttonCount1.setDouble(buttonCount1);
-
-    System.out.printf(
-        "axisCount0 %d buttonCount0 %d axisCount1 %d buttonCount1 %d\n ",
-        axisCount0, buttonCount0, axisCount1, buttonCount1);
-  }
-
   private void initSubsystems() {
-    // subsystems
-    controllerCheck();
 
-    driveBaseSubsystem = new DriveBaseSubsystem(driverJoystick, Constants.usingExternal);
+    driveBaseSubsystem = new DriveBaseSubsystem(Constants.usingExternal);
 
     cdsSubsystem = new CDSSubsystem();
 
@@ -144,13 +114,17 @@ private void initSubsystems() {
 
     limelightSubsystem = new LimelightSubsystem();
 
-    climbSubsystem = new ClimbSubsystem(operatorJoystick);
+    climbSubsystem = new ClimbSubsystem();
   }
 
   private void initCommands() {
     // Initializes commands based on enabled subsystems
     if (driveBaseSubsystem != null) {
-      driveBaseTeleopCommand = new DriveBaseTeleopCommand(driveBaseSubsystem);
+      driveBaseTeleopCommand =
+          new DriveBaseTeleopCommand(
+              driveBaseSubsystem,
+              OI.Driver.getDriveSpeedSupplier(),
+              OI.Driver.getDriveRotationSupplier());
       driveBaseSubsystem.setDefaultCommand(driveBaseTeleopCommand);
     }
     if (cdsSubsystem != null && shooterSubsystem != null) {
@@ -184,12 +158,16 @@ private void initCommands() {
     }
 
     if ((climbSubsystem != null) && (driveBaseSubsystem != null)) {
-      climbEnabling = new ClimbEnable(climbSubsystem, driveBaseSubsystem);
-      ClimbPeriodic = new ClimbPeriodic(climbSubsystem);
+      climbEnable = new InstantCommand(climbSubsystem::toggleEnabled, climbSubsystem);
+      climbPeriodic =
+          new ClimbPeriodic(
+              climbSubsystem,
+              OI.Operator.getClimbArmSupplier(),
+              OI.Operator.getClimbPoleSupplier());
       climbSequence1 = new ClimbSequence1(climbSubsystem);
       hookUnlock = new HookUnlock(climbSubsystem);
       hookLock = new HookLock(climbSubsystem);
-      climbSubsystem.setDefaultCommand(ClimbPeriodic);
+      climbSubsystem.setDefaultCommand(climbPeriodic);
     }
   }
 
@@ -200,47 +178,33 @@ private void initCommands() {
   // it to a {@link
   // edu.wpi.first.wpilibj2.command.button.JoystickButton}.
   private void configureButtonBindings() {
-    controllerCheck();
 
     // Intake / CDS
     if (outtakeCommand != null) {
-      // spits ball out
-      buttons[Constants.RBumper].whileHeld(outtakeCommand);
+      OI.Driver.getOuttakeButton().whileHeld(outtakeCommand);
     }
 
     if (combinedIntakeCDS != null) {
-      buttons[Constants.RTriggerButton].whileHeld(combinedIntakeCDS);
-    } /*else {
-        buttons[Constants.RTriggerButton].whileHeld(intakeForwardCommand);
-      }*/
+      OI.Driver.getIntakeButton().whileHeld(combinedIntakeCDS);
+    }
 
     if (shooterSubsystem != null && shooterHeldLow != null && shooterHeldAuto != null) {
-      // Auto Aim Shot
-      buttons[Constants.LTriggerButton].whileHeld(
-          shooterHeldAuto.beforeStarting(
-              () -> {
-                shooterSubsystem.setAimMode(Constants.AimModes.TARMAC);
-              },
-              shooterSubsystem));
-      // Fender Shot
-      buttons[Constants.LBumper].whileHeld(
-          shooterHeldLow.beforeStarting(
-              () -> {
-                shooterSubsystem.setAimMode(Constants.AimModes.LOW);
-              },
-              shooterSubsystem));
+      OI.Driver.getShootButton()
+          .whileHeld(
+              shooterHeldLow.beforeStarting(
+                  () -> {
+                    shooterSubsystem.setAimMode(Constants.AimModes.LOW);
+                  },
+                  shooterSubsystem));
     }
 
     if (climbSubsystem != null) {
-      // enable climb and spool out arms
-      buttons2[Constants.startButton].whenPressed(climbEnabling);
-      buttons2[Constants.XButton].whileHeld(climbSequence1);
-
-      // whenHeld button for ClimbSequence2
+      OI.Operator.getEnableClimbButton().whenPressed(climbEnable);
+      OI.Operator.getAutoClimbButton().whileHeld(climbSequence1);
     }
 
-    if (outtakeCommand != null && intakeForwardCommand != null) {
-      buttons2[Constants.RTriggerButton].whileHeld(intakeForwardCommand);
+    if (outtakeCommand != null && CDSForwardCommand != null) {
+      OI.Operator.getCDSForwardButton().whileHeld(CDSForwardCommand);
     }
   }