diff --git a/TeamCode/src/main/java/teamcode/FtcTeleOp.java b/TeamCode/src/main/java/teamcode/FtcTeleOp.java index f8d9567..f6650c5 100644 --- a/TeamCode/src/main/java/teamcode/FtcTeleOp.java +++ b/TeamCode/src/main/java/teamcode/FtcTeleOp.java @@ -67,14 +67,9 @@ public class FtcTeleOp extends FtcOpMode @Override public void robotInit() { - // // Create and initialize robot object. - // robot = new Robot(TrcRobot.getRunMode()); - - // // Open trace log. - // if (RobotParams.Preferences.useTraceLog) { String filePrefix = Robot.matchInfo != null? @@ -82,9 +77,7 @@ public class FtcTeleOp extends FtcOpMode "Unknown_TeleOp"; TrcDbgTrace.openTraceLog(RobotParams.Robot.LOG_FOLDER_PATH, filePrefix); } - // // Create and initialize Gamepads. - // driverGamepad = new FtcGamepad("DriverGamepad", gamepad1); driverGamepad.setButtonEventHandler(this::driverButtonEvent); driverGamepad.setLeftStickInverted(false, true); @@ -258,7 +251,7 @@ public class FtcTeleOp extends FtcOpMode * * @param orientation specifies the drive orientation (FIELD, ROBOT, INVERTED). */ - public void setDriveOrientation(TrcDriveBase.DriveOrientation orientation) + private void setDriveOrientation(TrcDriveBase.DriveOrientation orientation) { if (robot.robotDrive != null) { @@ -322,23 +315,17 @@ public class FtcTeleOp extends FtcOpMode break; case B: + break; + case X: + if (pressed) + { + robot.globalTracer.traceInfo(moduleName, ">>>>> Turtle mode."); + robot.turtle(); + } break; case Y: - if (pressed) - { - if (driverAltFunc) - { - robot.globalTracer.traceInfo(moduleName, ">>>>> Cancel all."); - robot.cancelAll(); - } - else - { - robot.globalTracer.traceInfo(moduleName, ">>>>> Turtle mode."); - robot.turtle(); - } - } break; case LeftBumper: @@ -347,15 +334,7 @@ public class FtcTeleOp extends FtcOpMode break; case RightBumper: - if (driverAltFunc) - { - if (!RobotParams.Preferences.updateDashboard) - { - // Toggle status update ON/OFF. - statusUpdateOn = !statusUpdateOn; - } - } - else + if (!driverAltFunc) { // Press and hold for slow drive. if (pressed) @@ -371,6 +350,14 @@ public class FtcTeleOp extends FtcOpMode turnPowerScale = RobotParams.Robot.TURN_NORMAL_SCALE; } } + else if (pressed) + { + if (!RobotParams.Preferences.updateDashboard) + { + // Toggle status update ON/OFF. + statusUpdateOn = !statusUpdateOn; + } + } break; case DpadUp: @@ -382,29 +369,40 @@ public class FtcTeleOp extends FtcOpMode case Back: if (pressed) { - robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating."); - robot.cancelAll(); - robot.zeroCalibrate(moduleName, null); - if (robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + if (!driverAltFunc) { - // Drive base is a Swerve Drive, align all steering wheels forward. - robot.globalTracer.traceInfo(moduleName, ">>>>> Set SteerAngle to zero."); - ((FtcSwerveDrive) robot.robotDrive).setSteerAngle(0.0, false, false); + // Cancel all operations and zero calibrate all subsystems (arm, elevator and turret). + robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating."); + robot.cancelAll(); + robot.zeroCalibrate(moduleName, null); + } + else + { + // If drive base is SwerveDrive, set all wheels pointing forward. + if (robot.robotDrive != null && robot.robotDrive instanceof FtcSwerveDrive) + { + // Drive base is a Swerve Drive, align all steering wheels forward. + robot.globalTracer.traceInfo(moduleName, ">>>>> Set SteerAngle to zero."); + ((FtcSwerveDrive) robot.robotDrive).setSteerAngle(0.0, false, false); + } } } break; case Start: + // Do AprilTag Vision re-localization. if (robot.vision != null && robot.robotDrive != null) { boolean hasAprilTagVision = robot.vision.isAprilTagVisionEnabled(); - + // If Webcam AprilTag vision is not enabled, check if we have Limelight since Limelight has + // AprilTag pipeline as well. if (!hasAprilTagVision && robot.vision.limelightVision != null) { hasAprilTagVision = true; if (pressed) { // Webcam AprilTag vision is not enable, enable Limelight AprilTag pipeline instead. + // Note: we assume pipeline 0 is the AprilTag pipeline. savedLimelightPipeline = robot.vision.limelightVision.getPipeline(); robot.vision.setLimelightVisionEnabled(0, true); } @@ -475,7 +473,7 @@ public class FtcTeleOp extends FtcOpMode case Back: if (pressed) { - // Zero calibrate all subsystems (arm, elevator and turret). + // Cancel all operations and zero calibrate all subsystems (arm, elevator and turret). robot.globalTracer.traceInfo(moduleName, ">>>>> ZeroCalibrating."); robot.cancelAll(); robot.zeroCalibrate(moduleName, null); diff --git a/TeamCode/src/main/java/teamcode/FtcTest.java b/TeamCode/src/main/java/teamcode/FtcTest.java index ec759c1..74fee7e 100644 --- a/TeamCode/src/main/java/teamcode/FtcTest.java +++ b/TeamCode/src/main/java/teamcode/FtcTest.java @@ -265,11 +265,6 @@ public class FtcTest extends FtcTeleOp testCommand.cancel(); } - if (robot.robotDrive != null) - { - robot.robotDrive.cancel(); - } - super.stopMode(prevMode, nextMode); } //stopMode diff --git a/TeamCode/src/main/java/teamcode/Robot.java b/TeamCode/src/main/java/teamcode/Robot.java index 8512e23..a7b7372 100644 --- a/TeamCode/src/main/java/teamcode/Robot.java +++ b/TeamCode/src/main/java/teamcode/Robot.java @@ -41,7 +41,6 @@ import trclib.robotcore.TrcEvent; import trclib.robotcore.TrcRobot; import trclib.sensor.TrcDigitalInput; import trclib.subsystem.TrcSubsystem; -import trclib.timer.TrcTimer; /** * This class creates the robot object that consists of sensors, indicators, drive base and all the subsystems. @@ -55,7 +54,6 @@ public class Robot public final FtcDashboard dashboard; public static FtcMatchInfo matchInfo = null; private static TrcPose2D endOfAutoRobotPose = null; - private static double nextStatusUpdateTime = 0.0; // Robot Drive. public RobotBase robotBase; public FtcRobotDrive.RobotInfo robotInfo; @@ -82,7 +80,6 @@ public class Robot opMode = FtcOpMode.getInstance(); globalTracer = TrcDbgTrace.getGlobalTracer(); dashboard = FtcDashboard.getInstance(); - nextStatusUpdateTime = TrcTimer.getCurrentTime(); speak("Init starting"); // Create and initialize Robot Base. robotBase = new RobotBase(); @@ -192,6 +189,8 @@ public class Robot */ public void stopMode(TrcRobot.RunMode runMode) { + // Cancel all operations. + cancelAll(); // // Print all performance counters if there are any. // diff --git a/TeamCode/src/main/java/teamcode/RobotParams.java b/TeamCode/src/main/java/teamcode/RobotParams.java index a4560b8..83f4f5f 100644 --- a/TeamCode/src/main/java/teamcode/RobotParams.java +++ b/TeamCode/src/main/java/teamcode/RobotParams.java @@ -49,7 +49,7 @@ public class RobotParams public static final boolean useLoopPerformanceMonitor = true; public static final boolean useBatteryMonitor = false; // Driver feedback - // Status Update: Status Update may affect robot loop time, don't do it when in competition. + // Status Update: Dashboard Update may affect robot loop time, don't do it when in competition. public static final boolean updateDashboard = !inCompetition; public static final boolean showDriveBase = false; public static final boolean showPidDrive = false;