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

Merge pull request #13 from Technical-Turbulence-FTC/feature/interpolation

Browse Source 
Feature/interpolation
This commit is contained in:
abhiram vishnubhotla
2026-01-22 20:21:05 -06:00
committed by GitHub
parent daccec4fdd 2f0fcad128
commit 298b7bca8c
4 changed files with 194 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -50,6 +50,8 @@ public class TeleopV3 extends LinearOpMode {
public static double spindexPos = spindexer_intakePos1;
public static double spinPow = 0.09;
public static double bMult = 1, bDiv = 2200;
public static double limelightKp = 0.001; // Proportional gain for limelight auto-aim
public static double limelightDeadband = 0.5; // Ignore tx values smaller than this
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static boolean manualTurret = true;
public double vel = 3000;
@@ -121,6 +123,7 @@ public class TeleopV3 extends LinearOpMode {
private double transferStamp = 0.0;
private int tickerA = 1;
private boolean transferIn = false;
boolean turretInterpolate = true;
public static double velPrediction(double distance) {
if (distance < 30) {
@@ -411,7 +414,7 @@ public class TeleopV3 extends LinearOpMode {
targetingSettings = targeting.calculateSettings
(robotX,robotY,robotHeading,0.0);
(robotX,robotY,robotHeading,0.0, turretInterpolate);
//VELOCITY AUTOMATIC
if (targetingVel) {
@@ -443,46 +446,100 @@ public class TeleopV3 extends LinearOpMode {
//TODO: test the camera teleop code
// TODO: TEST THIS 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;
//
// LLResult result = robot.light.getLatestResult();
// if (result != null) {
// if (result.isValid()) {
// bearing = result.getTx() * bMult;
//
// double bearingCorrection = bearing / bDiv;
//
// error += bearingCorrection;
//
// camTicker++;
// TELE.addData("tx", bearingCorrection);
// TELE.addData("ty", result.getTy());
// }
// }
//
// } else {
// camTicker = 0;
// overrideTurr = false;
// }
LLResult result = robot.limelight.getLatestResult();
boolean limelightActive = false;
if (!overrideTurr) {
double turretMin = 0.13;
double turretMax = 0.83;
if (result != null && result.isValid()) {
double tx = result.getTx();
double ty = result.getTy();
if (Math.abs(tx) > limelightDeadband) {
limelightActive = true;
overrideTurr = true;
double currentTurretPos = servo.getTurrPos();
// + tx means tag is right, so rotate right
double adjustment = -tx * limelightKp;
// calculate new position
double newTurretPos = currentTurretPos + adjustment;
if (newTurretPos < turretMin) {
double forwardDist = turretMin - newTurretPos;
double backwardDist = (currentTurretPos - turretMin) + (turretMax - newTurretPos);
// check path distance
if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
newTurretPos = turretMax - (turretMin - newTurretPos);
} else {
newTurretPos = turretMin;
}
} else if (newTurretPos > turretMax) {
double forwardDist = newTurretPos - turretMax;
double backwardDist = (turretMax - currentTurretPos) + (newTurretPos - turretMin);
if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
newTurretPos = turretMin + (newTurretPos - turretMax);
} else {
newTurretPos = turretMax;
}
}
// Final clamp
if (newTurretPos < turretMin) {
newTurretPos = turretMin;
} else if (newTurretPos > turretMax) {
newTurretPos = turretMax;
}
pos = newTurretPos;
turretPos = pos;
camTicker++;
TELE.addData("tx", tx);
TELE.addData("ty", ty);
TELE.addData("limelightAdjustment", adjustment);
TELE.addData("limelightActive", true);
} else {
limelightActive = true;
overrideTurr = true;
TELE.addData("tx", tx);
TELE.addData("ty", ty);
TELE.addData("limelightActive", true);
TELE.addData("limelightStatus", "Centered");
}
} else {
if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) {
TELE.addData("limelightActive", false);
TELE.addData("limelightStatus", "No target");
} else {
camTicker = 0;
overrideTurr = false;
limelightActive = false;
}
}
if (!limelightActive && !overrideTurr) {
turretPos = pos;
}
TELE.addData("posS3", turretPos);
if (manualTurret) {
if (manualTurret && !limelightActive) {
pos = turrDefault + (manualOffset / 100) + error;
}
if (!overrideTurr) {
if (!overrideTurr && !limelightActive) {
turretPos = pos;
}
if (Math.abs(gamepad2.left_stick_x)>0.2) {
if (Math.abs(gamepad2.left_stick_x)>0.2 && !limelightActive) {
manualOffset += 1.35 * gamepad2.left_stick_x;
}
@@ -873,6 +930,7 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData( "robotY", robotY);
TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData( "robotInchesY", targeting.robotInchesY);
TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);

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

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

@@ -84,28 +84,58 @@ public class Targeting {
{
}
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) {
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
Settings recommendedSettings = new Settings(0.0, 0.0);
double cos45 = Math.cos(Math.toRadians(-45));
double sin45 = Math.sin(Math.toRadians(-45));
double rotatedY = (robotX -40.0) * sin45 + (robotY +7.0) * cos45;
double rotatedX = (robotX -40.0) * cos45 - (robotY +7.0) * sin45;
double rotatedY = (robotX - 40.0) * sin45 + (robotY + 7.0) * cos45;
double rotatedX = (robotX - 40.0) * cos45 - (robotY + 7.0) * sin45;
// Convert robot coordinates to inches
robotInchesX = rotatedX * unitConversionFactor;
robotInchesY = rotatedY * unitConversionFactor;
// Find approximate location in the grid
robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1);
robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
// Use Grid Location to perform lookup
// Keep it simple for now but may want to interpolate results
//clamp
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
// basic search
if(!interpolate) {
if ((robotGridY < 6) && (robotGridX <6)) {
recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM;
recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle;
}
return recommendedSettings;
} else {
// bilinear interpolation
int x0 = robotGridX;
int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1);
int y0 = gridY;
int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1);
double x = (robotInchesX - (x0 * tileSize)) / tileSize;
double y = (robotInchesY - (y0 * tileSize)) / tileSize;
double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM;
double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM;
double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM;
double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM;
double angle00 = KNOWNTARGETING[y0][x0].hoodAngle;
double angle10 = KNOWNTARGETING[y0][x1].hoodAngle;
double angle01 = KNOWNTARGETING[y1][x0].hoodAngle;
double angle11 = KNOWNTARGETING[y1][x1].hoodAngle;
recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11;
recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11;
return recommendedSettings;
}
}
}

128
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){
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);
}
}