From 62b6d1cf81d1a97f1a0dce43a37033a110b15ccf Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Mon, 24 Nov 2025 17:01:02 -0600 Subject: [PATCH] Fix PID velocity --- TeamCode/build.gradle | 2 + .../teamcode/subsystems/AprilTagWebcam.java | 4 +- .../ftc/teamcode/subsystems/Robot.java | 25 ++++ .../teamcode/tests/AprilTagWebcamExample.java | 4 +- .../ftc/teamcode/tests/ShooterTest.java | 114 ++++++++++++++++++ .../ftc/teamcode/tests/TrackingTest.java | 4 + 6 files changed, 149 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 0355c38..c6a0b5c 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -36,4 +36,6 @@ dependencies { implementation "com.acmerobotics.roadrunner:core:1.0.1" implementation "com.acmerobotics.roadrunner:actions:1.0.1" implementation "com.acmerobotics.dashboard:dashboard:0.5.1" + implementation 'org.ftclib.ftclib:core:2.1.1' // core + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java index 70d97cd..6288a4f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/AprilTagWebcam.java @@ -21,7 +21,7 @@ public class AprilTagWebcam { private VisionPortal visionPortal; private List detectedTags = new ArrayList<>(); private MultipleTelemetry telemetry; - public void init(HardwareMap hwMap, MultipleTelemetry TELE){ + public void init(Robot robot, MultipleTelemetry TELE){ this.telemetry = TELE; @@ -34,7 +34,7 @@ public class AprilTagWebcam { .build(); VisionPortal.Builder builder = new VisionPortal.Builder(); - builder.setCamera(hwMap.get(WebcamName.class, "Webcam 1")); + builder.setCamera(robot.webcamName); builder.setCameraResolution(new Size(640, 480)); builder.addProcessor(aprilTagProcessor); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java new file mode 100644 index 0000000..7375f3f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java @@ -0,0 +1,25 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; + +public class Robot { + + public DcMotorEx shooter1; + public DcMotorEx shooter2; + public WebcamName webcamName; + + public Robot(HardwareMap hardwareMap) { + + webcamName = hardwareMap.get(WebcamName.class, "Webcam 1"); + shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); + shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); + + shooter1.setDirection(DcMotorSimple.Direction.REVERSE); + + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java index 0da0547..3957398 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/AprilTagWebcamExample.java @@ -7,7 +7,7 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.subsystems.AprilTagWebcam; - +import org.firstinspires.ftc.teamcode.subsystems.Robot; @Config @TeleOp @@ -22,7 +22,7 @@ public class AprilTagWebcamExample extends OpMode { telemetry, FtcDashboard.getInstance().getTelemetry() ); - aprilTagWebcam.init(hardwareMap, TELE); + aprilTagWebcam.init(new Robot(hardwareMap), TELE); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java new file mode 100644 index 0000000..e5eabd6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -0,0 +1,114 @@ +package org.firstinspires.ftc.teamcode.tests; + + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.controller.PIDController; +import com.arcrobotics.ftclib.controller.PIDFController; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; + +import org.firstinspires.ftc.teamcode.subsystems.Robot; + +@Config +@TeleOp +public class ShooterTest extends LinearOpMode { + + public static int mode = 0; + public static double parameter = 0.0; + Robot robot; + private DcMotorEx leftShooter; + private DcMotorEx rightShooter; + private DcMotorEx encoder; + private double encoderRevolutions = 0.0; + private double lastEncoderRevolutions = 0.0; + private double timeStamp = 0.0; + private double lastTimeStamp = 0.0; + + + // --- CONSTANTS YOU TUNE --- + public static double MAX_RPM = 2500; // your measured max RPM + public static double kP = 0.01; // small proportional gain (tune this) + public static double maxStep = 0.2; // prevents sudden jumps + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + leftShooter = robot.shooter1; + rightShooter = robot.shooter2; + encoder = robot.shooter1; + + MultipleTelemetry TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + + double kF = 1.0 / MAX_RPM; // baseline feedforward + + + + + encoderRevolutions = (double) encoder.getCurrentPosition() / 2048; + + double velocity = -60*(encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp); + + TELE.addLine("Mode: 0 = Manual, 1 = Vel, 2 = Pos"); + TELE.addLine("Parameter = pow, vel, or pos"); + TELE.addData("leftShooterPower", leftShooter.getPower()); + TELE.addData("rightShooterPower", rightShooter.getPower()); + TELE.addData("shaftEncoderPos", encoderRevolutions); + TELE.addData("shaftEncoderVel", velocity); + + double velPID = 0.0; + + if (mode == 0) { + rightShooter.setPower(parameter); + leftShooter.setPower(parameter); + } else if (mode == 1) { + + + + // --- FEEDFORWARD BASE POWER --- + double feed = kF * parameter; // Example: vel=2500 → feed=0.5 + + // --- PROPORTIONAL CORRECTION --- + double error = parameter - velocity; + double correction = kP * error; + + // limit how fast power changes (prevents oscillation) + correction = Math.max(-maxStep, Math.min(maxStep, correction)); + + // --- FINAL MOTOR POWER --- + velPID = feed + correction; + + // clamp to allowed range + velPID = Math.max(0, Math.min(1, velPID)); + + rightShooter.setPower(velPID); + leftShooter.setPower(velPID); + + } + + lastTimeStamp = getRuntime(); + lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048; + + + + TELE.update(); + + + + } + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java new file mode 100644 index 0000000..59986fa --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.tests; + +public class TrackingTest { +}