diff --git a/src/main/java/frc/robot/OperatorInput.java b/src/main/java/frc/robot/OperatorInput.java new file mode 100644 index 0000000..1b8bcd8 --- /dev/null +++ b/src/main/java/frc/robot/OperatorInput.java @@ -0,0 +1,74 @@ +package frc.robot; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class OperatorInput { + final CommandXboxController operatorControl = new CommandXboxController(1); + public final CommandXboxController driver = new CommandXboxController(0); + + public boolean actuallyIsTeleop = false; + final Trigger isTeleop = new Trigger(() -> actuallyIsTeleop); // TODO fill this out + + // DRIVER'S CONTROLS + public final Trigger slowModeHold = driver.leftTrigger(); + public final Trigger turboModeHold = driver.rightTrigger(); + + final Trigger resetGyroPress = driver.start(); + + final Trigger xNotDesired = + driver + .axisGreaterThan(XboxController.Axis.kLeftX.value, 0.1) + .or(driver.axisLessThan(XboxController.Axis.kLeftX.value, -0.1)) + .negate(); + final Trigger yNotDesired = + driver + .axisGreaterThan(XboxController.Axis.kLeftY.value, 0.1) + .or(driver.axisLessThan(XboxController.Axis.kLeftY.value, -0.1)) + .negate(); + final Trigger thetaNotDesired = + driver + .axisGreaterThan(XboxController.Axis.kRightX.value, 0.1) + .or(driver.axisLessThan(XboxController.Axis.kRightX.value, -0.1)) + .negate(); + public final Trigger movementNotDesired = xNotDesired.and(yNotDesired).and(thetaNotDesired); + public final Trigger movementDesired = movementNotDesired.negate(); + + /** + * @param input a value + * @return that value, deadbanded + */ + static double deadband(double input) { + double value = input; + + value = MathUtil.applyDeadband(value, 0.1); + value = Math.copySign(value * value, value); + + return value; + } + + public double getDriverRightComponentRaw() { + return -driver.getLeftX(); // reference frame stuff + } + + public double getRobotForwardComponentRaw() { + return -driver.getLeftY(); // reference frame stuff + } + + public double getRobotRotationRaw() { + return driver.getRightX(); + } + + public Rotation2d getDriverRightAsAngle() { + double rotZeroToOne = (driver.getRightX() + 1) % 1; + + return Rotation2d.fromRotations(rotZeroToOne); + } + + public double getDriverAngularComponentRaw() { + return deadband(-driver.getRightX()); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e015329..37b660a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -66,8 +66,6 @@ public void robotInit() { } Logger.start(); - - robotContainer = new RobotContainer(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 32aec83..c92dd1e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,12 +14,9 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; @@ -36,6 +33,7 @@ public class RobotContainer { // Subsystems private final Drive drive; // private final Flywheel flywheel; + public final OperatorInput operatorInput; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -47,6 +45,9 @@ public class RobotContainer { /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + + this.operatorInput = new OperatorInput(); + switch (Constants.currentMode) { case REAL: // Real robot, instantiate hardware IO implementations @@ -112,7 +113,7 @@ public RobotContainer() { "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); // Configure the button bindings - configureButtonBindings(); + loadCommands(); } /** @@ -121,24 +122,14 @@ public RobotContainer() { * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ - private void configureButtonBindings() { - drive.setDefaultCommand( - DriveCommands.joystickDrive( + void loadCommands() { + operatorInput.movementDesired.whileTrue( + DriveCommands.BaseDriveCommand( drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX())); - controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); - controller - .b() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), - drive) - .ignoringDisable(true)); - } + } // TODO FIX COMMAND THIS WILL BREAK DO NOT RUN IT /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index fc8f596..fb52c05 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -34,7 +34,7 @@ private DriveCommands() {} /** * Field relative drive command using two joysticks (controlling linear and angular velocities). */ - public static Command joystickDrive( + public static Command BaseDriveCommand( Drive drive, DoubleSupplier xSupplier, DoubleSupplier ySupplier,