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

limelight relocalization in progress

Browse Source
This commit is contained in:
DanTheMan-byte
2026-04-18 21:34:08 -05:00
parent 2a29e8181b
commit 7eebd42ea2
2 changed files with 20 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -41,6 +41,7 @@ import java.util.List;
@Config @Config
@TeleOp @TeleOp
public class TeleopV3 extends LinearOpMode { public class TeleopV3 extends LinearOpMode {
private double metersToInches = 39.3700787402;
public static double manualVel = 3000; public static double manualVel = 3000;
public static double hoodDefaultPos = 0.5; public static double hoodDefaultPos = 0.5;
private double predictedResetX, predictedResetY, predictedResetH; private double predictedResetX, predictedResetY, predictedResetH;
@@ -275,15 +276,17 @@ public class TeleopV3 extends LinearOpMode {
gamepad2.rumble(500); gamepad2.rumble(500);
} }
//TODO: relocalize using limelight if (relocalize){
// if (relocalize){ turret.relocalize();
// turret.relocalize(); xOffset = ((turret.getLimelightY()*metersToInches)+72) - robX;
// xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; yOffset = (72-(turret.getLimelightX()*metersToInches)) - robY;
// yOffset = (turret.getLimelightX() * 39.3701) - robY; hOffset = (Math.toRadians(turret.getLimelightH() + 90));
// hOffset = (Math.toRadians(turret.getLimelightH())) - robH; while (hOffset > 180) {hOffset-=360;}
// } else { while (hOffset < -180) {hOffset+=360;}
hOffset = hOffset - robH;
} else {
turret.trackGoal(deltaPose); turret.trackGoal(deltaPose);
//} }
//VELOCITY AUTOMATIC //VELOCITY AUTOMATIC
if (autoVel) { if (autoVel) {

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

@@ -109,7 +109,7 @@ public class Turret {
private void limelightRead() { // only for tracking purposes, not general reads private void limelightRead() { // only for tracking purposes, not general reads
Double xPos = null; Double xPos = null;
Double yPos = null; Double yPos = null;
Double zPos = null; double zPos;
Double hPos = null; Double hPos = null;
result = webcam.getLatestResult(); result = webcam.getLatestResult();
if (result != null) { if (result != null) {
@@ -117,27 +117,18 @@ public class Turret {
tx = result.getTx(); tx = result.getTx();
ty = result.getTy(); ty = result.getTy();
if (TeleopV3.relocalize){ if (TeleopV3.relocalize){
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults(); zPos = result.getBotpose().getPosition().z;
for (LLResultTypes.FiducialResult fiducial : fiducials) { if (zPos < 0.15){
limelightTagPose = fiducial.getRobotPoseTargetSpace(); xPos = result.getBotpose().getPosition().x;
if (limelightTagPose != null){ yPos = result.getBotpose().getPosition().y;
xPos = limelightTagPose.getPosition().x; hPos = result.getBotpose().getOrientation().getYaw();
yPos = limelightTagPose.getPosition().y; limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
zPos = limelightTagPose.getPosition().z; limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
hPos = limelightTagPose.getOrientation().getYaw(); limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
}
} }
} }
} }
} }
if (xPos != null){
if (zPos<0) {
limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ);
limelightTagH = (alphaPosConstant * hPos) + ((1 - alphaPosConstant) * limelightTagH);
}
}
} }
public double getBearing() { public double getBearing() {
@@ -150,7 +141,6 @@ public class Turret {
return ty; return ty;
} }
Pose3D limelightTagPose;
double limelightTagX = 0.0; double limelightTagX = 0.0;
double limelightTagY = 0.0; double limelightTagY = 0.0;
double limelightTagZ = 0.0; double limelightTagZ = 0.0;