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

trackGoal expected robot centric view, but was fed a field centric view. simple trig to use a deltaPos instead of just pos

Browse Source
This commit is contained in:
abhiramtx
2026-01-21 19:04:30 -06:00
parent c271c88e45
commit 50212015e3
2 changed files with 69 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,9 +1,12 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -35,7 +38,29 @@ public class TurretTest extends LinearOpMode {
while(opModeIsActive()){
drive.updatePoseEstimate();
turret.trackGoal(drive.localizer.getPose());
Pose2d robotPose = drive.localizer.getPose();
double dx = goalPose.position.x - robotPose.position.x;
double dy = goalPose.position.y - robotPose.position.y;
double heading = robotPose.heading.toDouble();
// field vector -> robot frame... avoids double calculation
double relX = dx * Math.cos(-heading) - dy * Math.sin(-heading);
double relY = dx * Math.sin(-heading) + dy * Math.cos(-heading);
Pose2d deltaPos = new Pose2d(
new Vector2d(relX, relY),
robotPose.heading
);
turret.trackGoal(deltaPos);
TELE.addData("Robot Pose", robotPose);
TELE.addData("Goal Pose", goalPose);
TELE.addData("Delta Pos", deltaPos);
TELE.update();
}
}

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

@@ -5,79 +5,61 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@Config
public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 1.009;
public static double turret180Range = 0.6;
public static double turrDefault = 0.4;
public static double cameraBearingEqual = 1.5;
public static double errorLearningRate = 2;
public static double turretRange = 0.6;
public static double turrMin = 0.2;
public static double turrMax = 0.8;
Robot robot;
MultipleTelemetry TELE;
AprilTagWebcam webcam;
public static double turretTolerance = 0.02;
public static double cameraBearingEqual = 1.5;
public static double errorLearningRate = 0.02; // must be low
public static double maxOffsetDeg = 30.0;
private final Robot robot;
private final MultipleTelemetry TELE;
private final AprilTagWebcam webcam;
private int obeliskID = 0;
private double turrPos = 0.0;
private double offset = 0.0;
private double bearing = 0.0;
private double offsetDeg = 0.0;
public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) {
this.TELE = tele;
this.robot = rob;
this.TELE = tele;
this.webcam = cam;
}
public double getTurrPos() {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3);
public double getTurretPos() {
return robot.turr1Pos.getVoltage() / 3.3;
}
public void manualSetTurret(double pos) {
pos = Range.clip(pos, turrMin, turrMax);
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1-pos);
robot.turr2.setPosition(1.0 - pos);
}
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < turretTolerance;
public boolean turretAt(double pos) {
return Math.abs(pos - getTurretPos()) < turretTolerance;
}
public double getBearing() {
if (redAlliance) {
AprilTagDetection d24 = webcam.getTagById(24);
if (d24 != null) {
bearing = d24.ftcPose.bearing;
return bearing;
} else {
return 1000.0;
}
} else {
AprilTagDetection d20 = webcam.getTagById(20);
if (d20 != null) {
bearing = d20.ftcPose.bearing;
return bearing;
} else {
return 1000.0;
}
}
public double getBearingDeg() {
AprilTagDetection tag =
redAlliance ? webcam.getTagById(24) : webcam.getTagById(20);
return (tag != null) ? tag.ftcPose.bearing : Double.NaN;
}
public int detectObelisk() {
AprilTagDetection id21 = webcam.getTagById(21);
AprilTagDetection id22 = webcam.getTagById(22);
AprilTagDetection id23 = webcam.getTagById(23);
if (id21 != null) {
obeliskID = 21;
} else if (id22 != null) {
obeliskID = 22;
} else if (id23 != null) {
obeliskID = 23;
}
if (webcam.getTagById(21) != null) obeliskID = 21;
else if (webcam.getTagById(22) != null) obeliskID = 22;
else if (webcam.getTagById(23) != null) obeliskID = 23;
return obeliskID;
}
@@ -85,58 +67,34 @@ public class Turret {
return obeliskID;
}
public void update() {
}
/*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
public void trackGoal(Pose2d deltaPos) {
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */
// Angle from robot to goal in robot frame
double desiredTurretAngleDeg = Math.toDegrees(
double turretAngleDeg = Math.toDegrees(
Math.atan2(deltaPos.position.y, deltaPos.position.x)
);
// Robot heading (field → robot)
double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble());
double bearingDeg = getBearingDeg();
// Turret angle needed relative to robot
double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
if (!Double.isNaN(bearingDeg) &&
Math.abs(bearingDeg) < cameraBearingEqual) {
// Normalize to [-180, 180]
while (turretAngleDeg > 180) turretAngleDeg -= 360;
while (turretAngleDeg < -180) turretAngleDeg += 360;
/* ---------------- APRILTAG CORRECTION ---------------- */
double tagBearingDeg = getBearing(); // + = target is to the left
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) < cameraBearingEqual) {
// Slowly learn turret offset (persistent calibration)
offset -= tagBearingDeg * errorLearningRate;
offsetDeg -= bearingDeg * errorLearningRate;
offsetDeg = Range.clip(offsetDeg, -maxOffsetDeg, maxOffsetDeg);
}
turretAngleDeg += offset;
turretAngleDeg += offsetDeg;
/* ---------------- ANGLE → SERVO ---------------- */
double turretPos =
turrDefault + (turretAngleDeg / 180.0) * turretRange;
double turretPos = turrDefault + (turretAngleDeg / (turret180Range * 2.0));
// Clamp to servo range
turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
turretPos = Range.clip(turretPos, turrMin, turrMax);
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1.0 - turretPos);
/* ---------------- TELEMETRY ---------------- */
TELE.addData("Turret Angle", turretAngleDeg);
TELE.addData("Bearing", tagBearingDeg);
TELE.addData("Offset", offset);
TELE.addData("Turret Angle (deg)", turretAngleDeg);
TELE.addData("Offset (deg)", offsetDeg);
TELE.addData("Tag Bearing (deg)", bearingDeg);
TELE.addData("Turret Servo", turretPos);
}
}