From a1585e605f5703f0167ff0c48cec0282c5c0157f Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Tue, 25 Nov 2025 15:54:15 -0600 Subject: [PATCH] Shooter Test --- .../teamcode/subsystems/AprilTagWebcam.java | 3 +- .../ftc/teamcode/subsystems/Robot.java | 25 --------- .../teamcode/tests/AprilTagWebcamExample.java | 2 +- .../ftc/teamcode/tests/ShooterTest.java | 56 +++++++++---------- .../ftc/teamcode/tests/TrackingTest.java | 44 ++++++++++++++- .../ftc/teamcode/utils/Robot.java | 1 + 6 files changed, 73 insertions(+), 58 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java 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 6288a4f..4cacde5 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 @@ -8,6 +8,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.vision.VisionPortal; import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; @@ -34,7 +35,7 @@ public class AprilTagWebcam { .build(); VisionPortal.Builder builder = new VisionPortal.Builder(); - builder.setCamera(robot.webcamName); + builder.setCamera(robot.webcam); 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 deleted file mode 100644 index 7375f3f..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Robot.java +++ /dev/null @@ -1,25 +0,0 @@ -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 3957398..2a74ca3 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; +import org.firstinspires.ftc.teamcode.utils.Robot; @Config @TeleOp 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 index e5eabd6..8eb3a1b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -1,16 +1,13 @@ 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; +import org.firstinspires.ftc.teamcode.utils.Robot; @Config @TeleOp @@ -18,35 +15,32 @@ 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 + //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED + public static double transferPower = 0.0; + public static double hoodPos = 0.501; + + public static double turretPos = 0.501; + Robot robot; + private double lastEncoderRevolutions = 0.0; + private double lastTimeStamp = 0.0; + @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - leftShooter = robot.shooter1; - rightShooter = robot.shooter2; - encoder = robot.shooter1; + DcMotorEx leftShooter = robot.shooter1; + DcMotorEx rightShooter = robot.shooter2; + DcMotorEx encoder = robot.shooter1; MultipleTelemetry TELE = new MultipleTelemetry( telemetry, FtcDashboard.getInstance().getTelemetry() ); - - waitForStart(); if (isStopRequested()) return; @@ -55,29 +49,24 @@ public class ShooterTest extends LinearOpMode { double kF = 1.0 / MAX_RPM; // baseline feedforward + double encoderRevolutions = (double) encoder.getCurrentPosition() / 2048; - - - encoderRevolutions = (double) encoder.getCurrentPosition() / 2048; - - double velocity = -60*(encoderRevolutions - lastEncoderRevolutions) / (getRuntime() - lastTimeStamp); + 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("leftShooterPower", leftShooter.getPower()); TELE.addData("rightShooterPower", rightShooter.getPower()); TELE.addData("shaftEncoderPos", encoderRevolutions); TELE.addData("shaftEncoderVel", velocity); - double velPID = 0.0; + double velPID; 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 @@ -102,12 +91,19 @@ public class ShooterTest extends LinearOpMode { lastTimeStamp = getRuntime(); lastEncoderRevolutions = (double) encoder.getCurrentPosition() / 2048; + if (hoodPos != 0.501) { + robot.hood.setPosition(hoodPos); + } + if (turretPos!=0.501){ + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(turretPos); + } + + robot.transfer.setPower(transferPower); 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 index 59986fa..5b125c7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TrackingTest.java @@ -1,4 +1,46 @@ package org.firstinspires.ftc.teamcode.tests; -public class TrackingTest { +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.arcrobotics.ftclib.gamepad.GamepadEx; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.subsystems.Drivetrain; +import org.firstinspires.ftc.teamcode.utils.Robot; + +public class TrackingTest extends LinearOpMode { + + Robot robot; + + Drivetrain drivetrain; + + MultipleTelemetry TELE; + + GamepadEx g1; + + + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + g1 = new GamepadEx(gamepad1); + + drivetrain = new Drivetrain(robot, TELE, g1); + + + waitForStart(); + if(isStopRequested()) return; + while (opModeIsActive()){ + drivetrain.update(); + } + + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index cbae6f0..3b56922 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -40,6 +40,7 @@ public class Robot { public Servo rejecter; + public Servo turr1; public Servo turr2;