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

heading relocalization done, need to test for flipping and consistency

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-22 17:55:56 -06:00
parent f69bffc3ee
commit 0264cf2c77
2 changed files with 16 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -65,6 +65,7 @@ public class TeleopV3 extends LinearOpMode {
boolean reject = false;
double xOffset = 0.0;
double yOffset = 0.0;
double hOffset = 0.0;
// double headingOffset = 0.0;
int ticker = 0;
@@ -166,10 +167,11 @@ public class TeleopV3 extends LinearOpMode {
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robH = drive.localizer.getPose().heading.toDouble();
double robotX = robX + xOffset;
double robotY = robY + yOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double robotHeading = robH + hOffset;
double goalX = -15;
double goalY = 0;
@@ -194,6 +196,7 @@ public class TeleopV3 extends LinearOpMode {
turret.relocalize();
xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
yOffset = (turret.getLimelightX() * 39.3701) - robY;
hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
} else {
turret.trackGoal(deltaPose);
}
@@ -353,13 +356,14 @@ public class TeleopV3 extends LinearOpMode {
// Targeting Debug
TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY);
TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData("robotInchesY", targeting.robotInchesY);
TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("robot H", robotHeading);
// 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);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
// TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
@@ -367,6 +371,7 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
TELE.update();

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

@@ -104,6 +104,7 @@ public class Turret {
Double xPos = null;
Double yPos = null;
Double zPos = null;
Double hPos = null;
result = webcam.getLatestResult();
if (result != null) {
if (result.isValid()) {
@@ -122,6 +123,7 @@ public class Turret {
xPos = limelightTagPose.getPosition().x;
yPos = limelightTagPose.getPosition().y;
zPos = limelightTagPose.getPosition().z;
hPos = limelightTagPose.getOrientation().getYaw();
}
}
@@ -131,6 +133,7 @@ public class Turret {
limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX);
limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY);
limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ);
limelightTagH = (alphaPosConstant*hPos) + ((1-alphaPosConstant)*limelightTagH);
}
}
@@ -148,11 +151,13 @@ public class Turret {
double limelightTagX = 0.0;
double limelightTagY = 0.0;
double limelightTagZ = 0.0;
double limelightTagH = 0.0;
public double getLimelightX() {
return limelightTagX;
}
public double getLimelightY() {return limelightTagY;}
public double getLimelightZ(){return limelightTagZ;}
public double getLimelightH(){return limelightTagH;}
public void relocalize(){
setTurret(turrDefault);