From 36ac31b3ec474af8f936c79b245b54e1b53a4880 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Wed, 26 Nov 2025 22:58:31 -0600 Subject: [PATCH] Auto track implemented with tunable constants --- .../ftc/teamcode/subsystems/Shooter.java | 162 ++++++++++++++++++ 1 file changed, 162 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java new file mode 100644 index 0000000..96bd384 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java @@ -0,0 +1,162 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.utils.Robot; + +@Config +public class Shooter { + + // ================================================================ + // ------------------- DASHBOARD CONSTANTS ----------------------- + // ================================================================ + + public static int mode = 0; // 0 = manual, 1 = velocity PID, 2 = autoTrack + public static double parameter = 0.0; // manual: power, vel: target RPM, auto: target RPM + + public static double MAX_RPM = 2500; + public static double kP = 0.01; + public static double maxStep = 0.2; + + public static double transferPower = 0.0; + public static double hoodPos = 0.501; + + // ================================================================ + // ------------- AUTO TRACK TUNING CONSTANTS ------------------- + // ================================================================ + + // z offset between shooter and goal + public static double dz = 30; // inches + + // Quadratic fit for shooter velocity vs distance + // v = A*d^2 + B*d + C + public static double A = 0.0004; + public static double B = 0.9; + public static double C = 1200; + + // Hood angle trig model + // hood = HOOD_A * atan(d * HOOD_B) + HOOD_C + public static double HOOD_A = 0.42; + public static double HOOD_B = 0.012; + public static double HOOD_C = 0.22; + + // ================================================================ + // ---------------------- INTERNAL STATE ------------------------ + // ================================================================ + + private DcMotorEx leftShooter, rightShooter, encoder; + + private double lastRevolutions = 0.0; + private double lastTime = 0.0; + + private MultipleTelemetry TELE; + private Robot robot; + + // ================================================================ + // --------------------------- INIT ------------------------------- + // ================================================================ + + public void init(Robot robot, MultipleTelemetry TELE) { + this.robot = robot; + this.TELE = TELE; + + leftShooter = robot.shooter1; + rightShooter = robot.shooter2; + encoder = robot.shooter1; + + lastTime = 0.0; + lastRevolutions = 0.0; + } + + // ================================================================ + // -------------------------- UPDATE ------------------------------ + // ================================================================ + + public void update(double runtimeSec) { + + double kF = 1.0 / MAX_RPM; + + double rev = encoder.getCurrentPosition() / 2048.0; + double velocity = -60 * (rev - lastRevolutions) / (runtimeSec - lastTime); + + TELE.addLine("Mode: 0=Manual, 1=Vel, 2=AutoTrack"); + TELE.addData("Parameter", parameter); + TELE.addData("Velocity", velocity); + + if (mode == 0) { + // Manual + leftShooter.setPower(parameter); + rightShooter.setPower(parameter); + } + else if (mode == 1 || mode == 2) { + // Velocity PID (shared logic) + double feed = kF * parameter; + double error = parameter - velocity; + double correction = kP * error; + + correction = Math.max(-maxStep, Math.min(maxStep, correction)); + + double finalPower = Math.max(0, Math.min(1, feed + correction)); + + leftShooter.setPower(finalPower); + rightShooter.setPower(finalPower); + } + + robot.hood.setPosition(hoodPos); + robot.transfer.setPower(transferPower); + + lastTime = runtimeSec; + lastRevolutions = rev; + + TELE.update(); + } + + // ================================================================ + // ------------------------ AUTO TRACK ---------------------------- + // ================================================================ + + public void autoTrack(Pose2d robotPose, Pose2d goalPose) { + + mode = 2; // Auto tracking → velocity PID + + // Compute 3D distance + double dx = goalPose.position.x - robotPose.position.x; + double dy = goalPose.position.y - robotPose.position.y; + + double distance = Math.sqrt(dx*dx + dy*dy + dz*dz); + + // ---- Velocity Fit ---- + double targetVelocity = A * distance * distance + B * distance + C; + parameter = targetVelocity; + + // ---- Hood Fit ---- + hoodPos = HOOD_A * Math.atan(distance * HOOD_B) + HOOD_C; + + // Telemetry + TELE.addLine("AUTO TRACK ACTIVE"); + TELE.addData("Distance", distance); + TELE.addData("Target Velocity", targetVelocity); + TELE.addData("Hood", hoodPos); + } + + // ================================================================ + // --------------------- USER CALL METHODS ------------------------ + // ================================================================ + + public void setManualPower(double p) { + mode = 0; + parameter = p; + } + + public void setVelocity(double rpm) { + mode = 1; + parameter = rpm; + } + + public void setHood(double pos) { hoodPos = pos; } + + public void setTransfer(double p) { transferPower = p; } +} \ No newline at end of file