From 1b871665f03cf3e1d2c5766ba5f7c629b8196e1d Mon Sep 17 00:00:00 2001 From: Matt Hanson Date: Fri, 13 Feb 2026 19:01:07 -0500 Subject: [PATCH 1/6] shooter ka --- .../2026-02-13-shooter-review.md | 49 +++ .../tuning/FlywheelsFeedbackTuning.java | 86 ++++ .../tuning/FlywheelsFeedforwardTuning.java | 374 ++++++++++++++++++ .../opmodes/tuning/FlywheelsTuningBase.java | 50 +++ .../ftc/teamcode/robot/Shooter.java | 273 ++++++++----- .../ftc/teamcode/robot/ShooterMotor.java | 113 ++++++ .../ftc/teamcode/utils/HubHelper.java | 59 +++ 7 files changed, 908 insertions(+), 96 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-13-shooter-review.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedbackTuning.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedforwardTuning.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsTuningBase.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/HubHelper.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-13-shooter-review.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-13-shooter-review.md new file mode 100644 index 00000000000..e4b9560e01d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-13-shooter-review.md @@ -0,0 +1,49 @@ +# Progress Report — 2026-02-13 + +## Session Goal + +Review recent changes to the Shooter subsystem (Shooter.java and ShooterMotor.java) covering the voltage-based feedforward rewrite, ShooterMotor extraction, and acceleration profiling. + +## Bugs Found and Fixed + +### Copy-paste error: `leftKA` passed to right motor + +In `Shooter.update()`, the right motor's `update()` call was receiving `leftKA` instead of `rightKA` for the acceleration feedforward gain. Since the two motors' kA values differ by ~30% (`12.5/2380.5` vs `12.5/1835.8`), the right motor's acceleration compensation was consistently wrong. + +**Fix:** Changed to `rightKA`. + +### Uninitialized `lastTimeNanos` causing massive first `dt` + +`lastTimeNanos` defaulted to 0. On the first call to `update()` with a nonzero target, `dt` would equal `System.nanoTime() / 1e9` (seconds since boot — potentially thousands of seconds), causing `profiledVelocity` to massively overshoot the target on the first iteration. The same problem recurred any time `shooterVelocity` was set to 0 and back, because the early return skipped updating `lastTimeNanos`. + +**Fix:** Added a first-call guard: when `lastTimeNanos == 0`, seed it with the current time and return without running the control loop. + +## Design Changes + +### Velocity filter runs continuously (even when shooter is off) + +The user wanted the smoothed velocity filters to stay current while the motors coast, so that on resume the profile can seed from the actual coasting speed rather than ramping from zero. + +**Approach:** Split `ShooterMotor.update()` into two methods: +- `updateFilter(double alpha)` — reads the encoder and updates the IIR low-pass filter. Called every loop unconditionally. +- `update(...)` — runs feedforward + feedback control using the already-smoothed value. Only called when the shooter is active. + +In `Shooter.update()`, timing and filter updates now run before the `shooterVelocity == 0` check. When velocity goes to zero, the motors coast but the filters keep tracking. On resume, `profiledVelocity` is seeded from `Math.max(left.smoothActualVelocity, right.smoothActualVelocity)` so the acceleration profile picks up from the faster motor's actual speed. + +### Eliminated redundant `acceleration` / `prevProfiledVelocity` + +The user noticed two acceleration variables: `accel` (clamped profile acceleration) and `acceleration` (derived from the delta of `profiledVelocity` and `prevProfiledVelocity`). Outside of the snap-to-target case, these are algebraically identical. During snap, the desired value is 0. Simplified to just use `accel` directly (set to 0 on snap), which also eliminated the `prevProfiledVelocity` field entirely. + +### Added `isReady()` method + +Added a method to indicate the shooter is up to speed and safe to fire. Returns true when: +1. `shooterVelocity != 0` (target is set) +2. `profiledVelocity == shooterVelocity` (profile has snapped to target, accel = 0) +3. Both motors' smoothed velocities are within `readyThreshold` (default 25 tps) of the target + +`readyThreshold` is a `@Config` static field, tunable via FTC Dashboard. + +## Minor Cleanup + +- Removed unused `voltageSensor` field from Shooter (voltage is now read once in the constructor as a local) +- Removed unused `VoltageSensor` import from ShooterMotor diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedbackTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedbackTuning.java new file mode 100644 index 00000000000..3735c6dce09 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedbackTuning.java @@ -0,0 +1,86 @@ +package org.firstinspires.ftc.teamcode.opmodes.tuning; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; + +/** + * Interactive tuning opmode for flywheel kP and IIR low-pass filter alpha values. + * + *

Paste kS and kV values from {@link FlywheelsFeedforwardTuning} into the static fields + * below (or edit them live via FTC Dashboard). Then adjust {@link #targetTPS}, + * {@link #targetAlpha}, {@link #velocityAlpha}, and {@link #kP} while watching the telemetry + * graphs to find values that track the target velocity quickly without excessive oscillation. + * + *

Set {@link #useLeftMotor} to {@code true} for the left motor or {@code false} for the + * right motor. The feedforward model is {@code voltage = kS + kV * smoothedTarget}, and the + * feedback term is {@code kP * (smoothedTarget - smoothedVelocity)}. + */ +@Config +@TeleOp(name = "Flywheels Feedback Tuning", group = "Tuning") +public class FlywheelsFeedbackTuning extends FlywheelsTuningBase { + + public static boolean useLeftMotor = true; + public static double targetTPS = 0; + public static double targetAlpha = 0.02; + public static double velocityAlpha = 0.05; + public static double kP = 0.002; + + public static double leftKS = 1.5109; + public static double leftKV = 0.005178; + public static double rightKS = 1.3725; + public static double rightKV = 0.004908; + + @Override + protected void runOpModeInternal() throws InterruptedException { + telemetry.addLine("Ready. Press START to begin."); + telemetry.addLine("Adjust values via FTC Dashboard."); + telemetry.update(); + + waitForStart(); + if (isStopRequested()) return; + + double smoothedTarget = 0; + double smoothedVelocity = 0; + + while (opModeIsActive()) { + DcMotorEx activeMotor = motors[useLeftMotor ? 0 : 1]; + DcMotorEx idleMotor = motors[useLeftMotor ? 1 : 0]; + double kS = useLeftMotor ? leftKS : rightKS; + double kV = useLeftMotor ? leftKV : rightKV; + + idleMotor.setPower(0); + + double rawVelocity = Math.abs(activeMotor.getVelocity()); + + smoothedTarget = targetAlpha * targetTPS + (1 - targetAlpha) * smoothedTarget; + // An IIR filter approaches its target asymptotically (never truly arrives). + // Snap to the exact target once we're within 1% to avoid lingering error. + if (targetTPS != 0 && Math.abs(smoothedTarget - targetTPS) / targetTPS < 0.01) { + smoothedTarget = targetTPS; + } + smoothedVelocity = velocityAlpha * rawVelocity + (1 - velocityAlpha) * smoothedVelocity; + + if (targetTPS == 0) { + activeMotor.setPower(0); + smoothedTarget = 0; + smoothedVelocity = 0; + } else { + double feedforward = kS + kV * smoothedTarget; + double feedback = kP * (smoothedTarget - smoothedVelocity); + double voltage = feedforward + feedback; + double batteryVoltage = module.getInputVoltage(VoltageUnit.VOLTS); + activeMotor.setPower(voltage / batteryVoltage); + } + + telemetry.addData("Motor", useLeftMotor ? "Left" : "Right"); + telemetry.addData("Raw Target", "%.1f", targetTPS); + telemetry.addData("Smoothed Target", "%.1f", smoothedTarget); + telemetry.addData("Raw Velocity", "%.1f", rawVelocity); + telemetry.addData("Smoothed Velocity", "%.1f", smoothedVelocity); + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedforwardTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedforwardTuning.java new file mode 100644 index 00000000000..73ca5fd7e64 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsFeedforwardTuning.java @@ -0,0 +1,374 @@ +package org.firstinspires.ftc.teamcode.opmodes.tuning; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import java.util.ArrayList; + +import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit; + +/** + * Automated feedforward tuner for the dual flywheel shooter. + *

+ * When MOTORS_COUPLED is true, all motors are driven together and + * velocity is read from motors[0], producing one set of gains. When false, + * each motor is tuned independently, producing separate kS, kV, kA per motor. + *

+ * Procedure (per motor or motor group): + * 1. Ramps power slowly until the motor(s) overcome stiction (start spinning). + * 2. Steps from stiction power to full power in NUM_STEPS equal steps. + * 3. At each step, waits SETTLE_TIME_S for the speed to stabilize, then + * takes NUM_SAMPLES readings of velocity and applied voltage. + * 4. Fits a line voltage = kS + kV * velocity via least-squares regression. + * 5. Applies a step input and records the velocity rise curve. Fits + * ln(1 - w/w_final) vs t to extract the time constant tau, then + * computes kA = tau * kV. Repeats for STEP_RESPONSE_TRIALS trials. + * 6. Displays kS, kV, kA, R², and all data points until the OpMode is stopped. + */ +@Config +@TeleOp(name = "FlywheelsFeedforwardTuning", group = "Tuning") +public class FlywheelsFeedforwardTuning extends FlywheelsTuningBase { + public static int NUM_STEPS = 6; + public static double SETTLE_TIME_S = 5.0; + public static int NUM_SAMPLES = 25; + public static double STICTION_RAMP_RATE = 0.03; // power per second + public static double STICTION_THRESHOLD_TPS = 50.0; + + public static double STEP_RESPONSE_POWER = 0.80; + public static double COAST_STOP_THRESHOLD_TPS = 10.0; + public static double COAST_TIMEOUT_S = 10.0; + public static int STEP_RESPONSE_TRIALS = 3; + public static double STEP_RESPONSE_MAX_TIME_S = 4.0; + public static double OMEGA_LOWER_FRACTION = 0.05; + public static double OMEGA_UPPER_FRACTION = 0.95; + + private static class LinRegResult { + double slope, intercept, rSquared; + } + + private static LinRegResult linearRegression(double[] x, double[] y, int n) { + double sumX = 0, sumY = 0, sumXY = 0, sumX2 = 0; + for (int i = 0; i < n; i++) { + sumX += x[i]; + sumY += y[i]; + sumXY += x[i] * y[i]; + sumX2 += x[i] * x[i]; + } + LinRegResult r = new LinRegResult(); + double denom = n * sumX2 - sumX * sumX; + r.slope = (n * sumXY - sumX * sumY) / denom; + r.intercept = (sumY - r.slope * sumX) / n; + + double meanY = sumY / n; + double ssRes = 0, ssTot = 0; + for (int i = 0; i < n; i++) { + double predicted = r.intercept + r.slope * x[i]; + ssRes += (y[i] - predicted) * (y[i] - predicted); + ssTot += (y[i] - meanY) * (y[i] - meanY); + } + r.rSquared = (ssTot == 0) ? 1.0 : 1.0 - ssRes / ssTot; + return r; + } + + private static class TuneResult { + String label; + double kS, kV, kA; + double rSquared, avgStepR2, avgTau; + int validTrials; + double stictionPower, stictionVoltage; + double[] avgVelocities, avgVoltages; + } + + /** + * Runs a full tuning pass (stiction, steady-state, regression, step response). + * + * @param label display name for telemetry + * @param powerMotors motors to drive during this pass + * @param encoder motor to read velocity from + * @param module LynxModule for voltage reads + * @param passIndex 0-based pass number (for telemetry) + * @param totalPasses total number of passes (for telemetry) + * @return results, or null if the OpMode is stopped or the motor never starts + */ + private TuneResult runTuningPass(String label, DcMotorEx[] powerMotors, + DcMotorEx encoder, LynxModule module, + int passIndex, int totalPasses) throws InterruptedException { + + TuneResult result = new TuneResult(); + result.label = label; + int steps = Math.max(NUM_STEPS, 2); + + // ── Phase 1: find stiction ─────────────────────────────────────────── + double power = 0; + double lastTime = getRuntime(); + + while (opModeIsActive()) { + double now = getRuntime(); + double dt = now - lastTime; + lastTime = now; + + power += STICTION_RAMP_RATE * dt; + if (power > 1.0) power = 1.0; + + for (DcMotorEx m : powerMotors) m.setPower(power); + + double velocity = encoder.getVelocity(); + + telemetry.addData("Pass", "%s (%d/%d)", label, passIndex + 1, totalPasses); + telemetry.addData("Phase", "Finding stiction"); + telemetry.addData("Power", "%.4f", power); + telemetry.addData("Velocity (tps)", "%.1f", velocity); + telemetry.update(); + + if (Math.abs(velocity) > STICTION_THRESHOLD_TPS) { + result.stictionPower = power; + break; + } + + if (power >= 1.0) { + for (DcMotorEx m : powerMotors) m.setPower(0); + telemetry.addData("ERROR", "Motor never started. Check connections."); + telemetry.update(); + while (opModeIsActive()) sleep(100); + return null; + } + } + + if (!opModeIsActive()) return null; + + result.stictionVoltage = result.stictionPower + * module.getInputVoltage(VoltageUnit.VOLTS); + + // ── Phase 2: step through powers and collect data ──────────────────── + result.avgVelocities = new double[steps]; + result.avgVoltages = new double[steps]; + + for (int step = 0; step < steps && opModeIsActive(); step++) { + double stepPower = result.stictionPower + + (1.0 - result.stictionPower) * step / (steps - 1); + + for (DcMotorEx m : powerMotors) m.setPower(stepPower); + + // settle + double settleStart = getRuntime(); + while (opModeIsActive() + && (getRuntime() - settleStart) < SETTLE_TIME_S) { + telemetry.addData("Pass", "%s (%d/%d)", label, passIndex + 1, totalPasses); + telemetry.addData("Phase", + "Step %d/%d – Settling", step + 1, steps); + telemetry.addData("Power", "%.4f", stepPower); + telemetry.addData("Velocity (tps)", "%.1f", + encoder.getVelocity()); + telemetry.addData("Time left", "%.1f s", + SETTLE_TIME_S - (getRuntime() - settleStart)); + telemetry.update(); + } + + // sample + double totalVel = 0; + double totalV = 0; + int samples = Math.max(NUM_SAMPLES, 1); + for (int s = 0; s < samples && opModeIsActive(); s++) { + double vel = encoder.getVelocity(); + double battV = module.getInputVoltage(VoltageUnit.VOLTS); + totalVel += vel; + totalV += stepPower * battV; + + telemetry.addData("Pass", "%s (%d/%d)", label, passIndex + 1, totalPasses); + telemetry.addData("Phase", + "Step %d/%d – Sampling %d/%d", + step + 1, steps, s + 1, samples); + telemetry.addData("Power", "%.4f", stepPower); + telemetry.addData("Velocity (tps)", "%.1f", vel); + telemetry.addData("Voltage (V)", "%.3f", stepPower * battV); + telemetry.update(); + + sleep(20); + } + + result.avgVelocities[step] = totalVel / samples; + result.avgVoltages[step] = totalV / samples; + } + + for (DcMotorEx m : powerMotors) m.setPower(0); + + if (!opModeIsActive()) return null; + + // ── Phase 3: least-squares fit voltage = kS + kV · velocity ───────── + LinRegResult fit = linearRegression( + result.avgVelocities, result.avgVoltages, steps); + result.kV = fit.slope; + result.kS = fit.intercept; + result.rSquared = fit.rSquared; + + // ── Phase 4: step response for kA ──────────────────────────────────── + double stepVoltage = STEP_RESPONSE_POWER + * module.getInputVoltage(VoltageUnit.VOLTS); + double wFinal = (stepVoltage - result.kS) / result.kV; + + ArrayList tauValues = new ArrayList<>(); + ArrayList stepRSquaredValues = new ArrayList<>(); + + if (wFinal > 0) { + for (int trial = 0; trial < STEP_RESPONSE_TRIALS && opModeIsActive(); trial++) { + // Coast to stop + for (DcMotorEx m : powerMotors) m.setPower(0); + double coastStart = getRuntime(); + while (opModeIsActive()) { + double vel = Math.abs(encoder.getVelocity()); + telemetry.addData("Pass", "%s (%d/%d)", + label, passIndex + 1, totalPasses); + telemetry.addData("Phase", + "Step Response – Coasting (trial %d/%d)", + trial + 1, STEP_RESPONSE_TRIALS); + telemetry.addData("Velocity (tps)", "%.1f", vel); + telemetry.update(); + if (vel < COAST_STOP_THRESHOLD_TPS) break; + if (getRuntime() - coastStart > COAST_TIMEOUT_S) break; + sleep(10); + } + if (!opModeIsActive()) break; + sleep(200); // brief pause at rest + + // Apply step and sample + ArrayList sampleTimes = new ArrayList<>(); + ArrayList sampleVels = new ArrayList<>(); + double stepStart = getRuntime(); + for (DcMotorEx m : powerMotors) m.setPower(STEP_RESPONSE_POWER); + + while (opModeIsActive() + && (getRuntime() - stepStart) < STEP_RESPONSE_MAX_TIME_S) { + double t = getRuntime() - stepStart; + double vel = encoder.getVelocity(); + sampleTimes.add(t); + sampleVels.add(vel); + + telemetry.addData("Pass", "%s (%d/%d)", + label, passIndex + 1, totalPasses); + telemetry.addData("Phase", + "Step Response – Sampling (trial %d/%d)", + trial + 1, STEP_RESPONSE_TRIALS); + telemetry.addData("Time", "%.3f s", t); + telemetry.addData("Velocity (tps)", "%.1f", vel); + telemetry.addData("w_final (tps)", "%.1f", wFinal); + telemetry.update(); + } + + for (DcMotorEx m : powerMotors) m.setPower(0); + + // Linearized regression: ln(1 - w/w_final) = -t/tau + ArrayList regT = new ArrayList<>(); + ArrayList regY = new ArrayList<>(); + + for (int i = 0; i < sampleTimes.size(); i++) { + double ratio = sampleVels.get(i) / wFinal; + if (ratio < OMEGA_LOWER_FRACTION || ratio > OMEGA_UPPER_FRACTION) + continue; + regT.add(sampleTimes.get(i)); + regY.add(Math.log(1.0 - ratio)); + } + + if (regT.size() >= 3) { + int n = regT.size(); + double[] tArr = new double[n]; + double[] yArr = new double[n]; + for (int i = 0; i < n; i++) { + tArr[i] = regT.get(i); + yArr[i] = regY.get(i); + } + LinRegResult stepFit = linearRegression(tArr, yArr, n); + if (stepFit.slope < 0) { + tauValues.add(-1.0 / stepFit.slope); + stepRSquaredValues.add(stepFit.rSquared); + } + } + } + } + + // Compute kA from averaged tau + result.validTrials = tauValues.size(); + if (result.validTrials > 0) { + for (double t : tauValues) result.avgTau += t; + result.avgTau /= result.validTrials; + for (double r : stepRSquaredValues) result.avgStepR2 += r; + result.avgStepR2 /= result.validTrials; + result.kA = result.avgTau * result.kV; + } + + return result; + } + + @Override + protected void runOpModeInternal() throws InterruptedException { + telemetry.addData("Status", "Ready. Press Start to begin tuning."); + telemetry.update(); + + waitForStart(); + + int numPasses = MOTORS_COUPLED ? 1 : motors.length; + int steps = Math.max(NUM_STEPS, 2); + TuneResult[] results = new TuneResult[numPasses]; + + for (int pass = 0; pass < numPasses && opModeIsActive(); pass++) { + String label; + DcMotorEx[] powerMotors; + DcMotorEx encoder; + + if (MOTORS_COUPLED) { + label = "All motors"; + powerMotors = motors; + encoder = motors[0]; + } else { + label = MOTOR_NAMES[pass]; + powerMotors = new DcMotorEx[]{motors[pass]}; + encoder = motors[pass]; + } + + results[pass] = runTuningPass(label, powerMotors, encoder, + module, pass, numPasses); + if (results[pass] == null) return; + } + + // ── Display results until stopped ──────────────────────────────────── + while (opModeIsActive()) { + for (int p = 0; p < numPasses; p++) { + TuneResult r = results[p]; + String pfx = numPasses > 1 ? r.label + " " : ""; + telemetry.addData("── " + r.label + " RESULTS ──", ""); + telemetry.addData(pfx + "kS", "%.4f V", r.kS); + telemetry.addData(pfx + "kV", "%.6f V·s/tick", r.kV); + if (r.validTrials > 0) { + telemetry.addData(pfx + "kA", "%.6f V·s²/tick", r.kA); + telemetry.addData(pfx + "tau", "%.4f s", r.avgTau); + telemetry.addData(pfx + "kA R²", "%.6f", r.avgStepR2); + telemetry.addData(pfx + "kA valid trials", "%d / %d", + r.validTrials, STEP_RESPONSE_TRIALS); + } else { + telemetry.addData(pfx + "kA", "FAILED – no valid trials"); + } + telemetry.addData(pfx + "kS/kV R²", "%.6f", r.rSquared); + telemetry.addData(pfx + "stiction power", "%.4f", + r.stictionPower); + telemetry.addData(pfx + "stiction voltage", "%.3f V", + r.stictionVoltage); + telemetry.addLine(""); + telemetry.addData("── " + r.label + " PASTE ──", ""); + telemetry.addData(pfx + " kS =", "%.4f", r.kS); + telemetry.addData(pfx + " kV =", "%.6f", r.kV); + telemetry.addData(pfx + " kA =", "%.6f", r.kA); + telemetry.addLine(""); + telemetry.addData("── " + r.label + " DATA POINTS ──", ""); + for (int i = 0; i < steps; i++) { + telemetry.addData( + String.format("%sStep %d", pfx, i + 1), + "%.1f tps @ %.3f V", + r.avgVelocities[i], r.avgVoltages[i]); + } + telemetry.addLine(""); + } + telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsTuningBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsTuningBase.java new file mode 100644 index 00000000000..88d9b686591 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/tuning/FlywheelsTuningBase.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.opmodes.tuning; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; + +import org.firstinspires.ftc.teamcode.utils.DashboardTelemetryPacketAccess; +import org.firstinspires.ftc.teamcode.utils.HubHelper; + +public abstract class FlywheelsTuningBase extends LinearOpMode { + public static boolean MOTORS_COUPLED = false; + public static String[] MOTOR_NAMES = {"shooterMotorLeft", "shooterMotorRight"}; + public static DcMotorSimple.Direction[] MOTOR_DIRECTIONS = { + DcMotorSimple.Direction.FORWARD, DcMotorSimple.Direction.REVERSE + }; + + protected DcMotorEx[] motors; + protected DashboardTelemetryPacketAccess packetAccess; + protected LynxModule module; + + protected void initHardware() { + motors = new DcMotorEx[MOTOR_NAMES.length]; + for (int i = 0; i < MOTOR_NAMES.length; i++) { + motors[i] = hardwareMap.get(DcMotorEx.class, MOTOR_NAMES[i]); + motors[i].setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER); + motors[i].setDirection(MOTOR_DIRECTIONS[i]); + motors[i].setZeroPowerBehavior(DcMotorEx.ZeroPowerBehavior.FLOAT); + } + + packetAccess = new DashboardTelemetryPacketAccess(); + telemetry = new MultipleTelemetry(telemetry, packetAccess.dashboardTelemetry); + + module = HubHelper.getHubForMotor(motors[0], hardwareMap); + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + public void stopMotors() { + for (DcMotorEx m : motors) m.setPower(0); + } + + @Override + public final void runOpMode() throws InterruptedException { + initHardware(); + runOpModeInternal(); + } + + protected abstract void runOpModeInternal() throws InterruptedException; +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 092f188bd6a..7105ea0a9a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -1,25 +1,24 @@ package org.firstinspires.ftc.teamcode.robot; import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.HardwareMap; - import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.opmodes.tuning.FlywheelsFeedforwardTuning; /** * Subsystem controlling the dual-flywheel ball shooter and its tilt servo. * - *

The shooter uses two motors ({@code "shooterMotorLeft"} and {@code "shooterMotorRight"}) - * spinning in opposite directions to launch balls, and a servo ({@code "tiltServo"}) to - * adjust the launch angle for near vs. far targets. + *

The shooter uses two {@link ShooterMotor} instances ({@code "shooterMotorLeft"} and + * {@code "shooterMotorRight"}) spinning in opposite directions to launch balls, and a + * servo ({@code "tiltServo"}) to adjust the launch angle for near vs. far targets. * - *

Velocity control uses a feedforward + proportional feedback loop with exponential - * smoothing (see {@link #setShooterVelocity}). The feedforward values - * ({@link #feedLeftForward}, {@link #feedRightForward}) set a baseline power proportional - * to the desired speed, while the feedback term corrects any error between the smoothed - * target and actual velocities using gain {@link #kp}. + *

Velocity control uses a voltage-based feedforward + proportional feedback loop with + * exponential smoothing (see {@link #update}). The feedforward model is + * {@code voltage = kS + kV * velocity}, and the feedback term corrects any error between + * the smoothed target and actual velocities using gain {@link #kp}. The total voltage is + * converted to motor power by dividing by the current battery voltage. * *

Fields annotated with {@code @Config} are tunable via FTC Dashboard. * @@ -28,42 +27,109 @@ @Config public class Shooter { - public final DcMotorEx shooterMotorLeft; - public final DcMotorEx shooterMotorRight; + private final ShooterMotor left; + private final ShooterMotor right; public final Servo tiltServo; - static public double nearTiltPosition = .03;//for testing - static public double farTiltPosition = .15;//for testing - static public double homeTiltPosition = 0;//for testing - /* *****************BALLPARK VELOCITY OF ENCODED SHOOTER MOTORS + private final Telemetry telemetry; + /** + * Tilt servo position for near-target shots. + */ + static public double nearTiltPosition = .03; + /** + * Tilt servo position for far-target shots. + */ + static public double farTiltPosition = .15; + /** + * Tilt servo home (flat) position. + */ + static public double homeTiltPosition = 0; -/*The near target as measured by tachometer is between 1700 and 1900 rpm so say 1800 (as measured by Mr. Beckstead on 1/21/2026). -The far target as measured by tachometer is between 3600 and 3700 rpm so say 3650 + /*The near target as measured by tachometer is between 1700 and 1900 rpm so say 1800 (as measured by Mr. Beckstead on 1/21/2026). + The far target as measured by tachometer is between 3600 and 3700 rpm so say 3650 -So, velocity for near target = (1800 rotations per minute /28 ticks per seconds)/60 seconds = 840 tps -So, velocity for far target = (3650 rotations per second/28 ticks per seconds)/60 seconds = 1,703 tps + So, velocity for near target = (1800 rotations per minute /28 ticks per seconds)/60 seconds = 840 tps + So, velocity for far target = (3650 rotations per second/28 ticks per seconds)/60 seconds = 1,703 tps - */ + */ static public double nearShooterVelocity = 650;//tps for use with encoders to set shooter speed static public double farShooterVelocity = 945;//tps for use with encoders to set shooter speed + /** + * Current target velocity (ticks per second). Set before calling {@link #update}. + */ static public double shooterVelocity = 0; - static public double maxShooterVelocityLeft = 2350;//tps when power set to 1 - static public double maxShooterVelocityRight = 2460;//tps when power set to 1 - double smoothTargetShooterVelocity = 0; - double smoothActualLeftShooterVelocity = 0; - double smoothActualRightShooterVelocity = 0; - public static double actualMotorPower = 0; + /** + * Maximum velocity error (ticks/sec) for {@link #isReady()} to return true. + */ + static public double readyThreshold = 25; + /** + * Proportional gain for the feedback term (volts per tick/sec error). + */ static public double kp = 0.002; - static public double smoothingFactor = .1; - public double shooterPower = 1; - static public double speed = .2; + /** + * The cutoffHz used for the first order IIR LPF of motor measurement + */ + static public double cutoffHz = 8; - static public double feedLeftForward = .41; - static public double feedRightForward = .35; - //********************************************* + /** + * Left motor static friction voltage (volts). From feedforward tuning. + */ + static public double leftKS = 0.6647; + /** + * Left motor velocity gain (volts per tick/sec). From feedforward tuning. + */ + static public double leftKV = 12.5 / 2377.3; + /** + * Left motor acceleration gain (volts per tick/sec²). From feedforward tuning. + */ + static public double leftKA = 12.5 / 2380.5; + /** + * Right motor static friction voltage (volts). From feedforward tuning. + */ + static public double rightKS = 0.8157; + /** + * Right motor velocity gain (volts per tick/sec). From feedforward tuning. + */ + static public double rightKV = 12.5 / 2742.4; + /** + * Right motor acceleration gain (volts per tick/sec²). From feedforward tuning. + */ + static public double rightKA = 12.5 / 1835.8; + + // ── Setpoint profile ──────────────────────────────────────────────── + /** + * Maximum profiled acceleration in ticks/s². + *

+ * The physical upper bound from a standstill is + * {@code (V_battery - kS) / kA}. Derate to ~80 % of that value + * (using a conservative battery voltage, e.g. 11 V) to leave headroom + * for the feedback term and for battery sag during a match: + * 

+     *   maxAccelTPS2 ≈ 0.8 * (11.0 - kS) / kA
+     *
+ * Setting this higher than the physical limit causes the profile to + * outrun the motor and forces the feedback controller to compensate. + */ + public static double maxAccelTPS2 = Math.min((11.0 - leftKS) / leftKA, (11.0 - rightKS) / rightKA); + /** + * Exponential approach time constant (seconds). Controls how smoothly + * the profiled setpoint settles onto the target velocity. + *

+ * Set this to {@code kA / kV} — the motor's physical time constant, + * which {@link FlywheelsFeedforwardTuning} + * already reports as {@code avgTau}. This makes the profile match the + * motor's natural dynamics so the feedforward does most of the work. + * Increase beyond {@code kA / kV} if the feedforward fit was noisy + * (low R² or few valid step-response trials). + */ + public static double approachTau = Math.max(leftKA / leftKV, rightKA / rightKV); + + private long lastTimeNanos; + private double profiledVelocity; + private boolean stopped = true; /** * Constructs a Shooter subsystem and maps the motors and tilt servo from hardware. @@ -71,79 +137,95 @@ public class Shooter { * * @param hardwareMap the robot's hardware map containing {@code "shooterMotorLeft"}, * {@code "shooterMotorRight"}, and {@code "tiltServo"} + * @param telemetry telemetry instance for logging velocity diagnostics */ - public Shooter(HardwareMap hardwareMap) { - - shooterMotorLeft = hardwareMap.get(DcMotorEx.class, "shooterMotorLeft"); - shooterMotorRight = hardwareMap.get(DcMotorEx.class, "shooterMotorRight"); + public Shooter(HardwareMap hardwareMap, Telemetry telemetry) { + this.telemetry = telemetry; - //enable the encoders on the shooter motors - // shooterMotorLeft.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); - // shooterMotorRight.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER); + double voltage = hardwareMap.voltageSensor.iterator().next().getVoltage(); - tiltServo = hardwareMap.get(Servo.class, "tiltServo");//controls angle of shooters + left = new ShooterMotor(hardwareMap, "shooterMotorLeft", telemetry, + DcMotorSimple.Direction.FORWARD, voltage); + right = new ShooterMotor(hardwareMap, "shooterMotorRight", telemetry, + DcMotorSimple.Direction.REVERSE, voltage); - shooterMotorRight.setDirection(DcMotorSimple.Direction.REVERSE); - shooterMotorLeft.setDirection(DcMotorSimple.Direction.FORWARD); + tiltServo = hardwareMap.get(Servo.class, "tiltServo"); } /** - * Runs the feedforward + feedback velocity control loop for both shooter motors. + * Runs one iteration of the feedforward + feedback velocity control loop. * *

Applies exponential smoothing to both the target and actual velocities, then * computes a proportional correction term added to the static feedforward power. - * If {@code shooterVelocity} is 0, both motors are stopped immediately. - * - * @param shooterVelocity desired flywheel velocity in ticks per second - * @param telemetry telemetry instance for logging velocity diagnostics + * If {@link #shooterVelocity} is 0, both motors are stopped immediately. + * Set {@link #shooterVelocity} before calling this method to change the target speed. */ - public void setShooterVelocity(double shooterVelocity, Telemetry telemetry) { + public void update() { telemetry.addData("Target T/S", shooterVelocity); - // shooterMotorLeft.setVelocity(shooterVelocity);//if using encoders, use this code. Otherwise, set power - // shooterMotorRight.setVelocity(shooterVelocity);//if using encoders, use this code. Otherwise, set power -// shooterMotorLeft.setPower(shooterPower); -// shooterMotorRight.setPower(shooterPower); - - //fail safe - if (shooterVelocity == 0){ - shooterMotorLeft.setPower(0); - shooterMotorRight.setPower(0); - return; - } - //convert target velocity to feed forward - - // double feedLeftForward = shooterVelocity/maxShooterVelocityLeft;//target/max (moved to class variable) - //double feedRightForward = shooterVelocity/maxShooterVelocityRight;//target/max (moved to class variable) - - //measure current velocity - telemetry.addData("FeedLeftForward ", feedLeftForward); - telemetry.addData("FeedRightForward ", feedRightForward); - double actualLeftVelocity = shooterMotorLeft.getVelocity();//removed mw's negative from shooterMotorLeft - double actualRightVelocity = shooterMotorRight.getVelocity();//removed mw's negative from shooterMotorRight - - //calculate smoothing - smoothTargetShooterVelocity = (shooterVelocity * smoothingFactor) + (1 - smoothingFactor) * smoothTargetShooterVelocity; - smoothActualLeftShooterVelocity = (actualLeftVelocity * smoothingFactor) + (1 - smoothingFactor) * smoothActualLeftShooterVelocity; - smoothActualRightShooterVelocity = (actualRightVelocity * smoothingFactor) + (1 - smoothingFactor) * smoothActualRightShooterVelocity; - - //how fast am I going versus how fast I want to go - double feedbackLeft = (smoothTargetShooterVelocity - smoothActualLeftShooterVelocity) * kp; - double feedbackRight = (smoothTargetShooterVelocity - smoothActualRightShooterVelocity) * kp; - - telemetry.addData("FeedbackLeft", feedbackLeft); - telemetry.addData("FeedbackRight", feedbackRight); - - //set the power of the shooter motors to their current speed plus or minus the feed foward factors - //so it is constantly adjusting itself to the target - //shooterMotorLeft.setPower(feedbackLeft + feedLeftForward); - actualMotorPower = (feedbackLeft + feedLeftForward) * 1000; - shooterMotorLeft.setPower(feedbackLeft + feedLeftForward); - shooterMotorRight.setPower(feedbackRight + feedRightForward); - //shooterMotorLeft.setPower(speed); - // shooterMotorRight.setPower(speed); - } + // ── Timing ────────────────────────────────────────────────────── + long now = System.nanoTime(); + if (lastTimeNanos == 0) { + lastTimeNanos = now; + return; + } + double dt = (now - lastTimeNanos) / 1e9; + lastTimeNanos = now; + if (dt < 1e-6) return; + + // ── Velocity filter (always runs so readings stay fresh) ───────── + // Exact discrete-time first-order LPF: alpha = 1 - exp(-dt/tau). + // This keeps the effective cutoff frequency constant regardless of loop rate. + double filterTau = 1.0 / (2.0 * Math.PI * cutoffHz); + double alpha = 1.0 - Math.exp(-dt / filterTau); + left.updateFilter(alpha); + right.updateFilter(alpha); + + // ── Coast when target is zero ──────────────────────────────────── + if (shooterVelocity == 0) { + left.stop(); + right.stop(); + stopped = true; + return; + } + + // ── Seed profile from coasting velocity on resume ──────────────── + if (stopped) { + profiledVelocity = Math.max(left.smoothActualVelocity, + right.smoothActualVelocity); + stopped = false; + } + + // ── Clamped-exponential setpoint profile ───────────────────────── + // Far from target: acceleration clamped at maxAccelTPS2 (linear ramp). + // Close to target: acceleration = error/tau, decaying smoothly to zero. + // The transition is continuous in acceleration — no step change for kA. + double error = shooterVelocity - profiledVelocity; + double rawAccel = error / approachTau; + double accel = Math.max(-maxAccelTPS2, Math.min(maxAccelTPS2, rawAccel)); + profiledVelocity += accel * dt; + + // Snap to target once negligibly close (avoids asymptotic creep) + if (Math.abs(shooterVelocity - profiledVelocity) < 1.0) { + profiledVelocity = shooterVelocity; + accel = 0; + } + + left.update(profiledVelocity, accel, leftKS, leftKV, leftKA, kp); + right.update(profiledVelocity, accel, rightKS, rightKV, rightKA, kp); + } + /** + * Returns true when the shooter is up to speed and ready to fire. + * Requires the acceleration profile to have settled and both motors' + * smoothed velocities to be within {@link #readyThreshold} of the target. + */ + public boolean isReady() { + return shooterVelocity != 0 + && profiledVelocity == shooterVelocity + && Math.abs(left.smoothActualVelocity - shooterVelocity) < readyThreshold + && Math.abs(right.smoothActualVelocity - shooterVelocity) < readyThreshold; + } /** * Sets the tilt servo to an arbitrary position. @@ -172,12 +254,11 @@ public void setFarTiltPosition(double farTiltPosition) { tiltServo.setPosition(farTiltPosition); } - /** Resets the tilt servo to the home (flat) position. */ + /** + * Resets the tilt servo to the home (flat) position. + */ public void setHomeTiltPosition() { tiltServo.setPosition(homeTiltPosition); } } - - - diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java new file mode 100644 index 00000000000..e94cbd40320 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java @@ -0,0 +1,113 @@ +package org.firstinspires.ftc.teamcode.robot; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/** + * Encapsulates per-motor state and the velocity control loop for a single flywheel motor. + * + *

Each instance tracks its own smoothed actual velocity and computes a + * feedforward + proportional feedback power output. Tunable gains are passed in + * from {@link Shooter}'s {@code @Config} static fields each loop iteration. + * + *

Telemetry diagnostics are reported automatically during {@link #update}, + * using the hardware name to distinguish left from right. + */ +@Config +public class ShooterMotor { + + static public double powerQuantum = 0.01; + + private final DcMotorEx motor; + private final String name; + private final Telemetry telemetry; + private double actualVelocity = 0; + public double smoothActualVelocity = 0; + public double actualMotorPower = 0; + private double lastSetPower = 0; + private final double batteryVoltage; + + /** + * Constructs a ShooterMotor, looking up the motor from the hardware map. + * + * @param hardwareMap the robot's hardware map + * @param name the hardware-map name for this motor (also used in telemetry) + * @param telemetry telemetry instance for logging diagnostics + * @param direction the spin direction for this motor + */ + public ShooterMotor(HardwareMap hardwareMap, String name, Telemetry telemetry, + DcMotorSimple.Direction direction, double batteryVoltage) { + this.name = name; + this.telemetry = telemetry; + this.motor = hardwareMap.get(DcMotorEx.class, name); + this.motor.setDirection(direction); + this.motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + this.batteryVoltage = batteryVoltage; + } + + /** + * Reads the encoder velocity and updates the smoothed velocity estimate. + * Call every loop iteration (even when the shooter is off) so the filter + * stays current for a smooth resume from coasting. + * + * @param alpha IIR low-pass filter coefficient (0..1) + */ + public void updateFilter(double alpha) { + actualVelocity = motor.getVelocity(); + smoothActualVelocity = (actualVelocity * alpha) + (1 - alpha) * smoothActualVelocity; + } + + /** + * Runs one iteration of the voltage-based feedforward + feedback loop for this motor. + * {@link #updateFilter} must be called first each loop to refresh the velocity estimate. + * + *

The feedforward model is {@code voltage = kS + kV * targetVelocity + kA * targetAccel}, + * matching the model from {@code FlywheelsFeedforwardTuning}. The feedback term + * {@code kP * (target - actual)} is also in volts. The total voltage is converted + * to a duty-cycle power by dividing by the current battery voltage. + * + * @param targetVelocity the target velocity (ticks/sec) + * @param targetAccel the target acceleration (ticks/sec²) + * @param kS static friction voltage (volts) + * @param kV velocity feedforward gain (volts per tick/sec) + * @param kA acceleration feedforward gain (volts per tick/sec²) + * @param kp proportional gain (volts per tick/sec error) + */ + public void update(double targetVelocity, double targetAccel, double kS, + double kV, double kA, double kp) { + double feedforward = kS * Math.signum(targetVelocity) + kV * targetVelocity + kA * targetAccel; + double feedback = kp * (targetVelocity - smoothActualVelocity); + double voltage = feedforward + feedback; + setPower(voltage / batteryVoltage); + + telemetry.addData(name + " Actual Velocity", actualVelocity); + telemetry.addData(name + " Smooth Velocity", smoothActualVelocity); + telemetry.addData(name + " Feedforward (V)", feedforward); + telemetry.addData(name + " Feedback (V)", feedback); + telemetry.addData(name + " Motor Power", actualMotorPower); + } + + /** + * Quantizes power to {@link #powerQuantum} and writes to the motor only if changed. + */ + private void setPower(double power) { + double scale = 1.0 / powerQuantum; + actualMotorPower = Math.round(power * scale) / scale; + if (actualMotorPower != lastSetPower) { + motor.setPower(actualMotorPower); + lastSetPower = actualMotorPower; + } + } + + /** + * Stops the motor by setting power to 0. + */ + public void stop() { + setPower(0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/HubHelper.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/HubHelper.java new file mode 100644 index 00000000000..43e216e1201 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/HubHelper.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.utils; + +import android.util.Log; + +import com.qualcomm.hardware.lynx.LynxDcMotorController; +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.lang.reflect.Method; +import java.util.List; + +public class HubHelper { + + /** + * Attempt to get the hub module the motor is actually attached to. + * Uses reflection to call private LynxDcMotorController.getModule(). + * Falls back to the first module in hardwareMap if anything goes wrong. + */ + public static LynxModule getHubForMotor(DcMotorEx motor, HardwareMap hardwareMap) { + try { + Object controller = motor.getController(); + + if (controller instanceof LynxDcMotorController) { + LynxDcMotorController lynxController = (LynxDcMotorController) controller; + + // getModule() may be declared on a superclass (e.g. LynxController), + // so walk the hierarchy to find it. + Method getModuleMethod = null; + for (Class cls = LynxDcMotorController.class; cls != null; cls = cls.getSuperclass()) { + try { + getModuleMethod = cls.getDeclaredMethod("getModule"); + break; + } catch (NoSuchMethodException ignored) {} + } + if (getModuleMethod == null) throw new NoSuchMethodException("getModule"); + getModuleMethod.setAccessible(true); + + Object module = getModuleMethod.invoke(lynxController); + if (module instanceof LynxModule) { + return (LynxModule) module; + } + } + + // If any check fails, fall through to fallback + } catch (Exception e) { + Log.e("HubHelper", "Failed to get hub for motor"); + // after logging, fallback + } + + // Fallback: return the first hub in hardware map + List hubs = hardwareMap.getAll(LynxModule.class); + if (!hubs.isEmpty()) { + return hubs.get(0); + } + + throw new IllegalStateException("No LynxModules found in hardwareMap"); + } +} From 942ae2a4151257d6aced7e6c4beb56f1bed78210 Mon Sep 17 00:00:00 2001 From: Matt Hanson Date: Fri, 13 Feb 2026 19:44:04 -0500 Subject: [PATCH 2/6] get it compiling and add AGENTS.md --- AGENTS.md | 176 ++++++++++++++++++ .../ftc/teamcode/robot/Shooter.java | 2 +- .../ftc/teamcode/robot/ShooterMotor.java | 10 +- .../ftc/teamcode/robot/SpiritAutoBlueFar.java | 63 +++---- .../teamcode/robot/SpiritAutoBlueNear.java | 72 ++++--- .../ftc/teamcode/robot/SpiritAutoNear.java | 70 +++---- .../ftc/teamcode/robot/SpiritAutoRedFar.java | 56 +++--- .../ftc/teamcode/robot/SpiritAutoRedNear.java | 88 ++++----- .../ftc/teamcode/robot/SpiritTeleop2.java | 90 ++++----- .../utils/DashboardTelemetryPacketAccess.java | 52 ++++++ 10 files changed, 427 insertions(+), 252 deletions(-) create mode 100644 AGENTS.md create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/DashboardTelemetryPacketAccess.java diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 00000000000..0b4391e4e2c --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,176 @@ +# AGENTS.md - Team Spirit FTC Robotics + +## Project Overview + +This is an FTC (FIRST Tech Challenge) robotics project for Team Spirit, built on Road Runner v1.0. The project is an Android application that deploys to an FTC Robot Controller REV Control Hub. Team code lives in the `TeamCode` module. + +## Build Commands + +All commands use Gradle with the Android Gradle Plugin. Run from the project root. + +```bash +# Build entire project +./gradlew build + +# Build debug APK only +./gradlew assembleDebug + +# Clean and rebuild +./gradlew clean assembleDebug +``` + +There are no unit tests in this project. Testing is done by deploying to the robot and running OpModes via the FTC Driver Station. + +## Code Style Guidelines + +### File Organization + +- Package declaration: `org.firstinspires.ftc.teamcode` +- Subpackages: `robot`, `opmodes.teleop`, `opmodes.auto`, `opmodes.tuning`, `utils`, `messages`, `tuning` +- One public class per file +- Filename matches class name + +### Imports + +Order imports by: + +1. Android/SDK imports +2. Road Runner / Acmerobotics imports +3. Qualcomm (robotcore) imports +4. Static imports +5. Team internal imports + +```java +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.teamcode.robot.Shooter; +``` + +### Naming Conventions + +- **Classes:** PascalCase (`MecanumDrive`, `SpiritTeleop2`) +- **Methods:** camelCase (`updatePoseEstimate`, `setDrivePowers`) +- **Constants:** SCREAMING_SNAKE_CASE with descriptive names (`nearTiltPosition`, `farShooterVelocity`) +- **Instance fields:** camelCase with `m` prefix optional; prefer descriptive names (`left`, `right`, `telemetry`) +- **Parameters:** camelCase (`hardwareMap`, `telemetry`) +- **Config fields (tunable):** `static public` with descriptive names + +### Formatting + +- 4-space indentation (no tabs) +- Line length: 100-120 characters max +- One statement per line +- Opening brace on same line +- Use blank lines to separate logical sections within methods + +### Annotations + +- `@Config` - Exposes static fields to FTC Dashboard for live tuning +- `@TeleOp` / `@Autonomous` - OpMode registration +- `@Override` - Always use when overriding methods + +### FTC SDK Patterns + +#### DcMotor RunMode + +- `RUN_USING_ENCODER` - Enables SDK's built-in PIDF velocity controller +- `RUN_WITHOUT_ENCODER` - Correct mode for custom feedback/feedforward; still reads encoders + +#### Feedforward (kV) Calculation + +The SDK's PIDF `F` coefficient is kV scaled by 32767: + +```java +F = 32767 * kV // e.g., max 2496 ticks/sec → F = 32767 / 2496 ≈ 13.13 +``` + +#### Gamepad Edge Detection + +Use SDK 11.0+ built-in methods: + +```java +gamepad1.leftBumperWasPressed() +gamepad1.rightBumperWasReleased() +gamepad1.xWasPressed() +``` + +#### Hub Detection + +Use `HubHelper` to determine which hub a motor is on - don't hardcode hub assumptions. + +### Class Structure + +#### Subsystems (robot package) + +- Constructor takes `HardwareMap` and optionally `Telemetry` +- `static public` fields for tunable parameters (with `@Config`) +- Update methods called each loop iteration +- Clear, descriptive Javadoc on public methods + +#### OpModes + +- Extend `LinearOpMode` for teleop/auto +- Initialize hardware in `runOpMode()` before `waitForStart()` +- Main loop uses `while (opModeIsActive())` +- Usually nothing goes after the main loop, but especially not hardware commands +- Use gamepad edge detection for button state changes + +#### Hardware Access + +```java +// Motors +DcMotorEx motor = hardwareMap.get(DcMotorEx.class, "motorName"); +motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + +// Servos +Servo servo = hardwareMap.get(Servo.class, "servoName"); +servo.setPosition(0.5); + +// Sensors +ColorSensor color = hardwareMap.get(ColorSensor.class, "color"); +``` + +### Error Handling + +- Avoid exceptions in control loops; use null checks instead +- Log errors via telemetry: `telemetry.addData("Error", message)` +- Don't catch exceptions silently - at minimum log and display +- Hardware initialization failures should stop the OpMode + +### Telemetry & Debugging + +- Use FTC Dashboard (`FtcDashboard.getInstance()`) for real-time tuning +- When using field overlay, use `DashboardTelemetryPacketAccess` to draw directly +- Don't create separate TelemetryPacket when using Dashboard telemetry +- Keep debug telemetry minimal during competition matches +- Use the format string parameter for `telemetry.addData` instead of `String.format`: + ```java + telemetry.addData("Velocity", "%.1f", value); // correct + telemetry.addData("Velocity", String.format("%.1f", value)); // avoid + ``` + +### Hardware Shutdown + +**Critical:** Never send hardware commands after `opModeIsActive()` returns false. This crashes the Control Hub. + +```java +@Override +public void runOpMode() { + DcMotorEx motor = hardwareMap.get(DcMotorEx.class, "motorName"); + motor.setPower(0) + waitForStart(); + while (opModeIsActive()) { + // only here + } + // never here +} +``` + +### Progress Reports + +When requested, create progress reports in: +`TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/` + +Include: problems discussed, decisions made, gotchas encountered, solutions reached. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 7105ea0a9a8..e0b6ec5219c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -161,7 +161,7 @@ public Shooter(HardwareMap hardwareMap, Telemetry telemetry) { * Set {@link #shooterVelocity} before calling this method to change the target speed. */ public void update() { - telemetry.addData("Target T/S", shooterVelocity); + telemetry.addData("shooterVelocity", shooterVelocity); // ── Timing ────────────────────────────────────────────────────── long now = System.nanoTime(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java index e94cbd40320..130a8e169b4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java @@ -85,11 +85,11 @@ public void update(double targetVelocity, double targetAccel, double kS, double voltage = feedforward + feedback; setPower(voltage / batteryVoltage); - telemetry.addData(name + " Actual Velocity", actualVelocity); - telemetry.addData(name + " Smooth Velocity", smoothActualVelocity); - telemetry.addData(name + " Feedforward (V)", feedforward); - telemetry.addData(name + " Feedback (V)", feedback); - telemetry.addData(name + " Motor Power", actualMotorPower); + telemetry.addData(name + " Actual Velocity", "%.0f", actualVelocity); + telemetry.addData(name + " Smooth Velocity", "%.0f", smoothActualVelocity); + telemetry.addData(name + " Feedforward (V)", "%.1f", feedforward); + telemetry.addData(name + " Feedback (V)", "%.1f", feedback); + telemetry.addData(name + " Motor Power", "%.2f", actualMotorPower); } /** diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java index 9bfe099ae4d..17d58dfd3d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java @@ -53,12 +53,11 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; + telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); - shooter.setHomeTiltPosition(); waitForStart(); @@ -88,12 +87,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(shooter.nearTiltPosition); + shooter.setNearTiltPosition(Shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(shooter.farTiltPosition); + shooter.setFarTiltPosition(Shooter.farTiltPosition); } if (gamepad1.x) { @@ -219,7 +218,7 @@ public void runOpMode() { shooter.shooterMotorLeft.setPower(.3); shooter.shooterMotorRight.setPower(.3); shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); tiltPosition = shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up @@ -230,12 +229,10 @@ public void runOpMode() { } */ // Far shot (left trigger) - - shooter.shooterMotorLeft.setPower(.425); - shooter.shooterMotorRight.setPower(.425); - shooter.shooterVelocity = Shooter.farShooterVelocity; - shooter.setShooterVelocity(Shooter.shooterVelocity, telemetry); - tiltPosition = shooter.farTiltPosition; + + Shooter.shooterVelocity = Shooter.farShooterVelocity; + shooter.update(); + tiltPosition = Shooter.farTiltPosition; rampUpTimer = getRuntime() + 2.0; currentState = State.RAMPING; @@ -245,7 +242,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -271,7 +268,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); stepStartTime = getRuntime(); launchStep++; } @@ -279,7 +276,7 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -289,7 +286,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -299,7 +296,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -309,7 +306,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -320,7 +317,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -330,7 +327,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -340,7 +337,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -350,7 +347,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -360,7 +357,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -371,7 +368,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -381,7 +378,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -391,7 +388,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -402,7 +399,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -424,8 +421,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - shooter.shooterVelocity = 0; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + Shooter.shooterVelocity = 0; + shooter.update(); //strafe to the field wall driveTimer = getRuntime() + .4; @@ -476,13 +473,7 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); - telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); - telemetry.addData("shooterVelocity ", shooter.shooterVelocity); - telemetry.addData("shooterPower ", shooter.shooterPower); - telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); - telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); - telemetry.addData("actualMotorPower ", shooter.actualMotorPower); + telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java index 771c486caa9..2d53e41cba4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java @@ -1,4 +1,5 @@ package org.firstinspires.ftc.teamcode.robot; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -39,6 +40,7 @@ public class SpiritAutoBlueNear extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; + //------------------------------------------- @Override public void runOpMode() { @@ -50,14 +52,14 @@ public void runOpMode() { State currentState = State.IDLE; - double tiltPosition = 0;//this is only used for testing, not in production - + double tiltPosition = 0;//this is only used for testing, not in production + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); + shooter.setHomeTiltPosition(); waitForStart(); @@ -75,8 +77,7 @@ public void runOpMode() { } //END KILL BUTTON************************************************************ - */ - { + */ { telemetry.addData("Test", 0); //Operate Intake: left bumper is intake and right bumper is eject if (gamepad2.left_bumper) { @@ -100,12 +101,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(shooter.nearTiltPosition); + shooter.setNearTiltPosition(Shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(shooter.farTiltPosition); + shooter.setFarTiltPosition(Shooter.farTiltPosition); } if (gamepad1.x) { @@ -224,15 +225,13 @@ public void runOpMode() { // Near shot, get shooter motors, tile and ramp up all set up and drive backwards 6 inches //shooter.shooterVelocity = shooter.nearShooterVelocity; - shooter.shooterMotorLeft.setPower(.3); - shooter.shooterMotorRight.setPower(.3); - shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - tiltPosition = shooter.nearTiltPosition; + Shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.update(); + tiltPosition = Shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up //drive backward 6 inches (i.e., .2 seconds) - driveTimer = getRuntime()+ .25; + driveTimer = getRuntime() + .25; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(1, 0 @@ -251,7 +250,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -277,7 +276,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); stepStartTime = getRuntime(); launchStep++; } @@ -285,8 +284,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { + shooter.update(); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -295,7 +294,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -305,7 +304,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -315,7 +314,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -326,7 +325,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -336,7 +335,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -346,7 +345,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -356,7 +355,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -366,7 +365,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -377,7 +376,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -387,7 +386,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -397,7 +396,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -408,7 +407,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -430,11 +429,11 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - shooter.shooterVelocity = 0; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + Shooter.shooterVelocity = 0; + shooter.update(); //strafe to the field wall - driveTimer = getRuntime()+ .75; + driveTimer = getRuntime() + .75; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(0, .5 @@ -482,13 +481,6 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); - telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); - telemetry.addData("shooterVelocity ", shooter.shooterVelocity); - telemetry.addData("shooterPower ", shooter.shooterPower); - telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); - telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); - telemetry.addData("actualMotorPower ", shooter.actualMotorPower); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java index f557e0e2354..36e93606cf5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java @@ -1,4 +1,5 @@ package org.firstinspires.ftc.teamcode.robot; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -46,6 +47,7 @@ public class SpiritAutoNear extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; + //------------------------------------------- @Override public void runOpMode() { @@ -57,14 +59,13 @@ public void runOpMode() { State currentState = State.IDLE; - double tiltPosition = 0;//this is only used for testing, not in production - + double tiltPosition = 0;//this is only used for testing, not in production + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); // Color sensor for auto-detecting alliance color ColorSensor colorSensor = hardwareMap.get(ColorSensor.class, "colorSensor"); @@ -98,8 +99,7 @@ public void runOpMode() { } //END KILL BUTTON************************************************************ - */ - { + */ { telemetry.addData("Test", 0); telemetry.addData("Alliance", isBlue ? "BLUE" : "RED"); //Operate Intake: left bumper is intake and right bumper is eject @@ -124,12 +124,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(shooter.nearTiltPosition); + shooter.setNearTiltPosition(Shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(shooter.farTiltPosition); + shooter.setFarTiltPosition(Shooter.farTiltPosition); } if (gamepad1.x) { @@ -248,15 +248,13 @@ public void runOpMode() { // Near shot, get shooter motors, tile and ramp up all set up and drive backwards 6 inches //shooter.shooterVelocity = shooter.nearShooterVelocity; - shooter.shooterMotorLeft.setPower(.3); - shooter.shooterMotorRight.setPower(.3); - shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - tiltPosition = shooter.nearTiltPosition; + Shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.update(); + tiltPosition = Shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up //drive backward 6 inches (i.e., .2 seconds) - driveTimer = getRuntime()+ .25; + driveTimer = getRuntime() + .25; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(1, 0 @@ -275,7 +273,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -301,7 +299,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); stepStartTime = getRuntime(); launchStep++; } @@ -309,8 +307,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { + shooter.update(); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -319,7 +317,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -329,7 +327,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -339,7 +337,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -350,7 +348,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -360,7 +358,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -370,7 +368,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -380,7 +378,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -390,7 +388,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -401,7 +399,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -411,7 +409,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -421,7 +419,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -432,7 +430,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -454,8 +452,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - shooter.shooterVelocity = 0; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + Shooter.shooterVelocity = 0; + shooter.update(); //strafe to the field wall — direction based on detected color double strafePower = isBlue ? 0.5 : -0.5; @@ -508,13 +506,7 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); - telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); - telemetry.addData("shooterVelocity ", shooter.shooterVelocity); - telemetry.addData("shooterPower ", shooter.shooterPower); - telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); - telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); - telemetry.addData("actualMotorPower ", shooter.actualMotorPower); + telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java index 819d9036efb..17e50b25c2b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java @@ -1,4 +1,5 @@ package org.firstinspires.ftc.teamcode.robot; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -8,7 +9,6 @@ import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.Drawing; @@ -54,11 +54,11 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); shooter.setHomeTiltPosition(); @@ -75,11 +75,9 @@ public void runOpMode() { //Far shot red target // set shooter power and apply to motors to get them spinning. set tilt, set timer, set ramp up timer - shooter.shooterMotorLeft.setPower(.425); - shooter.shooterMotorRight.setPower(.425); - shooter.shooterVelocity = Shooter.farShooterVelocity; - shooter.setShooterVelocity(Shooter.shooterVelocity, telemetry); - tiltPosition = shooter.farTiltPosition; + Shooter.shooterVelocity = Shooter.farShooterVelocity; + shooter.update(); + tiltPosition = Shooter.farTiltPosition; rampUpTimer = getRuntime() + 2.0; currentState = State.RAMPING; @@ -89,7 +87,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -115,7 +113,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); stepStartTime = getRuntime(); launchStep++; } @@ -123,7 +121,7 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -133,7 +131,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -143,7 +141,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -153,7 +151,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -164,7 +162,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -174,7 +172,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -184,7 +182,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -194,7 +192,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -204,7 +202,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -215,7 +213,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -225,7 +223,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -235,7 +233,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -246,7 +244,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -268,8 +266,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - shooter.shooterVelocity = 0; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + Shooter.shooterVelocity = 0; + shooter.update(); //strafe to the field wall driveTimer = getRuntime() + .4; @@ -318,13 +316,7 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); - telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); - telemetry.addData("shooterVelocity ", shooter.shooterVelocity); - telemetry.addData("shooterPower ", shooter.shooterPower); - telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); - telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); - telemetry.addData("actualMotorPower ", shooter.actualMotorPower); + telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java index f266723deba..f94a31a25d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java @@ -1,4 +1,5 @@ package org.firstinspires.ftc.teamcode.robot; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -39,6 +40,7 @@ public class SpiritAutoRedNear extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; + //------------------------------------------- @Override public void runOpMode() { @@ -54,11 +56,11 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); shooter.setHomeTiltPosition(); @@ -77,8 +79,7 @@ public void runOpMode() { } //END KILL BUTTON************************************************************ - */ - { + */ { telemetry.addData("Test", 0); //Operate Intake: left bumper is intake and right bumper is eject if (gamepad2.left_bumper) { @@ -102,12 +103,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(shooter.nearTiltPosition); + shooter.setNearTiltPosition(Shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(shooter.farTiltPosition); + shooter.setFarTiltPosition(Shooter.farTiltPosition); } if (gamepad1.x) { @@ -158,9 +159,6 @@ public void runOpMode() { //--------------------------END TESTING OF CAROUSEL - - - // ---------------- INTAKE + CAROUSEL SEQUENCE (GAMEPAD 1 RIGHT TRIGGER) ---------------- /* This section of the code is an intake sequence. It cycles 3 times. Upon pulling the trigger @@ -228,23 +226,21 @@ public void runOpMode() { case IDLE: // Near shot - //shooter.shooterVelocity = shooter.nearShooterVelocity; - shooter.shooterMotorLeft.setPower(.3); - shooter.shooterMotorRight.setPower(.3); - shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - tiltPosition = shooter.nearTiltPosition; - rampUpTimer = getRuntime() + 2.0; // 2-second spin-up + //shooter.shooterVelocity = shooter.nearShooterVelocity; + Shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.update(); + tiltPosition = Shooter.nearTiltPosition; + rampUpTimer = getRuntime() + 2.0; // 2-second spin-up //drive backward 6 inches (i.e., .2 seconds) - driveTimer = getRuntime()+ .25; - while (getRuntime() < driveTimer) { - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d(1, 0 + driveTimer = getRuntime() + .25; + while (getRuntime() < driveTimer) { + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d(1, 0 - ), - 0 - )); + ), + 0 + )); currentState = State.RAMPING; @@ -256,7 +252,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -282,7 +278,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); stepStartTime = getRuntime(); launchStep++; } @@ -290,8 +286,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { + shooter.update(); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -300,7 +296,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -310,7 +306,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -320,7 +316,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -331,7 +327,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -341,7 +337,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -351,7 +347,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -361,7 +357,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -371,7 +367,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -382,7 +378,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -392,7 +388,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -402,7 +398,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -413,7 +409,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -435,11 +431,11 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - shooter.shooterVelocity = 0; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + Shooter.shooterVelocity = 0; + shooter.update(); //strafe to the field wall - driveTimer = getRuntime()+ .7; + driveTimer = getRuntime() + .7; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(0, -.5 @@ -486,13 +482,7 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); - telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); - telemetry.addData("shooterVelocity ", shooter.shooterVelocity); - telemetry.addData("shooterPower ", shooter.shooterPower); - telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); - telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); - telemetry.addData("actualMotorPower ", shooter.actualMotorPower); + telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index a9ab37358c9..8cc9a8fb5a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -1,4 +1,5 @@ package org.firstinspires.ftc.teamcode.robot; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -53,6 +54,7 @@ public class SpiritTeleop2 extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; + //------------------------------------------- @Override public void runOpMode() { @@ -68,19 +70,18 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel Lift lift = new Lift(hardwareMap); - telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); - shooter.setHomeTiltPosition(); lift.homeLift(); waitForStart(); - while (opModeIsActive()){ + while (opModeIsActive()) { /* //KILL BUTTON***************************************************************** if(gamepad1.a){ @@ -95,7 +96,7 @@ public void runOpMode() { */ //LIFT ROBOT OFF OF MATT**************************************************** - if (gamepad2.a){//(gamepad2.x && gamepad2.b ){ + if (gamepad2.a) {//(gamepad2.x && gamepad2.b ){ lift.engageLift(); } @@ -128,12 +129,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(shooter.nearTiltPosition); + shooter.setNearTiltPosition(Shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(shooter.farTiltPosition); + shooter.setFarTiltPosition(Shooter.farTiltPosition); } if (gamepad1.x) { @@ -189,8 +190,7 @@ public void runOpMode() { } - - // ---------------- INTAKE + CAROUSEL SEQUENCE (GAMEPAD 1 RIGHT TRIGGER) ---------------- + // ---------------- INTAKE + CAROUSEL SEQUENCE (GAMEPAD 1 RIGHT TRIGGER) ---------------- /* This section of the code is an intake sequence. It cycles 3 times. Upon pulling the trigger on gamepad 1, the intake begins spinning and the carousel spins to intakePositionOne. @@ -258,23 +258,19 @@ public void runOpMode() { // Near shot (right trigger) if (gamepad2.right_trigger > 0.25 && gamepad2.left_trigger < 0.1) { //shooter.shooterVelocity = shooter.nearShooterVelocity; - shooter.shooterMotorLeft.setPower(.3); - shooter.shooterMotorRight.setPower(.3); - shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - tiltPosition = shooter.nearTiltPosition; + Shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.update(); + tiltPosition = Shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up currentState = State.RAMPING; } // Far shot (left trigger) - if (gamepad2.left_trigger > 0.25 && gamepad2.right_trigger < 0.1){ - shooter.shooterMotorLeft.setPower(.425); - shooter.shooterMotorRight.setPower(.425); - shooter.shooterVelocity = Shooter.farShooterVelocity; - shooter.setShooterVelocity(Shooter.shooterVelocity, telemetry); - tiltPosition = shooter.farTiltPosition; + if (gamepad2.left_trigger > 0.25 && gamepad2.right_trigger < 0.1) { + Shooter.shooterVelocity = Shooter.farShooterVelocity; + shooter.update(); + tiltPosition = Shooter.farTiltPosition; rampUpTimer = getRuntime() + 2.0; currentState = State.RAMPING; } @@ -284,7 +280,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -310,7 +306,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); stepStartTime = getRuntime(); launchStep++; } @@ -318,8 +314,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { + shooter.update(); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -328,7 +324,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -338,7 +334,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > flyWheelDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -348,7 +344,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -359,7 +355,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -369,7 +365,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -379,7 +375,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -389,7 +385,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > flyWheelDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -399,7 +395,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -410,7 +406,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -420,7 +416,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -430,7 +426,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); if (getRuntime() - stepStartTime > tiltDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -441,7 +437,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > flyWheelDelay) { - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + shooter.update(); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -463,8 +459,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - shooter.shooterVelocity = 0; - shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + Shooter.shooterVelocity = 0; + shooter.update(); currentState = State.IDLE; } break; @@ -485,24 +481,18 @@ public void runOpMode() { drive.updatePoseEstimate(); Pose2d pose = drive.localizer.getPose(); - // telemetry.addData("x", pose.position.x); - // telemetry.addData("y", pose.position.y); - // telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); - // telemetry.update(); + // telemetry.addData("x", pose.position.x); + // telemetry.addData("y", pose.position.y); + // telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + // telemetry.update(); TelemetryPacket packet = new TelemetryPacket(); packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); - telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); - telemetry.addData("shooterVelocity ", shooter.shooterVelocity); - telemetry.addData("shooterPower ", shooter.shooterPower); - telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); - telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); - telemetry.addData("actualMotorPower ", shooter.actualMotorPower); - telemetry.update(); + + telemetry.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/DashboardTelemetryPacketAccess.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/DashboardTelemetryPacketAccess.java new file mode 100644 index 00000000000..76f0af58fc7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/DashboardTelemetryPacketAccess.java @@ -0,0 +1,52 @@ +package org.firstinspires.ftc.teamcode.utils; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.lang.reflect.Field; + +/** + * Provides access to the internal {@link TelemetryPacket} from FTC Dashboard's telemetry adapter. + * + *

This uses reflection to access the private {@code currentPacket} field in + * {@code FtcDashboard.TelemetryAdapter}. The {@link Field} reference is cached + * at construction time to avoid repeated reflection lookups. + */ +public class DashboardTelemetryPacketAccess { + + public final Telemetry dashboardTelemetry; + private final Field currentPacketField; + + /** + * Constructs an accessor for the given dashboard telemetry instance. + * + * @throws RuntimeException if the field cannot be found (e.g., wrong telemetry type or API change). + */ + public DashboardTelemetryPacketAccess() { + this.dashboardTelemetry = FtcDashboard.getInstance().getTelemetry(); + try { + this.currentPacketField = dashboardTelemetry.getClass().getDeclaredField("currentPacket"); + this.currentPacketField.setAccessible(true); + } catch (NoSuchFieldException e) { + throw new RuntimeException("Failed to find currentPacket field. Is this a dashboard Telemetry?", e); + } + } + + /** + * Returns the current {@link TelemetryPacket} being built by the dashboard telemetry. + * + *

This packet is reset after each {@code telemetry.update()} call, so you should + * access it each loop iteration before calling update. + * + * @return The current TelemetryPacket, or null if access fails. + */ + public TelemetryPacket getTelemetryPacket() { + try { + return (TelemetryPacket) currentPacketField.get(dashboardTelemetry); + } catch (IllegalAccessException e) { + return null; + } + } +} From a4375912be5936939f5be7081707020546adb45f Mon Sep 17 00:00:00 2001 From: Matt Hanson Date: Sun, 15 Feb 2026 12:12:00 -0500 Subject: [PATCH 3/6] make the diffs much smaller by reverting the changes to opmodes and let Shooter adapt with deprecated methods --- .../ftc/teamcode/robot/Shooter.java | 70 ++++++++++++++- .../ftc/teamcode/robot/ShooterMotor.java | 34 +++---- .../ftc/teamcode/robot/SpiritAutoBlueFar.java | 63 +++++++------ .../teamcode/robot/SpiritAutoBlueNear.java | 72 ++++++++------- .../ftc/teamcode/robot/SpiritAutoNear.java | 70 ++++++++------- .../ftc/teamcode/robot/SpiritAutoRedFar.java | 56 +++++++----- .../ftc/teamcode/robot/SpiritAutoRedNear.java | 88 ++++++++++-------- .../ftc/teamcode/robot/SpiritTeleop2.java | 90 ++++++++++--------- 8 files changed, 330 insertions(+), 213 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index e0b6ec5219c..8e2f6625f4a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.hardware.HardwareMap; + import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.opmodes.tuning.FlywheelsFeedforwardTuning; @@ -26,9 +27,40 @@ */ @Config public class Shooter { + public static class DummyMotor { + private final ShooterMotor shooterMotor; + + private DummyMotor(ShooterMotor shooterMotor) { + this.shooterMotor = shooterMotor; + } + + @Deprecated + public void setPower(double power) { + // no-op. Only here to avoid more diffs in the opmodes + } + + @Deprecated + public double getVelocity() { + return shooterMotor.actualVelocity; + } + } + + @Deprecated + public final double shooterPower = 1; + @Deprecated + public double smoothActualLeftShooterVelocity; + @Deprecated + public double smoothActualRightShooterVelocity; + @Deprecated + public double actualMotorPower; + private final ShooterMotor left; private final ShooterMotor right; + @Deprecated + public final DummyMotor shooterMotorLeft; + @Deprecated + public final DummyMotor shooterMotorRight; public final Servo tiltServo; private final Telemetry telemetry; @@ -148,10 +180,35 @@ public Shooter(HardwareMap hardwareMap, Telemetry telemetry) { DcMotorSimple.Direction.FORWARD, voltage); right = new ShooterMotor(hardwareMap, "shooterMotorRight", telemetry, DcMotorSimple.Direction.REVERSE, voltage); + shooterMotorLeft = new DummyMotor(left); + shooterMotorRight = new DummyMotor(right); tiltServo = hardwareMap.get(Servo.class, "tiltServo"); } + /** + * this is purely here to support the old way of using Shooter + * + * @param hardwareMap + */ + @Deprecated + public Shooter(HardwareMap hardwareMap) { + this(hardwareMap, null); + } + + /** + * Does nothing except call update. The new calling convention + * is to optionally set shooterVelocity, and then call update(); + * update should run on every control loop iteration. + * + * @param shooterVelocity ignored + * @param telemetry ignored + */ + @Deprecated + public void setShooterVelocity(double shooterVelocity, Telemetry telemetry) { + update(); + } + /** * Runs one iteration of the feedforward + feedback velocity control loop. * @@ -161,7 +218,8 @@ public Shooter(HardwareMap hardwareMap, Telemetry telemetry) { * Set {@link #shooterVelocity} before calling this method to change the target speed. */ public void update() { - telemetry.addData("shooterVelocity", shooterVelocity); + if (telemetry != null) + telemetry.addData("shooterVelocity", shooterVelocity); // ── Timing ────────────────────────────────────────────────────── long now = System.nanoTime(); @@ -181,10 +239,14 @@ public void update() { left.updateFilter(alpha); right.updateFilter(alpha); + smoothActualLeftShooterVelocity = left.smoothActualVelocity; + smoothActualRightShooterVelocity = right.smoothActualVelocity; + // ── Coast when target is zero ──────────────────────────────────── if (shooterVelocity == 0) { - left.stop(); - right.stop(); + left.cutPower(); + right.cutPower(); + actualMotorPower = 0; stopped = true; return; } @@ -213,6 +275,7 @@ public void update() { left.update(profiledVelocity, accel, leftKS, leftKV, leftKA, kp); right.update(profiledVelocity, accel, rightKS, rightKV, rightKA, kp); + actualMotorPower = left.actualMotorPower + right.actualMotorPower / 2.0; } /** @@ -260,5 +323,4 @@ public void setFarTiltPosition(double farTiltPosition) { public void setHomeTiltPosition() { tiltServo.setPosition(homeTiltPosition); } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java index 130a8e169b4..eb59541b7c5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java @@ -26,7 +26,7 @@ public class ShooterMotor { private final DcMotorEx motor; private final String name; private final Telemetry telemetry; - private double actualVelocity = 0; + public double actualVelocity = 0; public double smoothActualVelocity = 0; public double actualMotorPower = 0; private double lastSetPower = 0; @@ -71,31 +71,33 @@ public void updateFilter(double alpha) { * {@code kP * (target - actual)} is also in volts. The total voltage is converted * to a duty-cycle power by dividing by the current battery voltage. * - * @param targetVelocity the target velocity (ticks/sec) - * @param targetAccel the target acceleration (ticks/sec²) - * @param kS static friction voltage (volts) - * @param kV velocity feedforward gain (volts per tick/sec) - * @param kA acceleration feedforward gain (volts per tick/sec²) - * @param kp proportional gain (volts per tick/sec error) + * @param targetVelocity the target velocity (ticks/sec) + * @param targetAccel the target acceleration (ticks/sec²) + * @param kS static friction voltage (volts) + * @param kV velocity feedforward gain (volts per tick/sec) + * @param kA acceleration feedforward gain (volts per tick/sec²) + * @param kp proportional gain (volts per tick/sec error) */ public void update(double targetVelocity, double targetAccel, double kS, double kV, double kA, double kp) { double feedforward = kS * Math.signum(targetVelocity) + kV * targetVelocity + kA * targetAccel; double feedback = kp * (targetVelocity - smoothActualVelocity); double voltage = feedforward + feedback; - setPower(voltage / batteryVoltage); + setPowerInternal(voltage / batteryVoltage); - telemetry.addData(name + " Actual Velocity", "%.0f", actualVelocity); - telemetry.addData(name + " Smooth Velocity", "%.0f", smoothActualVelocity); - telemetry.addData(name + " Feedforward (V)", "%.1f", feedforward); - telemetry.addData(name + " Feedback (V)", "%.1f", feedback); - telemetry.addData(name + " Motor Power", "%.2f", actualMotorPower); + if (telemetry != null) { + telemetry.addData(name + " Actual Velocity", "%.0f", actualVelocity); + telemetry.addData(name + " Smooth Velocity", "%.0f", smoothActualVelocity); + telemetry.addData(name + " Feedforward (V)", "%.1f", feedforward); + telemetry.addData(name + " Feedback (V)", "%.1f", feedback); + telemetry.addData(name + " Motor Power", "%.2f", actualMotorPower); + } } /** * Quantizes power to {@link #powerQuantum} and writes to the motor only if changed. */ - private void setPower(double power) { + private void setPowerInternal(double power) { double scale = 1.0 / powerQuantum; actualMotorPower = Math.round(power * scale) / scale; if (actualMotorPower != lastSetPower) { @@ -107,7 +109,7 @@ private void setPower(double power) { /** * Stops the motor by setting power to 0. */ - public void stop() { - setPower(0); + public void cutPower() { + setPowerInternal(0); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java index 17d58dfd3d3..9bfe099ae4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueFar.java @@ -53,11 +53,12 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; - telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel + telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); + shooter.setHomeTiltPosition(); waitForStart(); @@ -87,12 +88,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(Shooter.nearTiltPosition); + shooter.setNearTiltPosition(shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(Shooter.farTiltPosition); + shooter.setFarTiltPosition(shooter.farTiltPosition); } if (gamepad1.x) { @@ -218,7 +219,7 @@ public void runOpMode() { shooter.shooterMotorLeft.setPower(.3); shooter.shooterMotorRight.setPower(.3); shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); tiltPosition = shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up @@ -229,10 +230,12 @@ public void runOpMode() { } */ // Far shot (left trigger) - - Shooter.shooterVelocity = Shooter.farShooterVelocity; - shooter.update(); - tiltPosition = Shooter.farTiltPosition; + + shooter.shooterMotorLeft.setPower(.425); + shooter.shooterMotorRight.setPower(.425); + shooter.shooterVelocity = Shooter.farShooterVelocity; + shooter.setShooterVelocity(Shooter.shooterVelocity, telemetry); + tiltPosition = shooter.farTiltPosition; rampUpTimer = getRuntime() + 2.0; currentState = State.RAMPING; @@ -242,7 +245,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -268,7 +271,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); stepStartTime = getRuntime(); launchStep++; } @@ -276,7 +279,7 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -286,7 +289,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -296,7 +299,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -306,7 +309,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -317,7 +320,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -327,7 +330,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -337,7 +340,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -347,7 +350,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -357,7 +360,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -368,7 +371,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -378,7 +381,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -388,7 +391,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -399,7 +402,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -421,8 +424,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - Shooter.shooterVelocity = 0; - shooter.update(); + shooter.shooterVelocity = 0; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); //strafe to the field wall driveTimer = getRuntime() + .4; @@ -473,7 +476,13 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - + telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); + telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); + telemetry.addData("shooterVelocity ", shooter.shooterVelocity); + telemetry.addData("shooterPower ", shooter.shooterPower); + telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); + telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); + telemetry.addData("actualMotorPower ", shooter.actualMotorPower); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java index 2d53e41cba4..771c486caa9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoBlueNear.java @@ -1,5 +1,4 @@ package org.firstinspires.ftc.teamcode.robot; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -40,7 +39,6 @@ public class SpiritAutoBlueNear extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; - //------------------------------------------- @Override public void runOpMode() { @@ -52,14 +50,14 @@ public void runOpMode() { State currentState = State.IDLE; - double tiltPosition = 0;//this is only used for testing, not in production + double tiltPosition = 0;//this is only used for testing, not in production + - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel - + telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); shooter.setHomeTiltPosition(); waitForStart(); @@ -77,7 +75,8 @@ public void runOpMode() { } //END KILL BUTTON************************************************************ - */ { + */ + { telemetry.addData("Test", 0); //Operate Intake: left bumper is intake and right bumper is eject if (gamepad2.left_bumper) { @@ -101,12 +100,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(Shooter.nearTiltPosition); + shooter.setNearTiltPosition(shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(Shooter.farTiltPosition); + shooter.setFarTiltPosition(shooter.farTiltPosition); } if (gamepad1.x) { @@ -225,13 +224,15 @@ public void runOpMode() { // Near shot, get shooter motors, tile and ramp up all set up and drive backwards 6 inches //shooter.shooterVelocity = shooter.nearShooterVelocity; - Shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.update(); - tiltPosition = Shooter.nearTiltPosition; + shooter.shooterMotorLeft.setPower(.3); + shooter.shooterMotorRight.setPower(.3); + shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + tiltPosition = shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up //drive backward 6 inches (i.e., .2 seconds) - driveTimer = getRuntime() + .25; + driveTimer = getRuntime()+ .25; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(1, 0 @@ -250,7 +251,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -276,7 +277,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); stepStartTime = getRuntime(); launchStep++; } @@ -284,8 +285,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.update(); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -294,7 +295,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -304,7 +305,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -314,7 +315,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -325,7 +326,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -335,7 +336,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -345,7 +346,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -355,7 +356,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -365,7 +366,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -376,7 +377,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -386,7 +387,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -396,7 +397,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -407,7 +408,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -429,11 +430,11 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - Shooter.shooterVelocity = 0; - shooter.update(); + shooter.shooterVelocity = 0; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); //strafe to the field wall - driveTimer = getRuntime() + .75; + driveTimer = getRuntime()+ .75; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(0, .5 @@ -481,6 +482,13 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); + telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); + telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); + telemetry.addData("shooterVelocity ", shooter.shooterVelocity); + telemetry.addData("shooterPower ", shooter.shooterPower); + telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); + telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); + telemetry.addData("actualMotorPower ", shooter.actualMotorPower); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java index 36e93606cf5..f557e0e2354 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoNear.java @@ -1,5 +1,4 @@ package org.firstinspires.ftc.teamcode.robot; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -47,7 +46,6 @@ public class SpiritAutoNear extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; - //------------------------------------------- @Override public void runOpMode() { @@ -59,13 +57,14 @@ public void runOpMode() { State currentState = State.IDLE; - double tiltPosition = 0;//this is only used for testing, not in production + double tiltPosition = 0;//this is only used for testing, not in production + - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel + telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); // Color sensor for auto-detecting alliance color ColorSensor colorSensor = hardwareMap.get(ColorSensor.class, "colorSensor"); @@ -99,7 +98,8 @@ public void runOpMode() { } //END KILL BUTTON************************************************************ - */ { + */ + { telemetry.addData("Test", 0); telemetry.addData("Alliance", isBlue ? "BLUE" : "RED"); //Operate Intake: left bumper is intake and right bumper is eject @@ -124,12 +124,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(Shooter.nearTiltPosition); + shooter.setNearTiltPosition(shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(Shooter.farTiltPosition); + shooter.setFarTiltPosition(shooter.farTiltPosition); } if (gamepad1.x) { @@ -248,13 +248,15 @@ public void runOpMode() { // Near shot, get shooter motors, tile and ramp up all set up and drive backwards 6 inches //shooter.shooterVelocity = shooter.nearShooterVelocity; - Shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.update(); - tiltPosition = Shooter.nearTiltPosition; + shooter.shooterMotorLeft.setPower(.3); + shooter.shooterMotorRight.setPower(.3); + shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + tiltPosition = shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up //drive backward 6 inches (i.e., .2 seconds) - driveTimer = getRuntime() + .25; + driveTimer = getRuntime()+ .25; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(1, 0 @@ -273,7 +275,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -299,7 +301,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); stepStartTime = getRuntime(); launchStep++; } @@ -307,8 +309,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.update(); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -317,7 +319,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -327,7 +329,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -337,7 +339,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -348,7 +350,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -358,7 +360,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -368,7 +370,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -378,7 +380,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -388,7 +390,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -399,7 +401,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -409,7 +411,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -419,7 +421,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -430,7 +432,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -452,8 +454,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - Shooter.shooterVelocity = 0; - shooter.update(); + shooter.shooterVelocity = 0; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); //strafe to the field wall — direction based on detected color double strafePower = isBlue ? 0.5 : -0.5; @@ -506,7 +508,13 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - + telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); + telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); + telemetry.addData("shooterVelocity ", shooter.shooterVelocity); + telemetry.addData("shooterPower ", shooter.shooterPower); + telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); + telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); + telemetry.addData("actualMotorPower ", shooter.actualMotorPower); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java index 17e50b25c2b..819d9036efb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedFar.java @@ -1,5 +1,4 @@ package org.firstinspires.ftc.teamcode.robot; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -9,6 +8,7 @@ import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.Drawing; @@ -54,11 +54,11 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); shooter.setHomeTiltPosition(); @@ -75,9 +75,11 @@ public void runOpMode() { //Far shot red target // set shooter power and apply to motors to get them spinning. set tilt, set timer, set ramp up timer - Shooter.shooterVelocity = Shooter.farShooterVelocity; - shooter.update(); - tiltPosition = Shooter.farTiltPosition; + shooter.shooterMotorLeft.setPower(.425); + shooter.shooterMotorRight.setPower(.425); + shooter.shooterVelocity = Shooter.farShooterVelocity; + shooter.setShooterVelocity(Shooter.shooterVelocity, telemetry); + tiltPosition = shooter.farTiltPosition; rampUpTimer = getRuntime() + 2.0; currentState = State.RAMPING; @@ -87,7 +89,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -113,7 +115,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); stepStartTime = getRuntime(); launchStep++; } @@ -121,7 +123,7 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -131,7 +133,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -141,7 +143,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -151,7 +153,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -162,7 +164,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -172,7 +174,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -182,7 +184,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -192,7 +194,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -202,7 +204,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -213,7 +215,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -223,7 +225,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -233,7 +235,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -244,7 +246,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -266,8 +268,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - Shooter.shooterVelocity = 0; - shooter.update(); + shooter.shooterVelocity = 0; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); //strafe to the field wall driveTimer = getRuntime() + .4; @@ -316,7 +318,13 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - + telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); + telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); + telemetry.addData("shooterVelocity ", shooter.shooterVelocity); + telemetry.addData("shooterPower ", shooter.shooterPower); + telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); + telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); + telemetry.addData("actualMotorPower ", shooter.actualMotorPower); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java index f94a31a25d4..f266723deba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritAutoRedNear.java @@ -1,5 +1,4 @@ package org.firstinspires.ftc.teamcode.robot; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -40,7 +39,6 @@ public class SpiritAutoRedNear extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; - //------------------------------------------- @Override public void runOpMode() { @@ -56,11 +54,11 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel + telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); shooter.setHomeTiltPosition(); @@ -79,7 +77,8 @@ public void runOpMode() { } //END KILL BUTTON************************************************************ - */ { + */ + { telemetry.addData("Test", 0); //Operate Intake: left bumper is intake and right bumper is eject if (gamepad2.left_bumper) { @@ -103,12 +102,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(Shooter.nearTiltPosition); + shooter.setNearTiltPosition(shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(Shooter.farTiltPosition); + shooter.setFarTiltPosition(shooter.farTiltPosition); } if (gamepad1.x) { @@ -159,6 +158,9 @@ public void runOpMode() { //--------------------------END TESTING OF CAROUSEL + + + // ---------------- INTAKE + CAROUSEL SEQUENCE (GAMEPAD 1 RIGHT TRIGGER) ---------------- /* This section of the code is an intake sequence. It cycles 3 times. Upon pulling the trigger @@ -226,21 +228,23 @@ public void runOpMode() { case IDLE: // Near shot - //shooter.shooterVelocity = shooter.nearShooterVelocity; - Shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.update(); - tiltPosition = Shooter.nearTiltPosition; - rampUpTimer = getRuntime() + 2.0; // 2-second spin-up + //shooter.shooterVelocity = shooter.nearShooterVelocity; + shooter.shooterMotorLeft.setPower(.3); + shooter.shooterMotorRight.setPower(.3); + shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + tiltPosition = shooter.nearTiltPosition; + rampUpTimer = getRuntime() + 2.0; // 2-second spin-up //drive backward 6 inches (i.e., .2 seconds) - driveTimer = getRuntime() + .25; - while (getRuntime() < driveTimer) { - drive.setDrivePowers(new PoseVelocity2d( - new Vector2d(1, 0 + driveTimer = getRuntime()+ .25; + while (getRuntime() < driveTimer) { + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d(1, 0 - ), - 0 - )); + ), + 0 + )); currentState = State.RAMPING; @@ -252,7 +256,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -278,7 +282,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); stepStartTime = getRuntime(); launchStep++; } @@ -286,8 +290,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.update(); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -296,7 +300,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -306,7 +310,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -316,7 +320,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -327,7 +331,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -337,7 +341,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -347,7 +351,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -357,7 +361,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltToLaunchDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -367,7 +371,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -378,7 +382,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -388,7 +392,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -398,7 +402,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -409,7 +413,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > tiltToLaunchDelay) { - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -431,11 +435,11 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - Shooter.shooterVelocity = 0; - shooter.update(); + shooter.shooterVelocity = 0; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); //strafe to the field wall - driveTimer = getRuntime() + .7; + driveTimer = getRuntime()+ .7; while (getRuntime() < driveTimer) { drive.setDrivePowers(new PoseVelocity2d( new Vector2d(0, -.5 @@ -482,7 +486,13 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - + telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); + telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); + telemetry.addData("shooterVelocity ", shooter.shooterVelocity); + telemetry.addData("shooterPower ", shooter.shooterPower); + telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); + telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); + telemetry.addData("actualMotorPower ", shooter.actualMotorPower); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index 8cc9a8fb5a0..a9ab37358c9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -1,5 +1,4 @@ package org.firstinspires.ftc.teamcode.robot; - import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -54,7 +53,6 @@ public class SpiritTeleop2 extends LinearOpMode { int intakeStep = 0; // 0 → 1 → 2 boolean triggerHeld = false; // edge detection double intakePower = 1.0; - //------------------------------------------- @Override public void runOpMode() { @@ -70,18 +68,19 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; - telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel Lift lift = new Lift(hardwareMap); + telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); + shooter.setHomeTiltPosition(); lift.homeLift(); waitForStart(); - while (opModeIsActive()) { + while (opModeIsActive()){ /* //KILL BUTTON***************************************************************** if(gamepad1.a){ @@ -96,7 +95,7 @@ public void runOpMode() { */ //LIFT ROBOT OFF OF MATT**************************************************** - if (gamepad2.a) {//(gamepad2.x && gamepad2.b ){ + if (gamepad2.a){//(gamepad2.x && gamepad2.b ){ lift.engageLift(); } @@ -129,12 +128,12 @@ public void runOpMode() { } //JUST FOR TESTING POSITION OF TILT SERVO if (gamepad1.a) { - shooter.setNearTiltPosition(Shooter.nearTiltPosition); + shooter.setNearTiltPosition(shooter.nearTiltPosition); } if (gamepad1.b) { - shooter.setFarTiltPosition(Shooter.farTiltPosition); + shooter.setFarTiltPosition(shooter.farTiltPosition); } if (gamepad1.x) { @@ -190,7 +189,8 @@ public void runOpMode() { } - // ---------------- INTAKE + CAROUSEL SEQUENCE (GAMEPAD 1 RIGHT TRIGGER) ---------------- + + // ---------------- INTAKE + CAROUSEL SEQUENCE (GAMEPAD 1 RIGHT TRIGGER) ---------------- /* This section of the code is an intake sequence. It cycles 3 times. Upon pulling the trigger on gamepad 1, the intake begins spinning and the carousel spins to intakePositionOne. @@ -258,19 +258,23 @@ public void runOpMode() { // Near shot (right trigger) if (gamepad2.right_trigger > 0.25 && gamepad2.left_trigger < 0.1) { //shooter.shooterVelocity = shooter.nearShooterVelocity; - Shooter.shooterVelocity = Shooter.nearShooterVelocity; - shooter.update(); - tiltPosition = Shooter.nearTiltPosition; + shooter.shooterMotorLeft.setPower(.3); + shooter.shooterMotorRight.setPower(.3); + shooter.shooterVelocity = Shooter.nearShooterVelocity; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + tiltPosition = shooter.nearTiltPosition; rampUpTimer = getRuntime() + 2.0; // 2-second spin-up currentState = State.RAMPING; } // Far shot (left trigger) - if (gamepad2.left_trigger > 0.25 && gamepad2.right_trigger < 0.1) { - Shooter.shooterVelocity = Shooter.farShooterVelocity; - shooter.update(); - tiltPosition = Shooter.farTiltPosition; + if (gamepad2.left_trigger > 0.25 && gamepad2.right_trigger < 0.1){ + shooter.shooterMotorLeft.setPower(.425); + shooter.shooterMotorRight.setPower(.425); + shooter.shooterVelocity = Shooter.farShooterVelocity; + shooter.setShooterVelocity(Shooter.shooterVelocity, telemetry); + tiltPosition = shooter.farTiltPosition; rampUpTimer = getRuntime() + 2.0; currentState = State.RAMPING; } @@ -280,7 +284,7 @@ public void runOpMode() { // RAMPING — waiting for flywheel to reach speed // ------------------------------------------------------ case RAMPING: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() > rampUpTimer) { // Begin launch sequence currentState = State.LAUNCHING; @@ -306,7 +310,7 @@ public void runOpMode() { case 0: if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchOne(); - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); stepStartTime = getRuntime(); launchStep++; } @@ -314,8 +318,8 @@ public void runOpMode() { // STEP 1 — tiny kicker case 1: - shooter.update(); - if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .2) { + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); + if (getRuntime() - stepStartTime > defaultLaunchStepDelay +.2) { carousel.setTinyKicker(); stepStartTime = getRuntime(); launchStep++; @@ -324,7 +328,7 @@ public void runOpMode() { // STEP 2 — tilt shooter case 2: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -334,7 +338,7 @@ public void runOpMode() { // STEP 3 — full kicker (launch ball) case 3: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > flyWheelDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -344,7 +348,7 @@ public void runOpMode() { // STEP 4 — reset tilt + kicker case 4: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -355,7 +359,7 @@ public void runOpMode() { // STEP 5 — rotate carousel to position 2 case 5: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity,telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchTwo(); stepStartTime = getRuntime(); @@ -365,7 +369,7 @@ public void runOpMode() { // STEP 6 — second tiny kicker case 6: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -375,7 +379,7 @@ public void runOpMode() { // STEP 7 — tilt again case 7: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -385,7 +389,7 @@ public void runOpMode() { // STEP 8 — second full kicker case 8: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > flyWheelDelay) { carousel.setFullKicker(); stepStartTime = getRuntime(); @@ -395,7 +399,7 @@ public void runOpMode() { // STEP 9 — reset tilt + kicker again case 9: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { shooter.setHomeTiltPosition(); carousel.setHomePositionKicker(); @@ -406,7 +410,7 @@ public void runOpMode() { // STEP 10 — rotate to position 3 case 10: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.spinCarouselLaunchThree(); stepStartTime = getRuntime(); @@ -416,7 +420,7 @@ public void runOpMode() { // STEP 11 — tiny kicker third time case 11: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > defaultLaunchStepDelay) { carousel.setTinyKicker(); stepStartTime = getRuntime(); @@ -426,7 +430,7 @@ public void runOpMode() { // STEP 12 — tilt third time case 12: - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); if (getRuntime() - stepStartTime > tiltDelay) { shooter.setTiltPosition(tiltPosition); stepStartTime = getRuntime(); @@ -437,7 +441,7 @@ public void runOpMode() { // STEP 13 — full send #3 case 13: if (getRuntime() - stepStartTime > flyWheelDelay) { - shooter.update(); + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); carousel.setFullKicker(); stepStartTime = getRuntime(); launchStep++; @@ -459,8 +463,8 @@ public void runOpMode() { case 16: if (getRuntime() - stepStartTime > defaultLaunchStepDelay + .5) { carousel.spinCarouselLaunchOne();//add - Shooter.shooterVelocity = 0; - shooter.update(); + shooter.shooterVelocity = 0; + shooter.setShooterVelocity(shooter.shooterVelocity, telemetry); currentState = State.IDLE; } break; @@ -481,18 +485,24 @@ public void runOpMode() { drive.updatePoseEstimate(); Pose2d pose = drive.localizer.getPose(); - // telemetry.addData("x", pose.position.x); - // telemetry.addData("y", pose.position.y); - // telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); - // telemetry.update(); + // telemetry.addData("x", pose.position.x); + // telemetry.addData("y", pose.position.y); + // telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble())); + // telemetry.update(); TelemetryPacket packet = new TelemetryPacket(); packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - - telemetry.update(); + telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); + telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); + telemetry.addData("shooterVelocity ", shooter.shooterVelocity); + telemetry.addData("shooterPower ", shooter.shooterPower); + telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); + telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); + telemetry.addData("actualMotorPower ", shooter.actualMotorPower); + telemetry.update(); } } From a6ab53f308213e57f5f5cdd5da903e2da9cdfc35 Mon Sep 17 00:00:00 2001 From: Matt Hanson Date: Sun, 15 Feb 2026 12:16:48 -0500 Subject: [PATCH 4/6] better javadoc for deprecated methods --- .../org/firstinspires/ftc/teamcode/robot/Shooter.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 8e2f6625f4a..b7a59d85af6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -187,7 +187,9 @@ public Shooter(HardwareMap hardwareMap, Telemetry telemetry) { } /** - * this is purely here to support the old way of using Shooter + * This is purely here to support the old way of using Shooter. + * The better way is to pass telemetry so that {@link Shooter} and + * {@link ShooterMotor} can do their own logging. * * @param hardwareMap */ @@ -198,8 +200,8 @@ public Shooter(HardwareMap hardwareMap) { /** * Does nothing except call update. The new calling convention - * is to optionally set shooterVelocity, and then call update(); - * update should run on every control loop iteration. + * is to optionally set {@link #shooterVelocity}, and then call {@link #update()}. + * {@link #update} should run on every control loop iteration. * * @param shooterVelocity ignored * @param telemetry ignored From 4fc85bdc01c645bbd9d255d4760534934d8d44be Mon Sep 17 00:00:00 2001 From: Mars Robotics Date: Sun, 15 Feb 2026 20:58:24 -0500 Subject: [PATCH 5/6] changes during testing --- .../ftc/teamcode/robot/Shooter.java | 5 +++++ .../ftc/teamcode/robot/SpiritTeleop2.java | 21 +++++++++++-------- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index b7a59d85af6..571e73481a1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -269,6 +269,11 @@ public void update() { double accel = Math.max(-maxAccelTPS2, Math.min(maxAccelTPS2, rawAccel)); profiledVelocity += accel * dt; + if (telemetry != null){ + telemetry.addData("profile accel", "%.1f", accel); + telemetry.addData("profile velocity", "%.1f", profiledVelocity); + } + // Snap to target once negligibly close (avoids asymptotic creep) if (Math.abs(shooterVelocity - profiledVelocity) < 1.0) { profiledVelocity = shooterVelocity; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java index a9ab37358c9..40eaaf82bde 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/SpiritTeleop2.java @@ -68,12 +68,12 @@ public void runOpMode() { double tiltPosition = 0; //double homeTiltPosition = 0; + telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); Intake intake = new Intake(hardwareMap);//instantiate a new intake motor - Shooter shooter = new Shooter(hardwareMap);//instantiate a new shooter + Shooter shooter = new Shooter(hardwareMap, telemetry);//instantiate a new shooter Carousel carousel = new Carousel(hardwareMap);//instantiate a new carousel Lift lift = new Lift(hardwareMap); - telemetry = new MultipleTelemetry(telemetry,FtcDashboard.getInstance().getTelemetry()); shooter.setHomeTiltPosition(); @@ -495,13 +495,16 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), pose); FtcDashboard.getInstance().sendTelemetryPacket(packet); - telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); - telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); - telemetry.addData("shooterVelocity ", shooter.shooterVelocity); - telemetry.addData("shooterPower ", shooter.shooterPower); - telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); - telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); - telemetry.addData("actualMotorPower ", shooter.actualMotorPower); + if (false) { + telemetry.addData("Actual Left Shooter Velocity ", shooter.shooterMotorLeft.getVelocity()); + telemetry.addData("Actual Right Shooter Velocity ", shooter.shooterMotorRight.getVelocity()); + telemetry.addData("shooterVelocity ", shooter.shooterVelocity); + telemetry.addData("currentState", currentState); + telemetry.addData("launchStep", launchStep); + telemetry.addData(" smoothing vel left ", shooter.smoothActualLeftShooterVelocity); + telemetry.addData(" smoothing vel right", shooter.smoothActualRightShooterVelocity); + telemetry.addData("actualMotorPower ", shooter.actualMotorPower); + } telemetry.update(); } From 6bc8cb5420ed1b34e58a506cc64bf0efd7b30bf4 Mon Sep 17 00:00:00 2001 From: Matt Hanson Date: Mon, 16 Feb 2026 16:17:15 -0500 Subject: [PATCH 6/6] defensive changes to Shooter --- CLAUDE.md | 74 ++++++++++--------- .../2026-02-16-shooter-profile-gap-fix.md | 49 ++++++++++++ .../ftc/teamcode/robot/Shooter.java | 30 +++++++- .../ftc/teamcode/robot/ShooterMotor.java | 11 +++ 4 files changed, 128 insertions(+), 36 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-16-shooter-profile-gap-fix.md diff --git a/CLAUDE.md b/CLAUDE.md index abd993ac022..297e2649b66 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1,14 +1,24 @@ -# CLAUDE.md +## Workflow Preferences -This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. +- Do not run builds (`./gradlew`) or git commands (`git commit`, `git push`, etc.) by default — the user handles build testing and git operations themselves. Running builds is fine if the user reports a build problem and needs help diagnosing it. +- The user may ask for a progress report at the end of a session. These go in `TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/` and should summarize the full session: the problems the user brought up, the discussion and decisions made, any gotchas encountered, and the solutions reached — not just a list of code changes. ## Project Overview -This is an FTC (FIRST Tech Challenge) robotics project for Team Spirit, built on the Road Runner v1.0 quickstart. It is an Android application that deploys to an FTC Robot Controller phone/Control Hub. The robot features a mecanum drivetrain with a ball shooter mechanism (carousel, kicker, tilt servo), intake motor, and lift. +This is an FTC (FIRST Tech Challenge) robotics project for Team Spirit, built on the Road Runner v1.0 quickstart. It is an Android application that deploys to an FTC Robot Controller REV Control Hub. The robot features a mecanum drivetrain with a ball shooter mechanism (carousel, kicker, tilt servo), intake motor, and lift. The team's code starts after `08f0898` + +## Java Version + +The project targets Java 8 for Android compatibility. Avoid using features from newer Java versions: + +- Records (Java 16+) +- `var` keyword (Java 10+) +- Switch expressions (Java 14+) +- Text blocks (Java 15+) ## Build Commands -This project uses Gradle with the Android Gradle Plugin. All commands should be run from the project root. +This project uses Gradle with the Android Gradle Plugin. All commands should be run from the project root. Use `./gradlew` directly from bash — do NOT use `gradlew.bat` or `cmd /c`. For git commands, avoid using `git -C ` — just run `git` from the project root instead. ```bash # Build the project @@ -32,45 +42,39 @@ There are no unit tests in this project. Testing is done by deploying to the rob The build config is split across files: `build.common.gradle` (shared Android config, rarely modify), `build.dependencies.gradle` (FTC SDK dependencies v11.0.0), and `TeamCode/build.gradle` (team dependencies including Road Runner). -### Key Dependencies +### Important Patterns -- **FTC SDK 11.0.0** - Core robotics framework -- **Road Runner 1.0.1** (core + actions) + **Road Runner FTC 0.1.25** - Motion planning and trajectory following -- **FTC Dashboard 0.5.0** - Live telemetry and parameter tuning via `@Config` annotation -- Road Runner artifacts come from `https://maven.brott.dev/` +- Classes annotated with `@Config` expose their `static public` fields to FTC Dashboard for live tuning +- Drive parameters (kS, kV, kA, PID gains, wheel velocity limits) live in `MecanumDrive.Params` +- The `MecanumDrive` constructor uses `LynxModule.BulkCachingMode.AUTO` for efficient hub communication -### Code Organization (`TeamCode/src/main/java/.../teamcode/`) +## FTC SDK Gotchas -**Drive classes (root package):** -- `Localizer` - Interface for all localization strategies (`setPose`, `getPose`, `update`) -- `TankDrive` / `robot/MecanumDrive` - Drive classes with embedded `DriveLocalizer` inner class; contain Road Runner `PARAMS` (feedforward, PID gains, constraints). **The team uses `MecanumDrive`** (set in `TuningOpModes.DRIVE_CLASS`) -- `ThreeDeadWheelLocalizer`, `TwoDeadWheelLocalizer`, `PinpointLocalizer`, `OTOSLocalizer` - Alternative localizer implementations that can be swapped into the drive class +### DcMotor RunMode -**Robot subsystems (`robot/` package):** -- `Intake` - Single motor (`intakeMotor`) -- `Shooter` - Dual flywheel motors (`shooterMotorLeft`/`shooterMotorRight`) + tilt servo; uses feedforward+feedback velocity control with smoothing -- `Carousel` - Two servos (`carouselServo` + `kickerServo`) that rotate a ball carousel and kick balls into the shooter; positions are computed from degree offsets -- `Lift` - Encoder-controlled lift motor for raising the robot +- `RUN_USING_ENCODER` enables the SDK's built-in PIDF velocity controller. Only use this if you want the SDK to handle velocity control. +- `RUN_WITHOUT_ENCODER` is the correct mode when doing custom feedback/feedforward in team code. Despite the name, it still reads encoder values — it just doesn't use them for internal control. -**OpModes (`robot/` package):** -- `SpiritTeleop2` - Main teleop; gamepad1 drives + controls tilt/carousel positions, gamepad2 controls intake/shooter/lift. Launch sequence is a multi-step state machine (IDLE -> RAMPING -> LAUNCHING with 16 timed steps) -- `SpiritAutoFar`, `SpiritAutoBlueFar`, `SpiritAutoBlueNear`, `SpiritAutoRedFar`, `SpiritAutoRedNear` - Autonomous routines, mostly timed drive movements +### RUN_USING_ENCODER feedforward (kV) -**Tuning (`tuning/` package):** -- `TuningOpModes` - Registers all Road Runner tuning OpModes (ramp loggers, push tests, direction debuggers, feedforward/feedback tuners). See [Road Runner tuning docs](https://rr.brott.dev/docs/v1-0/tuning/) -- `LocalizationTest`, `ManualFeedbackTuner`, `SplineTest` - Custom tuning OpModes +The SDK's velocity PIDF `F` coefficient is a kV, but scaled by 32767. To calculate: `F = 32767 * kV`. For example, a motor measured at max 2496 ticks/sec → `F = 32767 / 2496 ≈ 13.13`. -**Messages (`messages/` package):** Data classes for Road Runner's FlightRecorder logging. +### Gamepad edge detection -### Hardware Configuration Names +SDK 11.0 added rising/falling edge detection methods directly on the Gamepad object, e.g. `gamepad1.leftBumperWasPressed()` and `gamepad1.leftBumperWasReleased()`. SDK 11.1 extended this to triggers. See `ConceptGamepadEdgeDetection` sample for usage. Code targeting older SDK versions must track previous button state manually. -Motors: `leftFront`, `leftBack`, `rightBack`, `rightFront`, `intakeMotor`, `shooterMotorLeft`, `shooterMotorRight`, `liftMotor` -Servos: `tiltServo`, `kickerServo`, `carouselServo` -IMU: `imu` +### Identifying which hub a motor is on -### Important Patterns +Use `HubHelper` to determine whether a motor is plugged into the Control Hub or the Expansion Hub. It returns the `LynxModule` for the hub the motor is connected to. Do not manually iterate `LynxModule` instances or hardcode hub assumptions. -- Classes annotated with `@Config` expose their `static public` fields to FTC Dashboard for live tuning -- Drive parameters (kS, kV, kA, PID gains, wheel velocity limits) live in `MecanumDrive.Params` -- The `MecanumDrive` constructor uses `LynxModule.BulkCachingMode.AUTO` for efficient hub communication -- `rightFront` motor direction is reversed in `MecanumDrive`; other motor directions are default +### Loop sleep + +Avoid calling `sleep()` to intentionally slow down control or sampling loops — prefer higher sample rates. Sleep is fine in situations where the OpMode is just waiting for stop (e.g., `while (opModeIsActive()) sleep(100);`). + +### FTC Dashboard field overlay + +When using FTC Dashboard telemetry alongside field overlay drawing (e.g., in `DrivingBase`), use `DashboardTelemetryPacketAccess` to get the underlying `TelemetryPacket` and draw on its `fieldOverlay()` directly. Do not create a separate `TelemetryPacket` and send it manually — that writes extra telemetry lines and duplicates data on the dashboard. + +### No hardware commands after opModeIsActive() returns false + +In a `LinearOpMode`, never send any hardware commands (motor power, servo position, sensor reads, etc.) after `opModeIsActive()` returns `false`. The SDK takes sole responsibility for shutting down hardware at that point. Sending commands after this will crash the Control Hub and force a restart. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-16-shooter-profile-gap-fix.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-16-shooter-profile-gap-fix.md new file mode 100644 index 00000000000..54edbe0628a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ProgressReports/2026-02-16-shooter-profile-gap-fix.md @@ -0,0 +1,49 @@ +# Progress Report — 2026-02-16 + +## Problem + +After the first shot in SpiritTeleop2, the shooter velocity profile goes wonky on the second shot. The root cause: `Shooter.update()` is not called every control loop iteration. The teleop's state machine only calls `setShooterVelocity` (which delegates to `update()`) inside specific state/step branches. There are multi-second gaps — especially during IDLE between launch sequences, and during certain LAUNCHING steps (step 0, steps 13-16). + +When `update()` finally runs after a gap, `dt` (computed from `System.nanoTime()`) has accumulated to seconds. The profile step `profiledVelocity += accel * dt` then massively overshoots the target. For example, with a 5-second gap targeting 650 tps: `accel ≈ 435 tps²`, so `profiledVelocity += 435 * 5.0 = 2175` — 3.3x the target. + +## Constraint + +The user wanted to avoid modifying the Spirit opmodes. The correct fix would be to call `update()` every loop iteration in SpiritTeleop2, but instead we made Shooter defensive against gaps. + +## Solution + +Two changes, both in the Shooter/ShooterMotor subsystem: + +### 1. Profile dt cap (`Shooter.java`) + +Added `public static double maxProfileDt = 0.100` (tunable via Dashboard). The profile step now uses `Math.min(dt, maxProfileDt)` instead of raw `dt`. The velocity filter still uses the true `dt` so its IIR alpha is computed correctly. + +This keeps the profile close to the motor's actual trajectory after a gap, which also keeps the kA feedforward term accurate — the profile's acceleration matches what the motor physically needs. + +Telemetry line `"profile dt CAPPED"` is emitted when the cap activates, for debugging on Dashboard. + +### 2. Filter reset on resume from coast (`ShooterMotor.java` + `Shooter.java`) + +Added `ShooterMotor.resetFilter()` which snaps `smoothActualVelocity` to the latest raw encoder reading, discarding stale filter history. Called in Shooter's `stopped` -> running transition, before seeding the profile. + +Without this, `smoothActualVelocity` could be stale from seconds ago (the last time `update()` ran). The IIR filter's alpha would be ~1.0 for a large dt (approximately correct), but relying on that is fragile. Explicit reset makes the intent clear and guarantees a clean start. + +## Discussion: analytical profile vs dt cap + +We explored whether the Euler integration could be replaced with the exact analytical solution for the clamped-exponential profile: + +- **Exponential regime** (`|error| <= maxAccel * tau`): `v(t) = target - error * exp(-t/tau)` — never overshoots for any dt. +- **Clamped regime**: linear ramp, Euler is exact. +- **Split step** (crosses from clamped to exponential within one dt): computable exactly. + +This eliminates numerical instability entirely. However, for this application, the dt cap is actually superior: + +- With the exact solution and a 5s gap, the profile jumps to ~627 tps (near target) on the first step. But the motor is at 0 tps. The kA feedforward term sees near-zero acceleration and provides almost no voltage — exactly when the motor needs a strong kick to start moving. +- With the dt cap, the profile starts at ~150 tps with `accel = maxAccel`, so `kA * maxAccel` provides the large voltage the motor needs. The profile tracks close to the motor's actual trajectory. + +**Decision:** Keep the dt cap. The profile's purpose is to give the motor a setpoint it can actually follow, not to solve the ODE correctly. When there's been a control gap, "resume from where the motor is and ramp normally" is the right behavior. + +## Files Changed + +- `TeamCode/.../robot/Shooter.java` — added `maxProfileDt` field, capped profile dt, added filter reset call on resume, added telemetry for capped dt +- `TeamCode/.../robot/ShooterMotor.java` — added `resetFilter()` method diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java index 571e73481a1..23a32f6c7cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/Shooter.java @@ -159,6 +159,23 @@ public double getVelocity() { */ public static double approachTau = Math.max(leftKA / leftKV, rightKA / rightKV); + /** + * Maximum elapsed time (seconds) applied to the setpoint profile per + * call to {@link #update}. + *

+ * If the caller does not invoke {@code update()} every loop iteration + * (e.g. the OpMode only calls it inside certain state-machine steps), + * the wall-clock {@code dt} can grow to seconds. Without a cap the + * profile would overshoot wildly + * ({@code profiledVelocity += maxAccel * hugeΔt}). + *

+ * Capping {@code dt} means the profile advances at most one + * "normal-sized" step regardless of how long the gap was — the + * velocity filter still uses the true {@code dt} so its estimate + * snaps to the latest encoder reading correctly. + */ + public static double maxProfileDt = 0.060; + private long lastTimeNanos; private double profiledVelocity; private boolean stopped = true; @@ -254,7 +271,11 @@ public void update() { } // ── Seed profile from coasting velocity on resume ──────────────── + // Reset the filter so the profile and feedback start from a clean + // encoder reading, not a stale smoothed value from before the gap. if (stopped) { + left.resetFilter(); + right.resetFilter(); profiledVelocity = Math.max(left.smoothActualVelocity, right.smoothActualVelocity); stopped = false; @@ -264,14 +285,21 @@ public void update() { // Far from target: acceleration clamped at maxAccelTPS2 (linear ramp). // Close to target: acceleration = error/tau, decaying smoothly to zero. // The transition is continuous in acceleration — no step change for kA. + // + // Cap dt for the profile so a gap between update() calls doesn't + // cause a massive overshoot. The velocity filter above still uses + // the true dt so its estimate stays accurate. + double profileDt = Math.min(dt, maxProfileDt); double error = shooterVelocity - profiledVelocity; double rawAccel = error / approachTau; double accel = Math.max(-maxAccelTPS2, Math.min(maxAccelTPS2, rawAccel)); - profiledVelocity += accel * dt; + profiledVelocity += accel * profileDt; if (telemetry != null){ telemetry.addData("profile accel", "%.1f", accel); telemetry.addData("profile velocity", "%.1f", profiledVelocity); + if (dt > maxProfileDt) + telemetry.addData("profile dt CAPPED", "%.3f -> %.3f", dt, profileDt); } // Snap to target once negligibly close (avoids asymptotic creep) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java index eb59541b7c5..a629df87f76 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java @@ -62,6 +62,17 @@ public void updateFilter(double alpha) { smoothActualVelocity = (actualVelocity * alpha) + (1 - alpha) * smoothActualVelocity; } + /** + * Discards all filter history and sets the smoothed velocity to the + * latest raw encoder reading (from the most recent {@link #updateFilter} + * call). Use this when resuming from a long idle period so the + * feedback loop starts from a clean measurement rather than a stale + * filtered value. + */ + public void resetFilter() { + smoothActualVelocity = actualVelocity; + } + /** * Runs one iteration of the voltage-based feedforward + feedback loop for this motor. * {@link #updateFilter} must be called first each loop to refresh the velocity estimate.