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

limelight relocalization of x,y is done. Still need to do heading

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-22 17:44:57 -06:00
parent 09347ce479
commit f69bffc3ee
4 changed files with 48 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -5,17 +5,17 @@ import com.acmerobotics.dashboard.config.Config;
@Config
public class ServoPositions {
public static double spindexer_intakePos1 = 0.06; //0.13;
public static double spindexer_intakePos1 = 0.13; //0.13;
public static double spindexer_intakePos2 = 0.25; //0.33;//0.5;
public static double spindexer_intakePos2 = 0.32; //0.33;//0.5;
public static double spindexer_intakePos3 = 0.43; //0.53;//0.66;
public static double spindexer_intakePos3 = 0.5; //0.53;//0.66;
public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24;
public static double spindexer_outtakeBall3b = 0.15; //0.65; //0.24;
public static double spindexer_outtakeBall3 = 0.78; //0.65; //0.24;
public static double spindexer_outtakeBall3b = 0.22; //0.65; //0.24;
public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4;
public static double spindexer_outtakeBall2 = 0.7; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.42; //0.27; //0.4;
public static double spinStartPos = 0.05;
public static double spinEndPos = 0.95;
@@ -27,7 +27,7 @@ public class ServoPositions {
public static double hoodAuto = 0.27;
public static double hoodOffset = 0.05; // offset from 0.93
public static double hoodOffset = -0.04; // offset from 0.93
public static double turret_redClose = 0;
public static double turret_blueClose = 0;

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

@@ -75,6 +75,7 @@ public class TeleopV3 extends LinearOpMode {
int intakeTicker = 0;
private boolean shootAll = false;
boolean relocalize = false;
@Override
public void runOpMode() throws InterruptedException {
@@ -111,7 +112,6 @@ public class TeleopV3 extends LinearOpMode {
light.setState(StateEnums.LightState.MANUAL);
limelightUsed = true;
robot.light.setPosition(1);
while (opModeInInit()) {
robot.limelight.start();
if (redAlliance) {
@@ -122,6 +122,7 @@ public class TeleopV3 extends LinearOpMode {
light.setManualLightColor(Color.LightBlue);
}
robot.light.setPosition(1);
light.update();
}
@@ -166,8 +167,8 @@ public class TeleopV3 extends LinearOpMode {
double robX = drive.localizer.getPose().position.x;
double robY = drive.localizer.getPose().position.y;
double robotX = robX - xOffset;
double robotY = robY - yOffset;
double robotX = robX + xOffset;
double robotY = robY + yOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -15;
@@ -182,7 +183,20 @@ public class TeleopV3 extends LinearOpMode {
targetingSettings = targeting.calculateSettings
(robotX, robotY, robotHeading, 0.0, turretInterpolate);
turret.trackGoal(deltaPose);
//RELOCALIZATION
if (gamepad2.squareWasPressed()){
relocalize = !relocalize;
gamepad2.rumble(500);
}
if (relocalize){
turret.relocalize();
xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX;
yOffset = (turret.getLimelightX() * 39.3701) - robY;
} else {
turret.trackGoal(deltaPose);
}
//VELOCITY AUTOMATIC
if (autoVel) {
@@ -351,8 +365,8 @@ public class TeleopV3 extends LinearOpMode {
TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime());
TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin());
TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin());
TELE.addData("Tag Pos X", turret.getLimelightX());
TELE.addData("Tag Pos Y", turret.getLimelightY());
TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset));
TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701);
TELE.update();

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

@@ -87,7 +87,7 @@ public class Targeting {
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
Settings recommendedSettings = new Settings(0.0, 0.0);
if (redAlliance){
if (!redAlliance){
sin54 = Math.sin(Math.toRadians(54));
} else {
sin54 = Math.sin(Math.toRadians(-54));

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

@@ -27,7 +27,7 @@ public class Turret {
public static double turrMin = 0;
public static double turrMax = 1;
public static boolean limelightUsed = true;
public static double limelightPosOffset = 5;
public static double manualOffset = 0.0;
// public static double visionCorrectionGain = 0.08; // Single tunable gain
@@ -99,8 +99,11 @@ public class Turret {
return Math.abs(pos - this.getTurrPos()) < turretTolerance;
}
public static double alphaPosConstant = 0.3;
private void limelightRead() { // only for tracking purposes, not general reads
Double xPos = null;
Double yPos = null;
Double zPos = null;
result = webcam.getLatestResult();
if (result != null) {
if (result.isValid()) {
@@ -116,13 +119,19 @@ public class Turret {
for (LLResultTypes.FiducialResult fiducial : fiducials) {
limelightTagPose = fiducial.getRobotPoseTargetSpace();
if (limelightTagPose != null){
limelightTagX = limelightTagPose.getPosition().x;
limelightTagY = limelightTagPose.getPosition().y;
xPos = limelightTagPose.getPosition().x;
yPos = limelightTagPose.getPosition().y;
zPos = limelightTagPose.getPosition().z;
}
}
}
}
if (xPos != null){
limelightTagX = (alphaPosConstant*xPos) + ((1-alphaPosConstant)*limelightTagX);
limelightTagY = (alphaPosConstant*yPos) + ((1-alphaPosConstant)*limelightTagY);
limelightTagZ = (alphaPosConstant*zPos) + ((1-alphaPosConstant)*limelightTagZ);
}
}
public double getBearing() {
@@ -138,12 +147,16 @@ public class Turret {
Pose3D limelightTagPose;
double limelightTagX = 0.0;
double limelightTagY = 0.0;
double limelightTagZ = 0.0;
public double getLimelightX() {
return limelightTagX;
}
public double getLimelightY() {return limelightTagY;}
public double getLimelightZ(){return limelightTagZ;}
public double getLimelightY() {
return limelightTagY;
public void relocalize(){
setTurret(turrDefault);
limelightRead();
}
public int detectObelisk() {