Skip to content

Commit

Permalink
Fixed the tests by now simulating input from correct input
Browse files Browse the repository at this point in the history
We simulate the input from the logitech controller (all xInput controllers more exactly) since the Command expects this input instead (Added extra comments that explain that)
  • Loading branch information
kytpbs committed Nov 2, 2024
1 parent b12aa38 commit 4a600fd
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 13 deletions.
11 changes: 11 additions & 0 deletions src/main/java/frc/robot/commands/DriveCommands.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,17 @@
import frc.robot.subsystems.DriveSubsystem;

public class DriveCommands {
/**
* A command that handles all boost and deadband calculations for the drive command. It also
* handles the inversion of the controller, so be careful when using this command.
*
* <p><b>Warning</b>: This command meant to be used with the <b>Logitech</b> controller only.
* Xbox/playstation controllers won't work due to forward and backward being inverted.
*
* @param driveSubsystem DriveSubsystem
* @param controller Logitech controller
* @return The command to drive the robot with the <b>Logitech</b> controller
*/
public static Command driveWithController(
DriveSubsystem driveSubsystem, XboxController controller) {
return driveSubsystem.drive(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;
import edu.wpi.first.wpilibj.simulation.XboxControllerSim;
import edu.wpi.first.wpilibj2.command.Command;
Expand Down Expand Up @@ -38,9 +37,9 @@ public void setUp() {

@AfterEach
public void tearDown() {
driveSubsystem.close();
resetController();
super.tearDown();
resetController();
driveSubsystem.close();
}

/** Sets all axis to 0 */
Expand All @@ -56,9 +55,9 @@ void testDefaultDriveCommandForward() {

assertTrue(commandScheduler.isScheduled(defaultDriveCommand));

controllerSim.setLeftY(1);
DriverStationSim.notifyNewData();
assertEquals(1, controller.getLeftY(), 0.001);
controllerSim.setLeftY(-1); // -1 because +y is inverted in the logitech controller
controllerSim.notifyNewData();
assertEquals(-1, controller.getLeftY(), 0.001, "Signal notify broke, check controllerSim");
commandScheduler.run();
SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in
commandScheduler.run();
Expand All @@ -80,8 +79,8 @@ void testDefaultDriveCommandSideways() {
assertTrue(commandScheduler.isScheduled(defaultDriveCommand));

controllerSim.setLeftX(1);
DriverStationSim.notifyNewData();
assertEquals(1, controller.getLeftX(), 0.001);
controllerSim.notifyNewData();
assertEquals(1, controller.getLeftX(), 0.001, "Signal notify broke, check controllerSim");
commandScheduler.run();
SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in
commandScheduler.run();
Expand All @@ -103,10 +102,10 @@ void testDefaultDriveCommandBoost() {

assertTrue(commandScheduler.isScheduled(defaultDriveCommand));

controllerSim.setLeftY(1);
controllerSim.setLeftY(-1); // -1 because +y is inverted in the logitech controller
controllerSim.setRightTriggerAxis(1);
DriverStationSim.notifyNewData();
assertEquals(1, controller.getLeftY(), 0.001);
controllerSim.notifyNewData();
assertEquals(-1, controller.getLeftY(), 0.001, "Signal notify broke, check controllerSim");
commandScheduler.run();
SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in
commandScheduler.run();
Expand All @@ -126,8 +125,8 @@ void testDefaultDriveCommandRotate() {

controllerSim.setRightX(1);
controllerSim.setRightTriggerAxis(0);
DriverStationSim.notifyNewData();
assertEquals(1, controller.getRightX(), 0.001);
controllerSim.notifyNewData();
assertEquals(1, controller.getRightX(), 0.001, "Signal notify broke, check controllerSim");
commandScheduler.run();
SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in
commandScheduler.run();
Expand Down

0 comments on commit 4a600fd

Please # to comment.