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
@TeleOp
public class TeleopV3 extends LinearOpMode {
private double metersToInches = 39.3700787402;
public static double manualVel = 3000;
public static double hoodDefaultPos = 0.5;
private double predictedResetX, predictedResetY, predictedResetH;
@@ -275,15 +276,17 @@ public class TeleopV3 extends LinearOpMode {
gamepad2.rumble(500);
}
//TODO: relocalize using limelight
// if (relocalize){
// turret.relocalize();
// xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
// yOffset = (turret.getLimelightX() * 39.3701) - robY;
// hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
// } else {
if (relocalize){
turret.relocalize();
xOffset = ((turret.getLimelightY()*metersToInches)+72) - robX;
yOffset = (72-(turret.getLimelightX()*metersToInches)) - robY;
hOffset = (Math.toRadians(turret.getLimelightH() + 90));
while (hOffset > 180) {hOffset-=360;}
while (hOffset < -180) {hOffset+=360;}
hOffset = hOffset - robH;
} else {
turret.trackGoal(deltaPose);
//}
}
//VELOCITY AUTOMATIC
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
Double xPos = null;
Double yPos = null;
Double zPos = null;
double zPos;
Double hPos = null;
result = webcam.getLatestResult();
if (result != null) {
@@ -117,27 +117,18 @@ public class Turret {
tx = result.getTx();
ty = result.getTy();
if (TeleopV3.relocalize){
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
for (LLResultTypes.FiducialResult fiducial : fiducials) {
limelightTagPose = fiducial.getRobotPoseTargetSpace();
if (limelightTagPose != null){
xPos = limelightTagPose.getPosition().x;
yPos = limelightTagPose.getPosition().y;
zPos = limelightTagPose.getPosition().z;
hPos = limelightTagPose.getOrientation().getYaw();
}
zPos = result.getBotpose().getPosition().z;
if (zPos < 0.15){
xPos = result.getBotpose().getPosition().x;
yPos = result.getBotpose().getPosition().y;
hPos = result.getBotpose().getOrientation().getYaw();
limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX);
limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY);
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() {
@@ -150,7 +141,6 @@ public class Turret {
return ty;
}
Pose3D limelightTagPose;
double limelightTagX = 0.0;
double limelightTagY = 0.0;
double limelightTagZ = 0.0;