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; boolean reject = false;
double xOffset = 0.0; double xOffset = 0.0;
double yOffset = 0.0; double yOffset = 0.0;
double hOffset = 0.0;
// double headingOffset = 0.0; // double headingOffset = 0.0;
int ticker = 0; int ticker = 0;
@@ -166,10 +167,11 @@ public class TeleopV3 extends LinearOpMode {
double robX = drive.localizer.getPose().position.x; double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y; double robY = drive.localizer.getPose().position.y;
double robH = drive.localizer.getPose().heading.toDouble();
double robotX = robX + xOffset; double robotX = robX + xOffset;
double robotY = robY + yOffset; double robotY = robY + yOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble(); double robotHeading = robH + hOffset;
double goalX = -15; double goalX = -15;
double goalY = 0; double goalY = 0;
@@ -194,6 +196,7 @@ public class TeleopV3 extends LinearOpMode {
turret.relocalize(); turret.relocalize();
xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
yOffset = (turret.getLimelightX() * 39.3701) - robY; yOffset = (turret.getLimelightX() * 39.3701) - robY;
hOffset = (Math.toRadians(turret.getLimelightH())) - robH;
} else { } else {
turret.trackGoal(deltaPose); turret.trackGoal(deltaPose);
} }
@@ -353,13 +356,14 @@ public class TeleopV3 extends LinearOpMode {
// Targeting Debug // Targeting Debug
TELE.addData("robotX", robotX); TELE.addData("robotX", robotX);
TELE.addData("robotY", robotY); TELE.addData("robotY", robotY);
TELE.addData("robotInchesX", targeting.robotInchesX); TELE.addData("robot H", robotHeading);
TELE.addData("robotInchesY", targeting.robotInchesY); // TELE.addData("robotInchesX", targeting.robotInchesX);
TELE.addData("Targeting Interpolate", turretInterpolate); // TELE.addData("robotInchesY", targeting.robotInchesY);
// TELE.addData("Targeting Interpolate", turretInterpolate);
TELE.addData("Targeting GridX", targeting.robotGridX); TELE.addData("Targeting GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY); TELE.addData("Targeting GridY", targeting.robotGridY);
TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); // TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
// TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub) // TELE.addData("Voltage", voltage); // returns alleged recorded voltage (not same as driver hub)
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); 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("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH()));
TELE.update(); 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 xPos = null;
Double yPos = null; Double yPos = null;
Double zPos = null; Double zPos = null;
Double hPos = null;
result = webcam.getLatestResult(); result = webcam.getLatestResult();
if (result != null) { if (result != null) {
if (result.isValid()) { if (result.isValid()) {
@@ -122,6 +123,7 @@ public class Turret {
xPos = limelightTagPose.getPosition().x; xPos = limelightTagPose.getPosition().x;
yPos = limelightTagPose.getPosition().y; yPos = limelightTagPose.getPosition().y;
zPos = limelightTagPose.getPosition().z; zPos = limelightTagPose.getPosition().z;
hPos = limelightTagPose.getOrientation().getYaw();
} }
} }
@@ -131,6 +133,7 @@ public class Turret {
limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX); limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX);
limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY); limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY);
limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ); limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ);
limelightTagH = (alphaPosConstant*hPos) + ((1-alphaPosConstant)*limelightTagH);
} }
} }
@@ -148,11 +151,13 @@ public class Turret {
double limelightTagX = 0.0; double limelightTagX = 0.0;
double limelightTagY = 0.0; double limelightTagY = 0.0;
double limelightTagZ = 0.0; double limelightTagZ = 0.0;
double limelightTagH = 0.0;
public double getLimelightX() { public double getLimelightX() {
return limelightTagX; return limelightTagX;
} }
public double getLimelightY() {return limelightTagY;} public double getLimelightY() {return limelightTagY;}
public double getLimelightZ(){return limelightTagZ;} public double getLimelightZ(){return limelightTagZ;}
public double getLimelightH(){return limelightTagH;}
public void relocalize(){ public void relocalize(){
setTurret(turrDefault); setTurret(turrDefault);