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

need to add zero code to properly test

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-24 14:45:35 -06:00
parent 5922f4e935
commit 8d29a80696
3 changed files with 60 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java
View File

@@ -19,6 +19,8 @@ public class LimelightTest extends LinearOpMode {
Robot robot;
public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
public static boolean turretMode = false;
public static double turretPos = 0.501;
@Override
public void runOpMode() throws InterruptedException {
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -55,6 +57,13 @@ public class LimelightTest extends LinearOpMode {
} else {
robot.limelight.pipelineSwitch(0);
}
if (turretMode){
if (turretPos != 0.501){
turret.manualSetTurret(turretPos);
}
}
}
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
View File

@@ -14,6 +14,7 @@ import org.firstinspires.ftc.teamcode.utils.Turret;
@Autonomous
@Config
public class TurretTest extends LinearOpMode {
public static boolean zeroTurr = false;
@Override
public void runOpMode() throws InterruptedException {
@@ -25,7 +26,6 @@ public class TurretTest extends LinearOpMode {
Turret turret = new Turret(robot, TELE, robot.limelight);
waitForStart();
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0));
while(opModeIsActive()){
@@ -33,6 +33,16 @@ public class TurretTest extends LinearOpMode {
drive.updatePoseEstimate();
turret.trackGoal(drive.localizer.getPose());
TELE.addData("tpos", turret.getTurrPos());
TELE.addData("Limelight tx", turret.getBearing());
TELE.addData("Limelight ty", turret.getTy());
TELE.addData("Limelight X", turret.getLimelightX());
TELE.addData("Limelight Y", turret.getLimelightY());
if(zeroTurr){
turret.zeroTurretEncoder();
}
TELE.update();
}

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

@@ -8,6 +8,7 @@ import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
@@ -17,38 +18,45 @@ import java.util.List;
public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 1.009;
public static double turrPosScalar = 0.00011264432;
public static double turret180Range = 0.4;
public static double turrDefault = 0.4;
public static double cameraBearingEqual = 1;
public static double errorLearningRate = 0.15;
public static double turrMin = 0.2;
public static double turrMax = 0.8;
public static double deltaAngleThreshold = 0.02;
public static double angleMultiplier = 0.0;
public static double mult = 0.0;
private boolean lockOffset = false;
Robot robot;
MultipleTelemetry TELE;
Limelight3A webcam;
private int obeliskID = 0;
private double turrPos = 0.0;
private double offset = 0.0;
private double bearing = 0.0;
double tx = 0.0;
double ty = 0.0;
double limelightPosX = 0.0;
double limelightPosY = 0.0;
public static double clampTolerance = 0.03;
public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
this.TELE = tele;
this.robot = rob;
this.webcam = cam;
webcam.start();
if (redAlliance){
webcam.pipelineSwitch(3);
} else {
webcam.pipelineSwitch(2);
}
}
public void zeroTurretEncoder() {
robot.intake.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.intake.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public double getTurrPos() {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3);
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3) + turrDefault;
}
@@ -121,11 +129,18 @@ public class Turret {
return obeliskID;
}
public void zeroOffset() {
offset = 0.0;
}
public void lockOffset(boolean lock) {
lockOffset = lock;
}
/*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
public void trackGoal(Pose2d deltaPos) {
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */
// Angle from robot to goal in robot frame
@@ -145,24 +160,33 @@ public class Turret {
while (turretAngleDeg > 180) turretAngleDeg -= 360;
while (turretAngleDeg < -180) turretAngleDeg += 360;
/* ---------------- APRILTAG CORRECTION ---------------- */
//
double tagBearingDeg = getBearing(); // + = target is to the left
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual) {
// Slowly learn turret offset (persistent calibration)
offset -= tagBearingDeg * errorLearningRate;
}
turretAngleDeg += offset;
/* ---------------- ANGLE → SERVO ---------------- */
double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
// Clamp to servo range
double currentEncoderPos = this.getTurrPos();
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 {
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual && !lockOffset) {
// Slowly learn turret offset (persistent calibration)
offset -= tagBearingDeg * errorLearningRate;
}
}
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1.0 - turretPos);