diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 878287a..773110e 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -12,8 +12,20 @@ // Custom definitions may go here // Include common definitions from above. + +buildscript { + repositories { + mavenCentral() + maven { url "https://repo.dairy.foundation/releases" } + } + dependencies { + classpath "dev.frozenmilk:Load:0.2.4" + } +} + apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' +apply plugin: 'dev.frozenmilk.sinister.sloth.load' android { namespace = 'org.firstinspires.ftc.teamcode' @@ -23,6 +35,16 @@ android { } } + +repositories { + maven { url = "https://repo.dairy.foundation/releases" } +} + dependencies { implementation project(':FtcRobotController') + // 1. Install Sloth + implementation("dev.frozenmilk.sinister:Sloth:0.2.4") + + // 2. Install the Sloth version of Panels + implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12") } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java index 18104b7..7fdb990 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java @@ -6,20 +6,30 @@ import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; +import com.qualcomm.robotcore.hardware.VoltageSensor; -// PANELS IMPORTS import com.bylazar.telemetry.JoinedTelemetry; +import org.firstinspires.ftc.teamcode.lib.actions.RoutineManager; import org.firstinspires.ftc.teamcode.teleOp.Constants; import org.firstinspires.ftc.teamcode.util.FPSCounter; + import java.util.List; public abstract class BaseOpMode extends OpMode { protected final SubsysManager manager = new SubsysManager(); + + protected final RoutineManager routines = new RoutineManager(); protected final FPSCounter fps = new FPSCounter(); private final ElapsedTime telemetryTimer = new ElapsedTime(); private List allHubs; + private final ElapsedTime voltageTimer = new ElapsedTime(); + private VoltageSensor batterySensor; + + + protected EnhancedGamepad g1; + protected EnhancedGamepad g2; protected Telemetry unifiedTelemetry; @@ -31,18 +41,24 @@ public abstract class BaseOpMode extends OpMode { public final void init() { unifiedTelemetry = new JoinedTelemetry(telemetry); + batterySensor = hardwareMap.voltageSensor.iterator().next(); + PIDController.currentSystemVoltage = batterySensor.getVoltage(); + allHubs = hardwareMap.getAll(LynxModule.class); for (LynxModule hub : allHubs) { hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); } + + g1 = new EnhancedGamepad(gamepad1); + g2 = new EnhancedGamepad(gamepad2); + setupSubsystems(); - manager.initAll(hardwareMap); - onInit(); } + @Override public final void start() { fps.reset(); @@ -52,23 +68,30 @@ public abstract class BaseOpMode extends OpMode { @Override public final void loop() { + fps.startLoop(); for (LynxModule hub : allHubs) { hub.clearBulkCache(); } - fps.startLoop(); - stateMachineUpdate(); + g1.update(); + g2.update(); + + if (voltageTimer.milliseconds() >= Constants.PIDController.VOLTAGE_CHECK_INTERVAL_MS) { + PIDController.currentSystemVoltage = batterySensor.getVoltage(); + voltageTimer.reset(); + } + stateMachineUpdate(); manager.updateAll(); + routines.updateAll(); if (telemetryTimer.milliseconds() >= Constants.GLOBAL.TELEMETRY_DELAY_MS) { - manager.publishTelemetryAll(unifiedTelemetry); if (Constants.GLOBAL.DEBUG_MODE) { + unifiedTelemetry.addData("System Voltage", PIDController.currentSystemVoltage); unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime()); - unifiedTelemetry.addData("Target FPS", Constants.GLOBAL.TARGET_FPS); - unifiedTelemetry.addData("Current FPS", fps.getAverageFps()); + unifiedTelemetry.addData("FPS", fps.getAverageFps()); } unifiedTelemetry.update(); @@ -78,6 +101,6 @@ public abstract class BaseOpMode extends OpMode { long sleepTime = fps.getSyncTime(Constants.GLOBAL.TARGET_FPS); if (sleepTime > 0) { SystemClock.sleep(sleepTime); - } + } // CAP FPS Logic (check Constants.GLOBAL.TARGET_FPS, if 0 then uncapped) } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/EnhancedGamepad.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/EnhancedGamepad.java new file mode 100644 index 0000000..9369968 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/EnhancedGamepad.java @@ -0,0 +1,110 @@ +package org.firstinspires.ftc.teamcode.lib; + +import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.teamcode.teleOp.Constants; + +public class EnhancedGamepad { + private final Gamepad hardwareGamepad; + private final Gamepad currentState; + private final Gamepad previousState; + + public EnhancedGamepad(Gamepad gamepad) { + this.hardwareGamepad = gamepad; + this.currentState = new Gamepad(); + this.previousState = new Gamepad(); + } + + public void update() { + previousState.copy(currentState); + currentState.copy(hardwareGamepad); + } + + private double scale(double input) { + if (Math.abs(input) < Constants.Gamepad.STICK_DEADBAND) return 0; + return Math.pow(input, 3); + } + + private boolean triggerAsBtn(float val) { + return val > Constants.Gamepad.TRIGGER_THRESHOLD; + } + + public double left_stick_x() { return scale(currentState.left_stick_x); } + public double left_stick_y() { return scale(currentState.left_stick_y); } + public double right_stick_x() { return scale(currentState.right_stick_x); } + public double right_stick_y() { return scale(currentState.right_stick_y); } + + public double left_trigger() { return currentState.left_trigger; } + public double right_trigger() { return currentState.right_trigger; } + + // --- BUTTONS: CURRENT STATE --- + public boolean a() { return currentState.a; } + public boolean b() { return currentState.b; } + public boolean x() { return currentState.x; } + public boolean y() { return currentState.y; } + + public boolean dpad_up() { return currentState.dpad_up; } + public boolean dpad_down() { return currentState.dpad_down; } + public boolean dpad_left() { return currentState.dpad_left; } + public boolean dpad_right() { return currentState.dpad_right; } + + public boolean left_bumper() { return currentState.left_bumper; } + public boolean right_bumper() { return currentState.right_bumper; } + public boolean left_stick_button() { return currentState.left_stick_button; } + public boolean right_stick_button() { return currentState.right_stick_button; } + + public boolean back() { return currentState.back; } + public boolean start() { return currentState.start; } + public boolean guide() { return currentState.guide; } + public boolean options() { return currentState.options; } + + // PS Specific Aliases + public boolean ps() { return currentState.ps; } + public boolean share() { return currentState.share; } + public boolean touchpad() { return currentState.touchpad; } + public boolean circle() { return currentState.circle; } + public boolean cross() { return currentState.cross; } + public boolean triangle() { return currentState.triangle; } + public boolean square() { return currentState.square; } + + // Triggers as Buttons + public boolean left_trigger_btn() { return triggerAsBtn(currentState.left_trigger); } + public boolean right_trigger_btn() { return triggerAsBtn(currentState.right_trigger); } + + // --- BUTTONS: WAS PRESSED (Rising Edge) --- + public boolean aWasPressed() { return currentState.a && !previousState.a; } + public boolean bWasPressed() { return currentState.b && !previousState.b; } + public boolean xWasPressed() { return currentState.x && !previousState.x; } + public boolean yWasPressed() { return currentState.y && !previousState.y; } + + public boolean dpad_upWasPressed() { return currentState.dpad_up && !previousState.dpad_up; } + public boolean dpad_downWasPressed() { return currentState.dpad_down && !previousState.dpad_down; } + public boolean dpad_leftWasPressed() { return currentState.dpad_left && !previousState.dpad_left; } + public boolean dpad_rightWasPressed() { return currentState.dpad_right && !previousState.dpad_right; } + + public boolean left_bumperWasPressed() { return currentState.left_bumper && !previousState.left_bumper; } + public boolean right_bumperWasPressed() { return currentState.right_bumper && !previousState.right_bumper; } + + public boolean left_stick_buttonWasPressed() { return currentState.left_stick_button && !previousState.left_stick_button; } + public boolean right_stick_buttonWasPressed() { return currentState.right_stick_button && !previousState.right_stick_button; } + + public boolean backWasPressed() { return currentState.back && !previousState.back; } + public boolean startWasPressed() { return currentState.start && !previousState.start; } + public boolean optionsWasPressed() { return currentState.options && !previousState.options; } + public boolean touchpadWasPressed() { return currentState.touchpad && !previousState.touchpad; } + + public boolean left_trigger_btnWasPressed() { return triggerAsBtn(currentState.left_trigger) && !triggerAsBtn(previousState.left_trigger); } + public boolean right_trigger_btnWasPressed() { return triggerAsBtn(currentState.right_trigger) && !triggerAsBtn(previousState.right_trigger); } + + // --- BUTTONS: WAS RELEASED (Falling Edge) --- + public boolean aWasReleased() { return !currentState.a && previousState.a; } + public boolean bWasReleased() { return !currentState.b && previousState.b; } + public boolean xWasReleased() { return !currentState.x && previousState.x; } + public boolean yWasReleased() { return !currentState.y && previousState.y; } + + public boolean left_bumperWasReleased() { return !currentState.left_bumper && previousState.left_bumper; } + public boolean right_bumperWasReleased() { return !currentState.right_bumper && previousState.right_bumper; } + + // --- RUMBLE CONTROLS --- + public void rumble(int durationMs) { hardwareGamepad.rumble(durationMs); } + public void rumbleBlips(int count) { hardwareGamepad.rumbleBlips(count); } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/LLUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/LLUtil.java new file mode 100644 index 0000000..0ba8dc6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/LLUtil.java @@ -0,0 +1,72 @@ +package org.firstinspires.ftc.teamcode.lib; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.teamcode.teleOp.Constants; + +public class LLUtil { + private Limelight3A limelight; + private LLResult lastResult; + private final ElapsedTime timer = new ElapsedTime(); + private double lastKnownDistance = 0; + + public LLUtil(HardwareMap hwMap) { + limelight = hwMap.get(Limelight3A.class, Constants.LIMELIGHT.NAME); + limelight.setPollRateHz(Constants.LIMELIGHT.POLL_RATE); + limelight.start(); + } + + public void update() { + lastResult = limelight.getLatestResult(); + if (hasTarget()) { + timer.reset(); + lastKnownDistance = getTrigDistance(); + } + } + + public boolean hasTarget() { + return lastResult != null && lastResult.isValid(); + } + + public boolean isDataFresh() { + return hasTarget() || (timer.seconds() < Constants.LIMELIGHT.STALE_DATA_TIMEOUT_SEC); + } + + public double getTx() { + return hasTarget() ? lastResult.getTx() : 0; + } + + public double getTy() { + return hasTarget() ? lastResult.getTy() : 0; + } + + public double getTa() { + return hasTarget() ? lastResult.getTa() : 0; + } + + public double getTrigDistance() { // Trig Distance + if (!hasTarget()) return lastKnownDistance; + + double angleToTargetRad = Math.toRadians(Constants.LIMELIGHT.MOUNT_ANGLE_DEGREES + getTy()); + double heightDiff = Constants.LIMELIGHT.TARGET_HEIGHT_IN - Constants.LIMELIGHT.MOUNT_HEIGHT_IN; + + return heightDiff / Math.tan(angleToTargetRad); + } + + public double getAreaDistance() { // TA Regr + if (!hasTarget()) return lastKnownDistance; + return (Constants.LIMELIGHT.AREA_COEFF * Math.pow(getTa(), Constants.LIMELIGHT.AREA_EXPONENT)) + + Constants.LIMELIGHT.AREA_OFFSET; + } + + public void setPipeline(int index) { + limelight.pipelineSwitch(index); + } + + public void stop() { + limelight.stop(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java index fe6bd7f..7443acc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java @@ -1,10 +1,12 @@ package org.firstinspires.ftc.teamcode.lib; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.teamcode.teleOp.Constants; public class PIDController { - private double kP, kI, kD; + public static double currentSystemVoltage = 12.0; + private double kP, kI, kD; private double lastError = 0; private double integralSum = 0; private final ElapsedTime timer = new ElapsedTime(); @@ -39,7 +41,11 @@ public class PIDController { integralSum += (error * dt); lastError = error; - return (kP * error) + (kI * integralSum) + (kD * derivative); + double rawOutput = (kP * error) + (kI * integralSum) + (kD * derivative); + + double compensationMultiplier = Constants.PIDController.VOLTAGE_REFERENCE / currentSystemVoltage; + + return rawOutput * compensationMultiplier; } public void reset() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Action.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Action.java new file mode 100644 index 0000000..46e90a9 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Action.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.lib.actions; + +public interface Action { + /** Called exactly once when the action begins. */ + void start(); + + /** + * Called every loop while the action is active. + * @return true if the action is finished, false if still running. + */ + boolean update(); +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Routine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Routine.java new file mode 100644 index 0000000..2e6161a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/Routine.java @@ -0,0 +1,130 @@ +package org.firstinspires.ftc.teamcode.lib.actions; + +import com.qualcomm.robotcore.util.ElapsedTime; +import java.util.Arrays; +import java.util.List; +import java.util.function.BooleanSupplier; + +public class Routine { + + /** Runs a single block of code instantly, then finishes. */ + public static Action instant(Runnable runnable) { + return new InstantAction(runnable); + } + + /** Waits for a specific amount of milliseconds without freezing the robot. */ + public static Action wait(double milliseconds) { + return new WaitAction(milliseconds); + } + + /** Waits until a specific condition becomes true (e.g. Color Sensor detects pixel) */ + public static Action waitUntil(BooleanSupplier condition) { + return new ConditionAction(condition); + } + + /** Runs multiple actions one after the other. */ + public static Action sequence(Action... actions) { + return new SequentialAction(Arrays.asList(actions)); + } + + /** Runs multiple actions at the exact same time. Finishes when ALL are done. */ + public static Action parallel(Action... actions) { + return new ParallelAction(Arrays.asList(actions)); + } + + private static class InstantAction implements Action { + private final Runnable task; + public InstantAction(Runnable task) { this.task = task; } + + @Override + public void start() { task.run(); } + + @Override + public boolean update() { return true; } + } + + private static class WaitAction implements Action { + private final double duration; + private final ElapsedTime timer = new ElapsedTime(); + + public WaitAction(double duration) { this.duration = duration; } + + @Override + public void start() { timer.reset(); } + + @Override + public boolean update() { return timer.milliseconds() >= duration; } + } + + private static class ConditionAction implements Action { + private final BooleanSupplier condition; + public ConditionAction(BooleanSupplier condition) { this.condition = condition; } + + @Override + public void start() {} + + @Override + public boolean update() { return condition.getAsBoolean(); } + } + + private static class SequentialAction implements Action { + private final List actions; + private int currentIndex = 0; + private boolean isCurrentStarted = false; + + public SequentialAction(List actions) { this.actions = actions; } + + @Override + public void start() { + currentIndex = 0; + isCurrentStarted = false; + } + + @Override + public boolean update() { + if (currentIndex >= actions.size()) return true; + + if (!isCurrentStarted) { + actions.get(currentIndex).start(); + isCurrentStarted = true; + } + + if (actions.get(currentIndex).update()) { + currentIndex++; + isCurrentStarted = false; + } + + return currentIndex >= actions.size(); + } + } + + private static class ParallelAction implements Action { + private final List actions; + private final boolean[] finished; + + public ParallelAction(List actions) { + this.actions = actions; + this.finished = new boolean[actions.size()]; + } + + @Override + public void start() { + for (int i = 0; i < actions.size(); i++) { + finished[i] = false; + actions.get(i).start(); + } + } + + @Override + public boolean update() { + boolean allDone = true; + for (int i = 0; i < actions.size(); i++) { + if (!finished[i]) { + finished[i] = actions.get(i).update(); + if (!finished[i]) allDone = false; + } + } + return allDone; + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/RoutineManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/RoutineManager.java new file mode 100644 index 0000000..9a8d08f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/RoutineManager.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.lib.actions; + +import java.util.ArrayList; +import java.util.Iterator; +import java.util.List; + +public class RoutineManager { + private final List activeRoutines = new ArrayList<>(); + + public void run(Action action) { + action.start(); + activeRoutines.add(action); + } + + public void updateAll() { + Iterator iterator = activeRoutines.iterator(); + while (iterator.hasNext()) { + Action action = iterator.next(); + if (action.update()) { + iterator.remove(); + } + } + } + + public void cancelAll() { + activeRoutines.clear(); + } + + public boolean isBusy() { + return !activeRoutines.isEmpty(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/routine.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/routine.md new file mode 100644 index 0000000..66dccc0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/actions/routine.md @@ -0,0 +1,99 @@ + + + +# Routines & Action Sequencer 🎬 + +The **Routines** system is one of the most powerful features in `ftc-lib`. It allows you to write complex, multi-step robot scripts (like an Auto-Score macro) **without freezing the robot**. + +In standard linear OpModes, calling `sleep(1000)` completely stops the robot from driving. The `ftc-lib` Routine Manager runs your scripts asynchronously in the background. Your driver retains full control of the drivetrain while the robot automatically orchestrates the lift, intake, and servos. + +--- + +## 🧩 The Building Blocks (Actions) + +Everything in a routine is an `Action`. You do not need to create these manually; use the `Routine` factory class to build them instantly. + +| Method | Description | Example | +| :--- | :--- | :--- | +| `Routine.instant()` | Runs a single block of code immediately. | `Routine.instant(() -> lift.setTarget(1000))` | +| `Routine.wait()` | Pauses **only this sequence** for `x` milliseconds. | `Routine.wait(500)` | +| `Routine.waitUntil()` | Pauses the sequence until a condition is `true`. | `Routine.waitUntil(() -> lift.isAtTarget())` | +| `Routine.sequence()` | Runs multiple Actions **one after the other**. | `Routine.sequence(action1, action2)` | +| `Routine.parallel()` | Runs multiple Actions **at the exact same time**. | `Routine.parallel(action1, action2)` | + +--- + +## 🚀 Running a Routine in TeleOp + +Because `BaseOpMode` automatically manages the `RoutineManager` under the hood, all you have to do is call `routines.run()` inside your `stateMachineUpdate()` loop. + +Here is a real-world example of an **Auto-Score Macro** mapped to the `Y` button: + +```java +@Override +protected void stateMachineUpdate() { + + // Drive code continues running perfectly while routines run! + drive.setDriveVectors(-g1.left_stick_y(), g1.left_stick_x(), g1.right_stick_x()); + + // 1. Trigger the Routine + if (g1.yWasPressed() && !routines.isBusy()) { + + routines.run( + Routine.sequence( + // Step 1: Tell the lift to go up + Routine.instant(() -> lift.setTargetState(LiftState.SCORING_HIGH)), + + // Step 2: Wait until the lift physically reaches the top + Routine.waitUntil(() -> lift.isAtTarget()), + + // Step 3: Open the claw + Routine.instant(() -> claw.open()), + + // Step 4: Wait 250ms for the pixel/sample to fall out + Routine.wait(250), + + // Step 5: Retract the lift AND close the claw simultaneously + Routine.parallel( + Routine.instant(() -> lift.setTargetState(LiftState.RETRACTED)), + Routine.instant(() -> claw.close()) + ), + + // Step 6: Vibrate the controller to tell the driver it's done + Routine.instant(() -> g1.rumble(500)) + ) + ); + } +} +``` + +--- + +## 🛑 Driver Overrides & Canceling (Crucial for Safety) + +Because routines run automatically, **you must give your driver a way to cancel them** in case something goes wrong (e.g., the lift gets stuck on another robot). + +Use `routines.cancelAll()` mapped to a panic button (like `B`) to instantly kill all running scripts and return manual control to the driver. + +```java +@Override +protected void stateMachineUpdate() { + + // PANIC BUTTON: Instantly kill the macro + if (g1.bWasPressed()) { + routines.cancelAll(); + lift.setTargetState(LiftState.RETRACTED); // Force hardware to a safe state + g1.rumbleBlips(2); // Audio/Haptic feedback that the macro died + } + + // ... routine triggering logic ... +} +``` + +--- + +## 💡 Best Practices + +1. **Check `!routines.isBusy()`**: Always ensure a routine isn't currently running before starting a new one, otherwise you might trigger the "Auto-Score" sequence twice at the same time and cause conflicting hardware commands. +2. **Use Subsystem States, Not Hardware!**: Do **not** put raw hardware commands (like `motor.setPower(1)`) inside a `Routine.instant()`. Always use your Subsystem's `setTargetState()` methods. The Subsystem should still be in charge of calculating its own PID and limits. +3. **Use `waitUntil` instead of `wait`**: Whenever possible, wait for a sensor or an encoder position rather than a fixed amount of time. If your battery is low, a `wait(500)` might not be enough time for the lift to go up. A `waitUntil(() -> lift.isAtTarget())` works perfectly on *every* battery voltage. \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java index ac81a53..782be77 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java @@ -12,6 +12,33 @@ public class Constants { public static int TARGET_FPS = 0; // No target FPS (UNCAPPED) } + public static class Gamepad { + public static double STICK_DEADBAND = 0.05; + public static double TRIGGER_THRESHOLD = 0.3; + } + public static class PIDController { + public static double VOLTAGE_REFERENCE = 12.0; // Voltage PID Controller was tuned at + public static double VOLTAGE_CHECK_INTERVAL_MS = 500.0; // Every half second + } + + @Configurable + public static class LIMELIGHT { + public static String NAME = "limelight"; + public static int POLL_RATE = 100; + + + // Trig Distance + public static double MOUNT_HEIGHT_IN = 0.0; + public static double TARGET_HEIGHT_IN = 29.5; // DECODE SEAON GOAL HEIGHT + public static double MOUNT_ANGLE_DEGREES = 0.0; + + public static double AREA_COEFF = 74.34095; + public static double AREA_EXPONENT = -0.518935; + public static double AREA_OFFSET = 4.0; + + public static double STALE_DATA_TIMEOUT_SEC = 0.5; + } + @Configurable public static class DRIVE { // Hardware Names diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java index 9d8e830..0042460 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java @@ -5,7 +5,8 @@ import org.firstinspires.ftc.teamcode.lib.BaseOpMode; import org.firstinspires.ftc.teamcode.subsys.Drivetrain; import org.firstinspires.ftc.teamcode.util.AutoTransfer; -@TeleOp(name = "Competition TeleOp", group = "Main") + +@TeleOp(name = "Basic TeleOp", group = "Main") public class teleOp extends BaseOpMode { public enum GlobalState { @@ -32,14 +33,24 @@ public class teleOp extends BaseOpMode { @Override protected void stateMachineUpdate() { - drive.setDriveVectors(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + if (g1.backWasPressed()) { + + routines.cancelAll(); + + robotState = GlobalState.IDLE; + + drive.setTargetState(Drivetrain.DriveState.NORMAL); + + g1.rumbleBlips(2); + } + drive.setDriveVectors(-g1.left_stick_y(), g1.left_stick_x(), g1.right_stick_y()); switch (robotState) { case IDLE: drive.setTargetState(Drivetrain.DriveState.NORMAL); - if (gamepad1.aWasPressed()) { + if (g1.aWasPressed()) { robotState = GlobalState.SCORING; } break; @@ -47,7 +58,7 @@ public class teleOp extends BaseOpMode { case SCORING: drive.setTargetState(Drivetrain.DriveState.PRECISION); - if (gamepad1.bWasPressed()) { + if (g2.bWasPressed()) { robotState = GlobalState.IDLE; } break;