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/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-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/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/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..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 @@ -1,25 +1,25 @@ 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. * @@ -27,43 +27,158 @@ */ @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; + } + } - public final DcMotorEx shooterMotorLeft; - public final DcMotorEx shooterMotorRight; + @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; - 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; + + /** + * 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); + + /** + * 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; - static public double feedLeftForward = .41; - static public double feedRightForward = .35; - //********************************************* + 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 +186,144 @@ 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); + shooterMotorLeft = new DummyMotor(left); + shooterMotorRight = new DummyMotor(right); - 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. + * 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. * - *

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 hardwareMap + */ + @Deprecated + public Shooter(HardwareMap hardwareMap) { + this(hardwareMap, null); + } + + /** + * Does nothing except call update. The new calling convention + * is to optionally set {@link #shooterVelocity}, and then call {@link #update()}. + * {@link #update} should run on every control loop iteration. * - * @param shooterVelocity desired flywheel velocity in ticks per second - * @param telemetry telemetry instance for logging velocity diagnostics + * @param shooterVelocity ignored + * @param telemetry ignored */ + @Deprecated public void setShooterVelocity(double shooterVelocity, Telemetry telemetry) { - 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); + update(); } + /** + * 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 {@link #shooterVelocity} is 0, both motors are stopped immediately. + * Set {@link #shooterVelocity} before calling this method to change the target speed. + */ + public void update() { + if (telemetry != null) + telemetry.addData("shooterVelocity", shooterVelocity); + + // ── 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); + + smoothActualLeftShooterVelocity = left.smoothActualVelocity; + smoothActualRightShooterVelocity = right.smoothActualVelocity; + + // ── Coast when target is zero ──────────────────────────────────── + if (shooterVelocity == 0) { + left.cutPower(); + right.cutPower(); + actualMotorPower = 0; + stopped = true; + return; + } + + // ── 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; + } + + // ── 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. + // + // 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 * 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) + 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); + actualMotorPower = left.actualMotorPower + right.actualMotorPower / 2.0; + } + /** + * 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 +352,10 @@ 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..a629df87f76 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/robot/ShooterMotor.java @@ -0,0 +1,126 @@ +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; + public 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; + } + + /** + * 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. + * + *

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; + setPowerInternal(voltage / batteryVoltage); + + 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 setPowerInternal(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 cutPower() { + setPowerInternal(0); + } +} 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(); } 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; + } + } +} 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"); + } +}