KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Redid turret math to adjust to pedropathing field.

Browse Source
This commit is contained in:
DanTheMan-byte
2026-04-18 00:03:07 -05:00
parent 4cbb09e088
commit 6b092bdaeb
8 changed files with 60 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java
View File

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

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_12Ball.java
View File

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

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/disabled/Auto_LT_Close_GateOpen.java
View File

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

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

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

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java
View File

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

34
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
View File

@@ -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();
}
}

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

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

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -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();
}
}