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

added obelisk code

Browse Source
This commit is contained in:
Keshav Anand
2026-05-25 16:29:38 -07:00
parent ed970eaf38
commit 9ab69f8fbe
1 changed files with 51 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java
View File

@@ -1,10 +1,14 @@
package org.firstinspires.ftc.teamcode.utilsv2; package org.firstinspires.ftc.teamcode.utilsv2;
import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.robotcore.util.Range; import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Robot;
import java.util.List;
@Config @Config
public class Turret { public class Turret {
Robot robot; Robot robot;
@@ -13,16 +17,16 @@ public class Turret {
private final double neutralPosition = 0.3; //TODO: Tune private final double neutralPosition = 0.3; //TODO: Tune
private final double turretMin = 0.04; //TODO: Tune private final double turretMin = 0.04; //TODO: Tune
private final double turretMax = 0.94; //TODO: Tune private final double turretMax = 0.94; //TODO: Tune
private final double hVelK = 0.12; // TODO: Tune private final double hVelK = 0.12; // TODO: Tune
private final double hAccK = 0.02; // TODO: Tune private final double hAccK = 0.02; // TODO: Tune
private final double xVelK = 0.10; // TODO: Tune private final double xVelK = 0.10; // TODO: Tune
private final double xAccK = 0.02; // TODO: Tune private final double xAccK = 0.02; // TODO: Tune
private final double yVelK = 0.10; // TODO: Tune private final double yVelK = 0.10; // TODO: Tune
private final double yAccK = 0.02; // TODO: Tune private final double yAccK = 0.02; // TODO: Tune
private int obeliskID = 0;
public Turret(Robot rob) { public Turret(Robot rob) {
this.robot = rob; this.robot = rob;
@@ -34,6 +38,50 @@ public class Turret {
return angle; return angle;
} }
public void trackObelisk(double dx, double dy, double h) {
double heading = wrapAngle(h);
double fieldRelativeHeading = Math.atan2(dy, dx);
double desiredAngle = fieldRelativeHeading - heading;
double angleDelta = desiredAngle - Math.PI;
angleDelta = wrapAngle(angleDelta);
double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180);
double servoAngle = neutralPosition + servoTicksFromNeutral;
servoAngle = Range.clip(servoAngle, turretMin, turretMax);
robot.turr1.setPosition(servoAngle);
robot.turr2.setPosition(1.0 - servoAngle);
detectObelisk();
}
public int getObeliskID() {
return obeliskID;
}
private int detectObelisk() {
robot.limelight.pipelineSwitch(1);
LLResult result = robot.limelight.getLatestResult();
if (result != null && result.isValid()) {
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
double prevTx = -1000;
for (LLResultTypes.FiducialResult fiducial : fiducials) {
double currentTx = fiducial.getTargetXDegrees();
if (currentTx > prevTx){
obeliskID = fiducial.getFiducialId();
}
}
}
return obeliskID;
}
public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) { public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) {
// dx, dy, dz is target - robot // dx, dy, dz is target - robot
// h is the raw heading where 0 degrees is positive x in the system of x, y // h is the raw heading where 0 degrees is positive x in the system of x, y