Redid turret math to adjust to pedropathing field.

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 robotX = 0.0;//currentPose.position.x;
-                double robotY = currentPose.position.y;
+                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);
                     }

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);
 

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);
 

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);
 

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);
 

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();
+//            TELE.addData("tpos", turret.getTurrPos());
-            turret.trackGoal(drive.localizer.getPose());
+//            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());
+//            if(zeroTurr){
-            TELE.addData("Limelight tx", turret.getBearing());
+//                turret.zeroTurretEncoder();
-            TELE.addData("Limelight ty", turret.getTy());
+//            }
-            TELE.addData("Limelight X", turret.getLimelightX());
-            TELE.addData("Limelight Y", turret.getLimelightY());
 
-            if(zeroTurr){
+//            TELE.update();
-                turret.zeroTurretEncoder();
-            }
-
-            TELE.update();
         }
 
     }

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;

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(
+        double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45;
-                Math.atan2(deltaPos.position.y, deltaPos.position.x)
-        );
 
         // 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("Turret Angle (deg)", "%.2f", turretAngleDeg);
-//        TELE.addData("Target Pos", "%.3f", targetTurretPos);
+        TELE.addData("Target Pos", "%.3f", targetTurretPos);
-//        TELE.addData("Current Pos", "%.3f", currentPos);
+        TELE.addData("Current Localization Pos", "%.3f", deltaPos);
-//        TELE.addData("Commanded Pos", "%.3f", turretPos);
+        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();
     }
 
 }