From 61d381e329b9f572c19423116e2a2347f270e45c Mon Sep 17 00:00:00 2001 From: Titan Robotics Club Date: Thu, 22 Aug 2024 01:28:16 -0700 Subject: [PATCH] Updated frclib. Updated trclib. Fixed Gamepad support to conform to library changes. --- TeamCode/src/main/java/ftclib | 2 +- .../src/main/java/teamcode/FtcTeleOp.java | 94 ++++++--------- TeamCode/src/main/java/teamcode/FtcTest.java | 111 ++++++++++-------- .../src/main/java/teamcode/RobotParams.java | 8 +- TeamCode/src/main/java/trclib | 2 +- 5 files changed, 103 insertions(+), 114 deletions(-) diff --git a/TeamCode/src/main/java/ftclib b/TeamCode/src/main/java/ftclib index e53d15f..95bc3dc 160000 --- a/TeamCode/src/main/java/ftclib +++ b/TeamCode/src/main/java/ftclib @@ -1 +1 @@ -Subproject commit e53d15f41aa2d14859d380dd3f9fded6a7f447d8 +Subproject commit 95bc3dcdcd1ccb16a2a884b80322fa295ea894f2 diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index aa6921f..ba4c1ca 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -30,7 +30,6 @@ import ftclib.drivebase.FtcSwerveDrive; import ftclib.driverio.FtcGamepad; import ftclib.robotcore.FtcOpMode; import trclib.drivebase.TrcDriveBase; -import trclib.driverio.TrcGameController; import trclib.pathdrive.TrcPose2D; import trclib.robotcore.TrcDbgTrace; import trclib.robotcore.TrcRobot; @@ -84,10 +83,12 @@ public class FtcTeleOp extends FtcOpMode // // Create and initialize Gamepads. // - driverGamepad = new FtcGamepad("DriverGamepad", gamepad1, this::driverButtonEvent); - operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2, this::operatorButtonEvent); - driverGamepad.setYInverted(true); - operatorGamepad.setYInverted(true); + driverGamepad = new FtcGamepad("DriverGamepad", gamepad1); + driverGamepad.setButtonEventHandler(this::driverButtonEvent); + operatorGamepad = new FtcGamepad("OperatorGamepad", gamepad2); + operatorGamepad.setButtonEventHandler(this::operatorButtonEvent); + driverGamepad.setLeftStickInverted(false, true); + operatorGamepad.setRightStickInverted(false, true); setDriveOrientation(robot.robotInfo.driveOrientation); } //robotInit @@ -237,17 +238,16 @@ public class FtcTeleOp extends FtcOpMode /** * This method is called when driver gamepad button event is detected. * - * @param gamepad specifies the game controller object that generated the event. - * @param button specifies the button ID that generates the event. + * @param button specifies the button that generates the event. * @param pressed specifies true if the button is pressed, false otherwise. */ - public void driverButtonEvent(TrcGameController gamepad, int button, boolean pressed) + public void driverButtonEvent(FtcGamepad.ButtonType button, boolean pressed) { - robot.dashboard.displayPrintf(8, "%s: %04x->%s", gamepad, button, pressed? "Pressed": "Released"); + robot.dashboard.displayPrintf(8, "Driver: %s=%s", button, pressed? "Pressed": "Released"); switch (button) { - case FtcGamepad.GAMEPAD_A: + case A: if (pressed) { robot.globalTracer.traceInfo(moduleName, ">>>>> CancelAll is pressed."); @@ -259,13 +259,13 @@ public class FtcTeleOp extends FtcOpMode } break; - case FtcGamepad.GAMEPAD_B: + case B: break; - case FtcGamepad.GAMEPAD_X: + case X: break; - case FtcGamepad.GAMEPAD_Y: + case Y: if (pressed && robot.robotDrive != null) { if (robot.robotDrive.driveBase.isGyroAssistEnabled()) @@ -283,7 +283,7 @@ public class FtcTeleOp extends FtcOpMode } break; - case FtcGamepad.GAMEPAD_LBUMPER: + case LeftBumper: // Toggle between field or robot oriented driving, only applicable for holonomic drive base. if (pressed && robot.robotDrive != null && robot.robotDrive.driveBase.supportsHolonomicDrive()) { @@ -300,7 +300,7 @@ public class FtcTeleOp extends FtcOpMode } break; - case FtcGamepad.GAMEPAD_RBUMPER: + case RightBumper: // Press and hold for slow drive. if (pressed) { @@ -316,19 +316,13 @@ public class FtcTeleOp extends FtcOpMode } break; - case FtcGamepad.GAMEPAD_DPAD_UP: + case DpadUp: + case DpadDown: + case DpadLeft: + case DpadRight: break; - case FtcGamepad.GAMEPAD_DPAD_DOWN: - break; - - case FtcGamepad.GAMEPAD_DPAD_LEFT: - break; - - case FtcGamepad.GAMEPAD_DPAD_RIGHT: - break; - - case FtcGamepad.GAMEPAD_START: + case Start: if (robot.vision != null && robot.vision.aprilTagVision != null && robot.robotDrive != null) { // On press of the button, we will start looking for AprilTag for re-localization. @@ -352,7 +346,7 @@ public class FtcTeleOp extends FtcOpMode } break; - case FtcGamepad.GAMEPAD_BACK: + case Back: if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) { // Drive base is a Swerve Drive, align all steering wheels forward. @@ -366,49 +360,34 @@ public class FtcTeleOp extends FtcOpMode /** * This method is called when operator gamepad button event is detected. * - * @param gamepad specifies the game controller object that generated the event. - * @param button specifies the button ID that generates the event. + * @param button specifies the button that generates the event. * @param pressed specifies true if the button is pressed, false otherwise. */ - public void operatorButtonEvent(TrcGameController gamepad, int button, boolean pressed) + public void operatorButtonEvent(FtcGamepad.ButtonType button, boolean pressed) { - robot.dashboard.displayPrintf(8, "%s: %04x->%s", gamepad, button, pressed? "Pressed": "Released"); + robot.dashboard.displayPrintf(8, "Operator: %s=%s", button, pressed? "Pressed": "Released"); switch (button) { - case FtcGamepad.GAMEPAD_A: + case A: + case B: + case X: + case Y: + case LeftBumper: break; - case FtcGamepad.GAMEPAD_B: - break; - - case FtcGamepad.GAMEPAD_X: - break; - - case FtcGamepad.GAMEPAD_Y: - break; - - case FtcGamepad.GAMEPAD_LBUMPER: - break; - - case FtcGamepad.GAMEPAD_RBUMPER: + case RightBumper: robot.globalTracer.traceInfo(moduleName, ">>>>> OperatorAltFunc=" + pressed); operatorAltFunc = pressed; break; - case FtcGamepad.GAMEPAD_DPAD_UP: + case DpadUp: + case DpadDown: + case DpadLeft: + case DpadRight: break; - case FtcGamepad.GAMEPAD_DPAD_DOWN: - break; - - case FtcGamepad.GAMEPAD_DPAD_LEFT: - break; - - case FtcGamepad.GAMEPAD_DPAD_RIGHT: - break; - - case FtcGamepad.GAMEPAD_BACK: + case Back: if (pressed) { // Zero calibrate all subsystems (arm, elevator and turret). @@ -416,6 +395,9 @@ public class FtcTeleOp extends FtcOpMode robot.zeroCalibrate(moduleName); } break; + + case Start: + break; } } //operatorButtonEvent diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index ce71884..cceef69 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -42,7 +42,6 @@ import trclib.command.CmdDriveMotorsTest; import trclib.command.CmdPidDrive; import trclib.command.CmdTimedDrive; import trclib.dataprocessor.TrcUtil; -import trclib.driverio.TrcGameController; import trclib.pathdrive.TrcPose2D; import trclib.robotcore.TrcDbgTrace; import trclib.robotcore.TrcPidController; @@ -527,24 +526,23 @@ public class FtcTest extends FtcTeleOp // /** - * This method is called when a driver gamepad button event occurs. + * This method is called when driver gamepad button event is detected. * - * @param gamepad specifies the game controller object that generated the event. - * @param button specifies the button ID that generates the event + * @param button specifies the button that generates the event. * @param pressed specifies true if the button is pressed, false otherwise. */ @Override - public void driverButtonEvent(TrcGameController gamepad, int button, boolean pressed) + public void driverButtonEvent(FtcGamepad.ButtonType button, boolean pressed) { boolean passToTeleOp = true; // // In addition to or instead of the gamepad controls handled by FtcTeleOp, we can add to or override the // FtcTeleOp gamepad actions. // - robot.dashboard.displayPrintf(8, "%s: %04x->%s", gamepad, button, pressed ? "Pressed" : "Released"); + robot.dashboard.displayPrintf(8, "Driver: %s=%s", button, pressed? "Pressed": "Released"); switch (button) { - case FtcGamepad.GAMEPAD_A: + case A: if (testChoices.test == Test.CALIBRATE_SWERVE_STEERING) { if (pressed && robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) @@ -578,7 +576,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_B: + case B: if (testChoices.test == Test.TUNE_COLORBLOB_VISION && robot.vision != null && robot.vision.rawColorBlobVision != null) { @@ -601,7 +599,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_X: + case X: if (testChoices.test == Test.TUNE_COLORBLOB_VISION && robot.vision != null && robot.vision.rawColorBlobVision != null) { @@ -618,7 +616,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_Y: + case Y: if (testChoices.test == Test.TUNE_COLORBLOB_VISION && robot.vision != null && robot.vision.rawColorBlobVision != null) { @@ -631,7 +629,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_LBUMPER: + case LeftBumper: if (testChoices.test == Test.VISION_TEST && robot.vision != null) { if (pressed) @@ -643,7 +641,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_RBUMPER: + case RightBumper: if (testChoices.test == Test.VISION_TEST && robot.vision != null) { if (pressed) @@ -655,7 +653,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_DPAD_UP: + case DpadUp: if (testChoices.test == Test.SUBSYSTEMS_TEST) { // If we are moving swerve steering, make sure TeleOp doesn't interfere. @@ -682,7 +680,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_DPAD_DOWN: + case DpadDown: if (testChoices.test == Test.SUBSYSTEMS_TEST) { // If we are moving swerve steering, make sure TeleOp doesn't interfere. @@ -709,7 +707,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_DPAD_LEFT: + case DpadLeft: if (testChoices.test == Test.SUBSYSTEMS_TEST) { // If we are moving swerve steering, make sure TeleOp doesn't interfere. @@ -733,7 +731,7 @@ public class FtcTest extends FtcTeleOp } break; - case FtcGamepad.GAMEPAD_DPAD_RIGHT: + case DpadRight: if (testChoices.test == Test.SUBSYSTEMS_TEST) { // If we are moving swerve steering, make sure TeleOp doesn't interfere. @@ -756,42 +754,11 @@ public class FtcTest extends FtcTeleOp passToTeleOp = false; } break; - } - // - // If the control was not processed by this method, pass it back to TeleOp. - // - if (passToTeleOp) - { - super.driverButtonEvent(gamepad, button, pressed); - } - } //driverButtonEvent - /** - * This method is called when an operator gamepad button event occurs. - * - * @param gamepad specifies the game controller object that generated the event. - * @param button specifies the button ID that generates the event - * @param pressed specifies true if the button is pressed, false otherwise. - */ - @Override - public void operatorButtonEvent(TrcGameController gamepad, int button, boolean pressed) - { - boolean passToTeleOp = true; - // - // In addition to or instead of the gamepad controls handled by FtcTeleOp, we can add to or override the - // FtcTeleOp gamepad actions. - // - robot.dashboard.displayPrintf(8, "%s: %04x->%s", gamepad, button, pressed ? "Pressed" : "Released"); - switch (button) - { - case FtcGamepad.GAMEPAD_A: - case FtcGamepad.GAMEPAD_B: - case FtcGamepad.GAMEPAD_X: - case FtcGamepad.GAMEPAD_Y: - case FtcGamepad.GAMEPAD_DPAD_UP: - case FtcGamepad.GAMEPAD_DPAD_DOWN: - case FtcGamepad.GAMEPAD_DPAD_LEFT: - case FtcGamepad.GAMEPAD_DPAD_RIGHT: + case Back: + break; + + case Start: break; } // @@ -799,7 +766,47 @@ public class FtcTest extends FtcTeleOp // if (passToTeleOp) { - super.operatorButtonEvent(gamepad, button, pressed); + super.driverButtonEvent(button, pressed); + } + } //driverButtonEvent + + /** + * This method is called when operator gamepad button event is detected. + * + * @param button specifies the button that generates the event. + * @param pressed specifies true if the button is pressed, false otherwise. + */ + @Override + public void operatorButtonEvent(FtcGamepad.ButtonType button, boolean pressed) + { + boolean passToTeleOp = true; + // + // In addition to or instead of the gamepad controls handled by FtcTeleOp, we can add to or override the + // FtcTeleOp gamepad actions. + // + robot.dashboard.displayPrintf(8, "Operator: %s=%s", button, pressed? "Pressed": "Released"); + switch (button) + { + case A: + case B: + case X: + case Y: + case LeftBumper: + case RightBumper: + case DpadUp: + case DpadDown: + case DpadLeft: + case DpadRight: + case Back: + case Start: + break; + } + // + // If the control was not processed by this method, pass it back to TeleOp. + // + if (passToTeleOp) + { + super.operatorButtonEvent(button, pressed); } } //operatorButtonEvent diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index e0fbc8b..7665b4d 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -30,10 +30,10 @@ import org.openftc.easyopencv.OpenCvCameraRotation; import ftclib.drivebase.FtcRobotDrive; import ftclib.drivebase.FtcSwerveDrive; -import ftclib.driverio.FtcGamepad.DriveMode; import ftclib.motor.FtcMotorActuator.MotorType; import trclib.dataprocessor.TrcUtil; import trclib.drivebase.TrcDriveBase.DriveOrientation; +import trclib.driverio.TrcGameController; import trclib.pathdrive.TrcPose2D; import trclib.robotcore.TrcPidController.PidCoefficients; import trclib.vision.TrcHomographyMapper; @@ -241,7 +241,7 @@ public class RobotParams ppdFollowingDistance = 6.0; velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0); // Robot Drive - driveMode = DriveMode.ARCADE_MODE; + driveMode = TrcGameController.DriveMode.ArcadeMode; driveOrientation = DriveOrientation.ROBOT; driveSlowScale = 0.3; driveNormalScale = 1.0; @@ -336,7 +336,7 @@ public class RobotParams ppdFollowingDistance = 6.0; velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0); // Robot Drive - driveMode = DriveMode.ARCADE_MODE; + driveMode = TrcGameController.DriveMode.ArcadeMode; driveOrientation = DriveOrientation.ROBOT; driveSlowScale = 0.3; driveNormalScale = 1.0; @@ -428,7 +428,7 @@ public class RobotParams ppdFollowingDistance = 6.0; velPidCoeffs = new PidCoefficients(0.0, 0.0, 0.0, 1.0 / profiledMaxVelocity, 0.0); // Robot Drive - driveMode = DriveMode.ARCADE_MODE; + driveMode = TrcGameController.DriveMode.ArcadeMode; driveOrientation = DriveOrientation.ROBOT; driveSlowScale = 0.3; driveNormalScale = 1.0; diff --git a/TeamCode/src/main/java/trclib b/TeamCode/src/main/java/trclib index 33ff7c1..ef240e3 160000 --- a/TeamCode/src/main/java/trclib +++ b/TeamCode/src/main/java/trclib @@ -1 +1 @@ -Subproject commit 33ff7c177ac84e076c1be8ad7c56e3341a9f7d30 +Subproject commit ef240e3f79783135706aaa452810c3688683b986