Skip to content

Commit

Permalink
Fix negation of joysticks
Browse files Browse the repository at this point in the history
  • Loading branch information
the-mighty-mo committed Feb 18, 2024
1 parent 3fdd1fa commit bda0a65
Showing 1 changed file with 13 additions and 13 deletions.
26 changes: 13 additions & 13 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,11 @@ public RobotContainer()
configureBindings();

AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase,
() -> MathUtil.applyDeadband(driverXbox.getLeftY(),
() -> -MathUtil.applyDeadband(driverXbox.getLeftY(),
OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(),
() -> -MathUtil.applyDeadband(driverXbox.getLeftX(),
OperatorConstants.LEFT_X_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getRightX(),
() -> -MathUtil.applyDeadband(driverXbox.getRightX(),
OperatorConstants.RIGHT_X_DEADBAND),
driverXbox.getHID()::getYButtonPressed,
driverXbox.getHID()::getAButtonPressed,
Expand All @@ -60,25 +60,25 @@ public RobotContainer()
// left stick controls translation
// right stick controls the desired angle NOT angular rotation
Command driveFieldOrientedDirectAngle = drivebase.driveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRightX(),
() -> driverXbox.getRightY());
() -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> -driverXbox.getRightX(),
() -> -driverXbox.getRightY());

// Applies deadbands and inverts controls because joysticks
// are back-right positive while robot
// controls are front-left positive
// left stick controls translation
// right stick controls the angular velocity of the robot
Command driveFieldOrientedAnglularVelocity = drivebase.driveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRawAxis(2));
() -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> -driverXbox.getRightX());

Command driveFieldOrientedDirectAngleSim = drivebase.simDriveCommand(
() -> MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> driverXbox.getRawAxis(2));
() -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND),
() -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.LEFT_X_DEADBAND),
() -> -driverXbox.getRawAxis(2));

drivebase.setDefaultCommand(
!RobotBase.isSimulation() ? driveFieldOrientedDirectAngle : driveFieldOrientedDirectAngleSim);
Expand Down

0 comments on commit bda0a65

Please sign in to comment.