Fix operator interface

src/main/java/competition/Robot.java

@@ -2,10 +2,10 @@
 package competition;
 
 import competition.injection.components.BaseRobotComponent;
-import competition.injection.components.DaggerPracticeRobotComponent;
 import competition.injection.components.DaggerRobotComponent;
 import competition.injection.components.DaggerRoboxComponent;
 import competition.injection.components.DaggerSimulationComponent;
+import competition.operator_interface.OperatorInterface;
 import competition.simulation.BaseSimulator;
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.pose.PoseSubsystem;
@@ -20,6 +20,7 @@ public class Robot extends BaseRobot {
     public static final double LOOP_INTERVAL = 0.02;
 
     BaseSimulator simulator;
+    OperatorInterface oi;
 
     Robot() {
         super(LOOP_INTERVAL);
@@ -35,6 +36,7 @@ protected void initializeSystems() {
         if (BaseRobot.isSimulation()) {
             simulator = getInjectorComponent().simulator();
         }
+        oi = getInjectorComponent().operatorInterface();
 
         dataFrameRefreshables.add((DriveSubsystem)getInjectorComponent().driveSubsystem());
         dataFrameRefreshables.add(getInjectorComponent().poseSubsystem());
@@ -50,9 +52,6 @@ protected BaseRobotComponent createDaggerComponent() {
             String chosenContract = Preferences.getString("ContractToUse", "Competition");
 
             switch (chosenContract) {
-                case "Practice":
-                    System.out.println("Using practice contract");
-                    return DaggerPracticeRobotComponent.create();
                 case "Robox":
                     System.out.println("Using Robox contract");
                     return DaggerRoboxComponent.create();
@@ -99,4 +98,13 @@ public void simulationPeriodic() {
             simulator.update();
         }
     }
+
+    @Override
+    protected void sharedPeriodic() {
+        super.sharedPeriodic();
+
+        if (this.oi != null) {
+            this.oi.periodic();
+        }
+    }
 }

src/main/java/competition/injection/components/BaseRobotComponent.java

@@ -1,6 +1,7 @@
 package competition.injection.components;
 
 import competition.operator_interface.OperatorCommandMap;
+import competition.operator_interface.OperatorInterface;
 import competition.simulation.BaseSimulator;
 import competition.subsystems.SubsystemDefaultCommandMap;
 import xbot.common.injection.components.BaseComponent;
@@ -17,4 +18,6 @@ public abstract class BaseRobotComponent extends BaseComponent {
     public abstract SwerveComponentHolder swerveComponentHolder();
 
     public abstract BaseSimulator simulator();
+
+    public abstract OperatorInterface operatorInterface();
 }

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -16,15 +16,6 @@ public class OperatorCommandMap {
 
     @Inject
     public OperatorCommandMap() {}
-
-    // Example for setting up a command to fire when a button is pressed:
-    @Inject
-    public void setupMyCommands(
-            OperatorInterface operatorInterface,
-            SetRobotHeadingCommand resetHeading) {
-        resetHeading.setHeadingToApply(0);
-        operatorInterface.gamepad.getifAvailable(1).onTrue(resetHeading);
-    }
 
     @Inject
     public void setupDriveCommands(OperatorInterface operatorInterface,

src/main/java/competition/operator_interface/OperatorInterface.java

@@ -16,24 +16,28 @@
  */
 @Singleton
 public class OperatorInterface {
-    public XXboxController gamepad;
     public XXboxController driverGamepad;
+    public XXboxController operatorGamepad;
+    public XXboxController setupDebugGamepad;
 
     final DoubleProperty driverDeadband;
     final DoubleProperty operatorDeadband;
 
     @Inject
-    public OperatorInterface(XXboxControllerFactory controllerFactory, RobotAssertionManager assertionManager) {
-        gamepad = controllerFactory.create(0);
-        gamepad.setLeftInversion(false, true);
-        gamepad.setRightInversion(true, true);
-
     public OperatorInterface(XXboxControllerFactory controllerFactory, RobotAssertionManager assertionManager,
-            PropertyFactory pf) {
+                             PropertyFactory pf) {
         driverGamepad = controllerFactory.create(0);
         driverGamepad.setLeftInversion(false, true);
         driverGamepad.setRightInversion(true, true);
 
+        operatorGamepad = controllerFactory.create(1);
+        operatorGamepad.setLeftInversion(false,true);
+        operatorGamepad.setRightInversion(true,true);
+
+        setupDebugGamepad = controllerFactory.create(2);
+        setupDebugGamepad.setLeftInversion(false,true);
+        setupDebugGamepad.setRightInversion(true,true);
+
         pf.setPrefix("OperatorInterface");
         pf.setDefaultLevel(Property.PropertyLevel.Debug);
         driverDeadband = pf.createPersistentProperty("Driver Deadband", 0.12);
@@ -47,4 +51,9 @@ public double getDriverGamepadTypicalDeadband() {
     public double getOperatorGamepadTypicalDeadband() {
         return operatorDeadband.get();
     }
+
+    public void periodic() {
+        driverGamepad.getRumbleManager().periodic();
+        operatorGamepad.getRumbleManager().periodic();
+    }
 }

src/main/java/competition/subsystems/drive/commands/DebugSwerveModuleCommand.java

@@ -0,0 +1,49 @@
+package competition.subsystems.drive.commands;
+
+import competition.operator_interface.OperatorInterface;
+import competition.subsystems.drive.DriveSubsystem;
+import edu.wpi.first.math.MathUtil;
+import xbot.common.command.BaseCommand;
+
+import javax.inject.Inject;
+
+public class DebugSwerveModuleCommand extends BaseCommand {
+
+    final DriveSubsystem drive;
+    final OperatorInterface oi;
+
+    @Inject
+    public DebugSwerveModuleCommand(DriveSubsystem drive, OperatorInterface oi) {
+        this.drive = drive;
+        this.oi = oi;
+        this.addRequirements(drive);
+
+        this.addRequirements(drive.getFrontLeftSwerveModuleSubsystem().getDriveSubsystem());
+        this.addRequirements(drive.getFrontRightSwerveModuleSubsystem().getDriveSubsystem());
+        this.addRequirements(drive.getRearLeftSwerveModuleSubsystem().getDriveSubsystem());
+        this.addRequirements(drive.getRearRightSwerveModuleSubsystem().getDriveSubsystem());
+
+        this.addRequirements(drive.getFrontLeftSwerveModuleSubsystem().getSteeringSubsystem());
+        this.addRequirements(drive.getFrontRightSwerveModuleSubsystem().getSteeringSubsystem());
+        this.addRequirements(drive.getRearLeftSwerveModuleSubsystem().getSteeringSubsystem());
+        this.addRequirements(drive.getRearRightSwerveModuleSubsystem().getSteeringSubsystem());
+    }
+
+    @Override
+    public void initialize() {
+        log.info("Initializing");
+    }
+
+    @Override
+    public void execute() {
+        double drivePower = MathUtil.applyDeadband(oi.driverGamepad.getLeftStickY(), oi.getDriverGamepadTypicalDeadband());
+        double turnPower = MathUtil.applyDeadband(oi.driverGamepad.getRightStickX(), oi.getDriverGamepadTypicalDeadband());
+
+        drive.controlOnlyActiveSwerveModuleSubsystem(drivePower, turnPower);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java

@@ -83,8 +83,6 @@ public void execute() {
     }
 
     private XYPair getRawHumanTranslationIntent() {
-        double xIntent = MathUtils.deadband(oi.gamepad.getLeftVector().getX(), 0.15);
-        double yIntent = MathUtils.deadband(oi.gamepad.getLeftVector().getY(), 0.15);
         double xIntent = MathUtils.deadband(oi.driverGamepad.getLeftVector().getX(), 0.15);
         double yIntent = MathUtils.deadband(oi.driverGamepad.getLeftVector().getY(), 0.15);
 
@@ -100,8 +98,6 @@ private XYPair getRawHumanTranslationIntent() {
 
     private double getRawHumanRotationIntent() {
         // Deadband is to prevent buggy joysticks/triggers
-        double rotateLeftIntent = MathUtils.deadband(oi.gamepad.getLeftTrigger(), 0.05);
-        double rotateRightIntent = MathUtils.deadband(oi.gamepad.getRightTrigger(), 0.05);
         double rotateLeftIntent = MathUtils.deadband(oi.driverGamepad.getLeftTrigger(), 0.05);
         double rotateRightIntent = MathUtils.deadband(oi.driverGamepad.getRightTrigger(), 0.05);
 
@@ -114,8 +110,10 @@ public double getSuggestedRotationIntent(double triggerRotateIntent) {
         // Checks the right joystick input to see if we want to snap to a certain side
         // Apparently, we need to invert the x input here as it has been inverted for other commands already
         // And of course, we must rotate -90 (similar to how we got raw translation) for default alignment
-        XYPair joystickInput = new XYPair(-oi.gamepad.getRightVector().getX(), oi.gamepad.getRightVector().getY()).rotate(-90);
-        XYPair joystickInput = new XYPair(-oi.driverGamepad.getRightVector().getX(), oi.driverGamepad.getRightVector().getY()).rotate(-90);
+        XYPair joystickInput = new XYPair(
+                -oi.driverGamepad.getRightVector().getX(),
+                oi.driverGamepad.getRightVector().getY()
+        ).rotate(-90);
 
         SwerveSuggestedRotation suggested = advisor.getSuggestedRotationValue(joystickInput, triggerRotateIntent);
         return processSuggestedRotationValueIntoPower(suggested);

src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java

@@ -28,8 +28,6 @@ public void initialize() {
     @Override
     public void execute() {
         driveSubsystem.tankDrive(
-            MathUtils.deadband(oi.gamepad.getLeftVector().getY(), 0.15), 
-            MathUtils.deadband(oi.gamepad.getRightVector().getX(), 0.15),
             MathUtils.deadband(oi.driverGamepad.getLeftVector().getY(), 0.15),
             MathUtils.deadband(oi.driverGamepad.getRightVector().getX(), 0.15)
         );