diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 79da774..745d994 100644 --- a/src/main/java/competition/Robot.java +++ b/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(); + } + } } diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 6ee8d7c..aa4474e 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/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(); } diff --git a/src/main/java/competition/injection/components/PracticeRobotComponent.java b/src/main/java/competition/injection/components/PracticeRobotComponent.java deleted file mode 100644 index 80a18b3..0000000 --- a/src/main/java/competition/injection/components/PracticeRobotComponent.java +++ /dev/null @@ -1,17 +0,0 @@ -package competition.injection.components; - -import competition.injection.modules.CommonModule; -import competition.injection.modules.PracticeModule; -import dagger.Component; -import xbot.common.injection.modules.RealControlsModule; -import xbot.common.injection.modules.RealDevicesModule; -import xbot.common.injection.modules.RobotModule; - -import javax.inject.Singleton; - -@Singleton -@Component(modules = { RobotModule.class, RealDevicesModule.class, RealControlsModule.class, - PracticeModule.class, CommonModule.class}) -public abstract class PracticeRobotComponent extends BaseRobotComponent { - -} diff --git a/src/main/java/competition/injection/modules/PracticeModule.java b/src/main/java/competition/injection/modules/PracticeModule.java deleted file mode 100644 index e429827..0000000 --- a/src/main/java/competition/injection/modules/PracticeModule.java +++ /dev/null @@ -1,44 +0,0 @@ -package competition.injection.modules; - -import competition.electrical_contract.ElectricalContract; -import competition.electrical_contract.PracticeContract; -import competition.simulation.BaseSimulator; -import competition.simulation.NoopSimulator; -import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.pose.PoseSubsystem; -import dagger.Binds; -import dagger.Module; -import xbot.common.injection.electrical_contract.XCameraElectricalContract; -import xbot.common.injection.electrical_contract.XSwerveDriveElectricalContract; -import xbot.common.subsystems.drive.BaseDriveSubsystem; -import xbot.common.subsystems.drive.BaseSwerveDriveSubsystem; -import xbot.common.subsystems.pose.BasePoseSubsystem; - -import javax.inject.Singleton; - -@Module -public abstract class PracticeModule { - @Binds - @Singleton - public abstract ElectricalContract getElectricalContract(PracticeContract impl); - - @Binds - @Singleton - public abstract XSwerveDriveElectricalContract getSwerveContract(ElectricalContract impl); - - @Binds - @Singleton - public abstract BasePoseSubsystem getPoseSubsystem(PoseSubsystem impl); - - @Binds - @Singleton - public abstract BaseSwerveDriveSubsystem getSwerveDriveSubsystem(DriveSubsystem impl); - - @Binds - @Singleton - public abstract BaseDriveSubsystem getDriveSubsystem(BaseSwerveDriveSubsystem impl); - - @Binds - @Singleton - public abstract BaseSimulator getSimulator(NoopSimulator impl); -} diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 6f99493..2961c7e 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/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, diff --git a/src/main/java/competition/operator_interface/OperatorInterface.java b/src/main/java/competition/operator_interface/OperatorInterface.java index ff88363..6ec6c89 100644 --- a/src/main/java/competition/operator_interface/OperatorInterface.java +++ b/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(); + } } diff --git a/src/main/java/competition/subsystems/drive/commands/DebugSwerveModuleCommand.java b/src/main/java/competition/subsystems/drive/commands/DebugSwerveModuleCommand.java new file mode 100644 index 0000000..b8a09a2 --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java index 3342d23..817ca0e 100644 --- a/src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java +++ b/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); diff --git a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java index 911ac81..d97a429 100644 --- a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java +++ b/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) );