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

stash

Browse Source
This commit is contained in:
Keshav Anand
2026-01-26 16:19:44 -06:00
parent d1434fbaa8
commit b08fe5ada5
1 changed files with 96 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,4 +1,3 @@
package org.firstinspires.ftc.teamcode.utils;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
@@ -6,7 +5,6 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
@@ -18,43 +16,47 @@ import java.util.List;
@Config
public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4;
public static double turrDefault = 0.4;
// TODO: tune these values for limelight
// At the top with other static variables:
public static double kP = 0.015; // Proportional gain - tune this first
public static double kI = 0.0005; // Integral gain - add slowly if needed
public static double kD = 0.002; // Derivative gain - helps prevent overshoot
public static double kF = 0.002; // Derivative gain - helps prevent overshoot
public static double maxOffset = 10; // degrees - safety limit
// Add these as instance variables:
private double lastTagBearing = 0.0;
private double offsetIntegral = 0.0;
public static double cameraBearingEqual = 1;
public static double turrMin = 0.2;
public static double turrMax = 0.8;
public static double errorLearningRate = -0.15;
public static double turrMin = 0.15;
public static double turrMax = 0.85;
public static double mult = 0.0;
private boolean lockOffset = false;
public static double staticOffsetRate = -0.15;
public static double deltaAngleThreshold = 0.02;
public static double angleMultiplier = 0.0;
public static double fastSeekThreshold = 10.0; // Switch to medium mode below this
public static double mediumSeekThreshold = 3.0; // Switch to fine mode below this
public static double fastCorrectionGain = 0.75; // Correction gain for large errors
public static double mediumCorrectionGain = 0.4; // Correction gain for medium errors
public static double fineCorrectionGain = 0.1; // Correction gain for small errors
public static double maxOffsetChangePerCycle = 15; // Max offset change per cycle (degrees)
public static double finalInterpolation = 0.5; // Final position interpolation factor
// TODO: tune these values for limelight
public static double clampTolerance = 0.03;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;
private int obeliskID = 0;
private double offset = 0.0;
private PIDFController controller = new PIDFController(kP, kI, kD, kF);
double tx = 0.0;
double ty = 0.0;
double limelightPosX = 0.0;
double limelightPosY = 0.0;
public static double clampTolerance = 0.03;
private boolean lockOffset = false;
private int obeliskID = 0;
private double offset = 0.0;
private double permanentOffset = 0.0;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
@@ -158,9 +160,9 @@ public class Turret {
/*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
public void trackGoal(Pose2d deltaPos) {
controller.setPIDF(kP, kI, kD, kF);
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */
// Angle from robot to goal in robot frame
@@ -173,55 +175,88 @@ public class Turret {
// Turret angle needed relative to robot
double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
turretAngleDeg = -turretAngleDeg;
// Normalize to [-180, 180]
while (turretAngleDeg > 180) turretAngleDeg -= 360;
while (turretAngleDeg < -180) turretAngleDeg += 360;
/* ---------------- APRILTAG CORRECTION ---------------- */
//
double tagBearingDeg = getBearing(); // + = target is to the left
/* ---------------- LIMELIGHT VISION CORRECTION ---------------- */
double tagBearingDeg = getBearing(); // + = target is to the left
boolean hasValidTarget = (tagBearingDeg != 1000.0);
turretAngleDeg += permanentOffset;
// Active correction if we see the target
if (hasValidTarget && !lockOffset) {
double bearingError = Math.abs(tagBearingDeg);
if (bearingError > cameraBearingEqual) {
// Dual-mode correction: fast when far, gentle when close
double correctionGain;
if (bearingError > fastSeekThreshold) {
correctionGain = fastCorrectionGain;
} else if (bearingError > mediumSeekThreshold) {
correctionGain = mediumCorrectionGain;
} else {
correctionGain = fineCorrectionGain;
}
offset = correctionGain*tagBearingDeg;
TELE.addData("offset", offset);
TELE.addData("offsetChange", offsetChange);
TELE.addData("Correction Mode", bearingError > fastSeekThreshold ? "FAST" :
bearingError > mediumSeekThreshold ? "MEDIUM" : "FINE");
} else {
permanentOffset = offset;
}
}
// Apply persistent offset from previous corrections
turretAngleDeg += offset;
/* ---------------- ANGLE → SERVO ---------------- */
double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
/* ---------------- ANGLE → SERVO POSITION ---------------- */
// Clamp to servo range
double currentEncoderPos = this.getTurrPos();
double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
if (!turretEqual(turretPos)) {
double diff = turretPos - currentEncoderPos;
turretPos = turretPos + diff * mult;
}
if (currentEncoderPos < (turrMin + clampTolerance) || currentEncoderPos > (turrMax - clampTolerance)) {
// Clamp to servo range
turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
} else { // TODO: add so it only adds error when standstill
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) {
// PID-based offset correction for faster, smoother tracking
// Proportional: respond to current error
offset = -controller.calculate(tagBearingDeg);
}
// Clamp to physical servo limits
targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));
// Interpolate towards target position
double currentPos = getTurrPos();
double turretPos = targetTurretPos;
if (targetTurretPos == turrMin){
turretPos = turrMin;
} else if (targetTurretPos == turrMax){
turretPos = turrMax;
}
// Set servo positions
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1.0 - turretPos);
/* ---------------- TELEMETRY ---------------- */
TELE.addData("Turret Angle", turretAngleDeg);
TELE.addData("Bearing", tagBearingDeg);
TELE.addData("Offset", offset);
TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg);
TELE.addData("Target Pos", "%.3f", targetTurretPos);
TELE.addData("Current Pos", "%.3f", currentPos);
TELE.addData("Commanded Pos", "%.3f", turretPos);
TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET");
TELE.addData("Learned Offset", "%.2f", offset);
}
}