diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index c8e698d..1ef325a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -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); + } + } + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 781f1a6..8823522 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -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(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 75cf6d1..3ddb14a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -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,16 +160,10 @@ 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 ---------------- */ @@ -162,7 +171,22 @@ public class Turret { double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360); // Clamp to servo range - turretPos = Math.max(turrMin, Math.min(turretPos, turrMax)); + 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);