From 6b092bdaebec5b3fd9621ea88df5af57b3990f18 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sat, 18 Apr 2026 00:03:07 -0500 Subject: [PATCH] Redid turret math to adjust to pedropathing field. --- .../autonomous/actions/AutoActions.java | 20 ++++++----- .../disabled/Auto_LT_Close_12Ball.java | 3 +- .../disabled/Auto_LT_Close_GateOpen.java | 3 +- .../ftc/teamcode/teleop/TeleopV3.java | 3 +- .../ftc/teamcode/tests/ShooterTest.java | 3 +- .../ftc/teamcode/tests/TurretTest.java | 34 ++++++++++++------- .../ftc/teamcode/utils/Targeting.java | 2 +- .../ftc/teamcode/utils/Turret.java | 28 ++++++++++----- 8 files changed, 60 insertions(+), 36 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 968545d..d696af1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -18,6 +18,8 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.geometry.Pose; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -525,7 +527,7 @@ public class AutoActions { drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); + Pose2d currentPose = null; //drive.localizer.getPose(); if (ticker == 0) { stamp = System.currentTimeMillis(); @@ -542,10 +544,10 @@ public class AutoActions { ticker++; - double robotX = currentPose.position.x; - double robotY = currentPose.position.y; + double robotX = 0.0;//currentPose.position.x; + double robotY = 0.0;//currentPose.position.y; - double robotHeading = currentPose.heading.toDouble(); + double robotHeading = 0.0;//currentPose.heading.toDouble(); double goalX = -15; double goalY = 0; @@ -554,11 +556,11 @@ public class AutoActions { double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose; + Pose deltaPose; if (posX != 0.501) { - deltaPose = new Pose2d(posX, posY, Math.toRadians(posH)); + deltaPose = new Pose(posX, posY, Math.toRadians(posH)); } else { - deltaPose = new Pose2d(dx, dy, robotHeading); + deltaPose = new Pose(dx, dy, robotHeading); } Turret.limelightUsed = true; @@ -646,9 +648,9 @@ public class AutoActions { double dx = robotX - goalX; // delta x from robot to goal double dy = robotY - goalY; // delta y from robot to goal - Pose2d deltaPose; + Pose deltaPose; if (turr == 0.501) { - deltaPose = new Pose2d(dx, dy, robotHeading); + deltaPose = new Pose(dx, dy, robotHeading); if (!detectingObelisk) { turret.trackGoal(deltaPose); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java index b4ca67a..9d0f8c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java @@ -72,6 +72,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -489,7 +490,7 @@ public class Auto_LT_Close_12Ball extends LinearOpMode { 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); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java index c32dc38..1ff9d28 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java @@ -77,6 +77,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -628,7 +629,7 @@ public class Auto_LT_Close_GateOpen extends LinearOpMode { 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); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); 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 af73da3..8ab4d34 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 @@ -12,6 +12,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDFController; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -238,7 +239,7 @@ public class TeleopV3 extends LinearOpMode { 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); + Pose deltaPose = new Pose(dx, dy, robotHeading); // double distanceToGoal = Math.sqrt(dx * dx + dy * dy); 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 12a3c8b..b058ea9 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 @@ -15,6 +15,7 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -107,7 +108,7 @@ public class ShooterTest extends LinearOpMode { double dx = robX - goalX; // delta x from robot to goal double dy = robY - goalY; // delta y from robot to goal - Pose2d deltaPose = new Pose2d(dx, dy, robotHeading); + Pose deltaPose = new Pose(dx, dy, robotHeading); double distanceToGoal = Math.sqrt(dx * dx + dy * dy); 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 34c34e3..0b3fcaf 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 @@ -4,11 +4,14 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Turret; @@ -25,26 +28,31 @@ public class TurretTest extends LinearOpMode { ); Turret turret = new Turret(robot, TELE, robot.limelight); + + Follower follower; + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(72, 72, 0); + follower.setStartingPose(start); + follower.update(); waitForStart(); - MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0)); + Turret.limelightUsed = false; while(opModeIsActive()){ + follower.update(); + turret.trackGoal(follower.getPose()); - drive.updatePoseEstimate(); - turret.trackGoal(drive.localizer.getPose()); +// TELE.addData("tpos", turret.getTurrPos()); +// TELE.addData("Limelight tx", turret.getBearing()); +// TELE.addData("Limelight ty", turret.getTy()); +// TELE.addData("Limelight X", turret.getLimelightX()); +// TELE.addData("Limelight Y", turret.getLimelightY()); - TELE.addData("tpos", turret.getTurrPos()); - TELE.addData("Limelight tx", turret.getBearing()); - TELE.addData("Limelight ty", turret.getTy()); - TELE.addData("Limelight X", turret.getLimelightX()); - TELE.addData("Limelight Y", turret.getLimelightY()); +// if(zeroTurr){ +// turret.zeroTurretEncoder(); +// } - if(zeroTurr){ - turret.zeroTurretEncoder(); - } - - TELE.update(); +// TELE.update(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index bec1745..c9edac2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -84,7 +84,7 @@ public class Targeting { double cos54 = Math.cos(Math.toRadians(-54)); double sin54 = Math.sin(Math.toRadians(-54)); - + //TODO: change code so it uses pedropathing paths public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); int gridX; 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 96ddb29..ceb2432 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 @@ -6,6 +6,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDController; +import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; @@ -243,17 +244,25 @@ public class Turret { } double targetTurretPos; - public void trackGoal(Pose2d deltaPos) { + public void trackGoal(Pose deltaPos) { /* ---------------- FIELD → TURRET GEOMETRY ---------------- */ + double posX; + if (Color.redAlliance){ + posX = 144 - deltaPos.getX(); + } else { + posX = deltaPos.getX(); + } + double posY = 144 - deltaPos.getY(); + double posH = Math.toDegrees(deltaPos.getHeading()); + while (posH > 180) posH -= 360; + while (posH < -180) posH += 360; // Angle from robot to goal in robot frame - double desiredTurretAngleDeg = Math.toDegrees( - Math.atan2(deltaPos.position.y, deltaPos.position.x) - ); + double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45; // Robot heading (field → robot) - double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble()); + double robotHeadingDeg = posH + 135; // Turret angle needed relative to robot double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; @@ -353,15 +362,16 @@ public class Turret { /* ---------------- TELEMETRY ---------------- */ -// TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); -// TELE.addData("Target Pos", "%.3f", targetTurretPos); -// TELE.addData("Current Pos", "%.3f", currentPos); -// TELE.addData("Commanded Pos", "%.3f", turretPos); + TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); + TELE.addData("Target Pos", "%.3f", targetTurretPos); + TELE.addData("Current Localization Pos", "%.3f", deltaPos); + TELE.addData("Commanded Pos", "%.3f", turretPos); // TELE.addData("LL Valid", result.isValid()); // TELE.addData("LL getTx", result.getTx()); // TELE.addData("LL Offset", offset); // TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); // TELE.addData("Learned Offset", "%.2f", offset); + TELE.update(); } }