From fbdeb6e291316a3eb4a0af13c72533a496bfaeaf Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Thu, 22 Jan 2026 21:04:25 -0600 Subject: [PATCH] Turret works y8ippee horray hurrah ig --- .../ftc/teamcode/constants/Poses.java | 4 +- .../ftc/teamcode/teleop/TeleopV3.java | 77 +++++-------------- .../ftc/teamcode/tests/TurretTest.java | 10 ++- .../ftc/teamcode/utils/Robot.java | 2 + .../ftc/teamcode/utils/Turret.java | 21 +++-- 5 files changed, 46 insertions(+), 68 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index d2a1382..bf2fa34 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -12,7 +12,7 @@ public class Poses { public static double relativeGoalHeight = goalHeight - turretHeight; - public static Pose2d goalPose = new Pose2d(-15, 0, 0); + public static Pose2d goalPose = new Pose2d(-10, 0, 0); public static double rx1 = 40, ry1 = -7, rh1 = 0; public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140); @@ -38,6 +38,6 @@ public class Poses { public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140); public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this - public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1); + public static Pose2d teleStart = new Pose2d(0, 0, 0); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 1830322..91f0f24 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -1,11 +1,9 @@ package org.firstinspires.ftc.teamcode.teleop; -import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault; import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; import static org.firstinspires.ftc.teamcode.utils.Servos.spinF; import static org.firstinspires.ftc.teamcode.utils.Servos.spinI; @@ -21,7 +19,6 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; import com.arcrobotics.ftclib.controller.PIDFController; -import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -29,11 +26,13 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; import org.firstinspires.ftc.teamcode.utils.Spindexer; import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; import java.util.ArrayList; import java.util.List; @@ -152,7 +151,7 @@ public class TeleopV3 extends LinearOpMode { drive = new MecanumDrive(hardwareMap, teleStart); spindexer = new Spindexer(hardwareMap); targeting = new Targeting(); - targetingSettings = new Targeting.Settings(0.0,0.0); + targetingSettings = new Targeting.Settings(0.0, 0.0); PIDFController tController = new PIDFController(tp, ti, td, tf); @@ -166,6 +165,12 @@ public class TeleopV3 extends LinearOpMode { // robot.limelight.start(); + AprilTagWebcam webcam = new AprilTagWebcam(); + webcam.init(robot, TELE); + + Turret turret = new Turret(robot, TELE, webcam); + waitForStart(); + waitForStart(); if (isStopRequested()) return; @@ -379,39 +384,21 @@ public class TeleopV3 extends LinearOpMode { double robotY = robY - yOffset; double robotHeading = drive.localizer.getPose().heading.toDouble(); - double goalX = -10; + double goalX = -15; double goalY = 0; - double dx = goalX - robotX; // delta x from robot to goal - double dy = goalY - robotY; // delta y from robot to goal + double dx = robotX - goalX; // delta x from robot to goal + double dy = robotY - goalY; // delta y from robot to goal + Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); - desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360; - - desiredTurretAngle += manualOffset + error; - - offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset)); - - if (offset > 135) { - offset -= 360; - } - - double pos = turrDefault; - - TELE.addData("offset", offset); - - pos -= offset * ((double) 1 / 360); - - if (pos < 0.13) { - pos = 0.13; - } else if (pos > 0.83) { - pos = 0.83; - } - - targetingSettings = targeting.calculateSettings - (robotX,robotY,robotHeading,0.0); + (robotX, robotY, robotHeading, 0.0); + + turret.trackGoal(deltaPose); + + webcam.update(); //VELOCITY AUTOMATIC if (targetingVel) { @@ -443,8 +430,6 @@ public class TeleopV3 extends LinearOpMode { //TODO: test the camera teleop code - TELE.addData("posS2", pos); - // if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving // double bearing; // @@ -468,27 +453,6 @@ public class TeleopV3 extends LinearOpMode { // overrideTurr = false; // } - if (!overrideTurr) { - turretPos = pos; - } - - TELE.addData("posS3", turretPos); - - if (manualTurret) { - pos = turrDefault + (manualOffset / 100) + error; - } - - if (!overrideTurr) { - turretPos = pos; - } - - if (Math.abs(gamepad2.left_stick_x)>0.2) { - manualOffset += 1.35 * gamepad2.left_stick_x; - } - - robot.turr1.setPosition(pos); - robot.turr2.setPosition(1 - pos); - //HOOD: if (targetingHood) { @@ -672,7 +636,6 @@ public class TeleopV3 extends LinearOpMode { } } - // // if (shootAll) { // @@ -870,9 +833,9 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("shootall commanded", shootAll); // Targeting Debug TELE.addData("robotX", robotX); - TELE.addData( "robotY", robotY); + TELE.addData("robotY", robotY); TELE.addData("robotInchesX", targeting.robotInchesX); - TELE.addData( "robotInchesY", targeting.robotInchesY); + TELE.addData("robotInchesY", targeting.robotInchesY); TELE.addData("Targeting GridX", targeting.robotGridX); TELE.addData("Targeting GridY", targeting.robotGridY); TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 9c8a1cd..2542488 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -30,12 +30,20 @@ public class TurretTest extends LinearOpMode { Turret turret = new Turret(robot, TELE, webcam); waitForStart(); - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(10, 0,0)); + + MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0)); while(opModeIsActive()){ drive.updatePoseEstimate(); turret.trackGoal(drive.localizer.getPose()); + + webcam.update(); + webcam.displayAllTelemetry(); + + + + TELE.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 a3cb45c..e4345b1 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 @@ -79,8 +79,10 @@ public class Robot { shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + shooter1.setVelocity(1400); shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + shooter2.setVelocity(1400); hood = hardwareMap.get(Servo.class, "hood"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index a32b58e..202293c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -13,12 +13,14 @@ public class Turret { public static double turretTolerance = 0.02; public static double turrPosScalar = 1.009; - public static double turret180Range = 0.6; + public static double turret180Range = 0.4; public static double turrDefault = 0.4; - public static double cameraBearingEqual = 1.5; - public static double errorLearningRate = 2; + public static double cameraBearingEqual = 1; + public static double errorLearningRate = 0.15; public static double turrMin = 0.2; public static double turrMax = 0.8; + public static double deltaAngleThreshold = 0.02; + public static double angleMultiplier = 0.0; Robot robot; MultipleTelemetry TELE; AprilTagWebcam webcam; @@ -27,6 +29,8 @@ public class Turret { private double offset = 0.0; private double bearing = 0.0; + + public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) { this.TELE = tele; this.robot = rob; @@ -85,9 +89,7 @@ public class Turret { return obeliskID; } - public void update() { - } /* Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading @@ -107,15 +109,18 @@ public class Turret { // Turret angle needed relative to robot double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; + turretAngleDeg = -turretAngleDeg; + // Normalize to [-180, 180] while (turretAngleDeg > 180) turretAngleDeg -= 360; while (turretAngleDeg < -180) turretAngleDeg += 360; - /* ---------------- APRILTAG CORRECTION ---------------- */ + /* ---------------- APRILTAG CORRECTION ---------------- */ +// double tagBearingDeg = getBearing(); // + = target is to the left - if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) < cameraBearingEqual) { + if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual) { // Slowly learn turret offset (persistent calibration) offset -= tagBearingDeg * errorLearningRate; } @@ -124,7 +129,7 @@ public class Turret { /* ---------------- ANGLE → SERVO ---------------- */ - double turretPos = turrDefault + (turretAngleDeg / (turret180Range * 2.0)); + double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); // Clamp to servo range turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));