2/11 changes

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java

@@ -3,39 +3,50 @@ package org.firstinspires.ftc.teamcode;
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
 
 @Config
-@TeleOp(name="FlyOpTuner", group="Test")
+@TeleOp
 public class FlyOp extends LinearOpMode {
 
     private Hware robot;
 
     // Dashboard-editable
-    public static double TARGET = 1500.0;   // ticks/sec (flip sign if needed)
+    public static double TARGET = 1500;   // ticks/sec (flip sign if needed)
-    public static double launchPos = 1.0;   // servo position (0..1 usually)
+    public static double launchPos = 0.7;   // servo position (0..1 usually)
+    public static double intake = 0.6;
 
     @Override
     public void runOpMode() throws InterruptedException {
 
         robot = new Hware();
         robot.initialize(hardwareMap);
+        robot.activeTransfer.setPosition(0.7);
+
+        robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         waitForStart();
 
         while (opModeIsActive() && !isStopRequested()) {
 
+
             robot.bottomFly.setVelocity(TARGET);
             robot.topFly.setVelocity(TARGET);
             robot.launchHood.setPosition(launchPos);
+            robot.intake.set(intake);
+
+            robot.turret.setPower(0);
+
 
             if (gamepad1.dpadUpWasPressed()) {
-                robot.intake.set(.8);
+                robot.activeTransfer.setPosition(.95);
             }
 
             if (gamepad1.dpadDownWasPressed()) {
-                robot.intake.set(0);
+                robot.activeTransfer.setPosition(.7);
             }
 
             telemetry.addData("Target", (int)Math.round(TARGET));

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.OUTDATED;
 
 import com.arcrobotics.ftclib.hardware.SimpleServo;
 import com.arcrobotics.ftclib.hardware.motors.Motor;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -16,12 +17,16 @@ public class Hware {
     public Motor frontRight, frontLeft, backRight, backLeft, intake;
     public DcMotorEx turret, bottomFly, topFly;
     public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
+    public Limelight3A limelight;
     WebcamName webcam;
     public void initialize(HardwareMap hwMap) {
         hardwareMap = hwMap;
 
+
         /** MOTORS **/
 
+
+
 //         Chassis Motors
         frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435);
         frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435);
@@ -58,10 +63,14 @@ public class Hware {
         backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
         intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
 
-        PIDFCoefficients pidf = new PIDFCoefficients(0.45, 0, 0, 16.3);
+        PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14
+        );
         bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
         topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
 
         bottomFly.setDirection(DcMotorSimple.Direction.REVERSE);
+
+        //Vision
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java

@@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -22,8 +24,16 @@ public class TeleOpTurretLock extends LinearOpMode {
 
     // Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system.
     // You said your start is (0,0). So measure these from that same origin.
-    public static double GOAL_X = 64.0;
+    public static double GOAL_X = 75;
-    public static double GOAL_Y = 75.0;
+    public static double GOAL_Y = 75;
+
+    public static double p = 1, i = 0, d = 0, f = 1;
+
+
+    public static double ticksperdegree = 9.738888 * 0.29969;
+
+    Limelight3A limelight;
+
 
     // Set to a specific tag ID if you want (ex: 5). Use -1 to lock to any visible tag.
     public static int TARGET_TAG_ID = -1;
@@ -35,13 +45,10 @@ public class TeleOpTurretLock extends LinearOpMode {
     public static double DRIVE_SPEED = 1.0;
     public static double SLOW_SPEED = 0.45;
 
-    // If your webcam name in RC config is different, change this
+    LLResult result = null;
-    public static String WEBCAM_NAME = "Webcam 1";
     
     /* ----------------------------------------------------- */
 
-    private VisionPortal visionPortal;
-    private AprilTagProcessor aprilTag;
 
     @Override
     public void runOpMode() {
@@ -49,25 +56,17 @@ public class TeleOpTurretLock extends LinearOpMode {
         // Hardware
         Hware robot = new Hware();
         robot.initialize(hardwareMap);
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
 
         // Drive
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
 
         // Start pose (you said 0,0)
         drive.setPoseEstimate(new Pose2d(0, 0, 0));
+        TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry, p, i, d, f);
 
-        // AprilTag
+        // limelight
-        aprilTag = AprilTagProcessor.easyCreateWithDefaults();
-        visionPortal = new VisionPortal.Builder()
-                .setCamera(hardwareMap.get(WebcamName.class, WEBCAM_NAME))
-                .addProcessor(aprilTag)
-                .build();
         
-        // TurretLock subsystem
-        TurretLock turretLock = new TurretLock(robot.turret);
-        turretLock.goalX = GOAL_X;
-        turretLock.goalY = GOAL_Y;
-        turretLock.targetTagId = TARGET_TAG_ID;
 
         // Optional: if you want these controlled from dashboard too, you can,
         // but you already have them inside TurretLock.
@@ -80,8 +79,14 @@ public class TeleOpTurretLock extends LinearOpMode {
 
         waitForStart();
 
+//        turretLock.cameraInit();
+        limelight.pipelineSwitch(1);
+        limelight.start();
+
         while (opModeIsActive()) {
 
+            result = robot.limelight.getLatestResult();
+
             /* ---------------- DRIVE ---------------- */
             double speed = DRIVE_SPEED;
             if (gamepad1.left_bumper || gamepad1.right_bumper) speed = SLOW_SPEED;
@@ -95,49 +100,37 @@ public class TeleOpTurretLock extends LinearOpMode {
             drive.setWeightedDrivePower(drivePower);
             drive.update();
 
-            Pose2d pose = drive.getPoseEstimate();
 
-            /* ---------------- TOGGLE AUTOLOCK ---------------- */
+            Pose2d pose = drive.getPoseEstimate();
-            boolean xNow = gamepad2.x;
-            if (xNow && !xPrev) autoLockEnabled = !autoLockEnabled;
-            xPrev = xNow;
 
             /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
             turretLock.goalX = GOAL_X;
             turretLock.goalY = GOAL_Y;
-            turretLock.targetTagId = TARGET_TAG_ID;
 
             /* ---------------- MANUAL TRIM ---------------- */
             // Left trigger = +trim, Right trigger = -trim (same style as your old code)
             double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger);
             if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg);
-
             if (gamepad2.y) turretLock.resetManualTrim();  // reset driver trim
-            if (gamepad2.b) turretLock.resetVisionTrim();  // reset vision trim
+//            telemetry.update();\
+//
+            turretLock.update(pose, result);
 
-            /* ---------------- AUTOLOCK UPDATE ---------------- */
+            telemetry.addData("VisionAngle", turretLock.getBearing());
-            List<AprilTagDetection> detections = aprilTag.getDetections();
+            telemetry.addData("Angle", turretLock.getFinalDeg());
+            telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543));
+            telemetry.addData("MODE", turretLock.getTYPE());
+            telemetry.addData("Tag ID", turretLock.getObeliskID());
+            telemetry.addData("Does Lime have Result", result.isValid());
 
-            if (autoLockEnabled) {
+            telemetry.addData("Botpose", result.getBotpose());
-                turretLock.update(pose, detections);
+            telemetry.addData("Yaw", result.getTx());
-            } else {
+
-                // If autolock is off, just stop turret motor where it is
+            telemetry.addData("Status", turretLock.getStatus());
-                // (or you can put your own manual turret code here)
+
-                robot.turret.setPower(0);
+            telemetry.addData("LimelightInfo", robot.limelight.getStatus());
-            }
 
-            /* ---------------- TELEMETRY ---------------- */
-            telemetry.addData("AutoLock", autoLockEnabled);
-            telemetry.addData("Pose", "x=%.1f y=%.1f h=%.1fdeg",
-                    pose.getX(), pose.getY(), Math.toDegrees(pose.getHeading()));
-            telemetry.addData("Goal", "x=%.1f y=%.1f", GOAL_X, GOAL_Y);
-            telemetry.addData("TargetTag", TARGET_TAG_ID);
-            telemetry.addData("TagCount", (detections == null) ? 0 : detections.size());
-            telemetry.addData("TurretTicks", robot.turret.getCurrentPosition());
-            telemetry.addLine("Controls: GP2 X=toggle, triggers=trim, Y=reset trim, B=reset vision");
             telemetry.update();
         }
-
-        if (visionPortal != null) visionPortal.close();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java

@@ -1,50 +1,65 @@
 package org.firstinspires.ftc.teamcode;
 
+import static java.lang.Math.abs;
+
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
 import java.util.List;
 
 public class TurretLock {
 
+    public enum MODE{
+        LIME,
+        ODOMETRY
+    }
+
+    public MODE TYPE = MODE.ODOMETRY;
     /* ------------------- TUNABLES ------------------- */
 
     // Field target (in RoadRunner field units; usually inches)
     // Set this to the AprilTag/backdrop location you want to aim at.
-    public double goalX = 64.0;
+    public double goalX = 75;
-    public double goalY = 75.0;
+    public double goalY = 75;
+
+    PersonalPID pid;
 
     // Turret conversion
     // If you already trust your TICKS_PER_DEGREE, keep it.
-    public double ticksPerDegree = 9.738888;
+    public double ticksPerDegree = 2.543; //9.738888 * 0.29969;
 
     // Mechanical limits (ticks). Keep your existing numbers.
     public int minTicks = (int) (ticksPerDegree * (-90));
     public int maxTicks = (int) (ticksPerDegree * ( 90 ));
 
-    // Which AprilTag to lock to (set -1 to accept any visible tag)
+    public double finalDeg = 0;
-    public int targetTagId = -1;
 
-    // Vision correction: degrees of turret change per degree of tag "bearing"/yaw error.
+    public static boolean limelightUsed = true;
-    // Start small (0.6–1.2) and tune.
-    public double visionDegPerDegError = 0.9;
 
-    // Deadband so it doesn’t twitch
+    public static double visionCorrectionGain = 0.08;  // Single tunable gain
-    public double visionDeadbandDeg = 0.6;
+    public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
+    public static double cameraBearingEqual = 0.5;      // Deadband
 
-    // How hard we trust vision vs odom.
+    // TODO: tune these values for limelight
-    // 0 = ignore vision, 1 = full correction.
+    Hware robot;
-    public double visionBlend = 1.0;
+    Limelight3A webcam;
+    double tx = 0.0;
+    double ty = 0.0;
+    double limelightPosX = 0.0;
+    double limelightPosY = 0.0;
+    private int obeliskID = 0;
 
-    // Smooth the vision correction so it doesn’t jump
+    double limelightOffset;
-    // 0.0 = no smoothing, 0.8 = heavy smoothing
-    public double visionLowPass = 0.65;
-
-    // If we haven't seen a tag in this many seconds, slowly decay vision trim back to 0
-    public double trimDecayPerSec = 12.0; // deg/sec back toward 0
 
     // Optional: flip direction if your turret sign is backwards
     public boolean invertTurret = false;
@@ -56,29 +71,108 @@ public class TurretLock {
     private double visionTrimDeg = 0.0;      // learned trim from camera
     private double filteredVisionTrimDeg = 0.0;
 
-    private final ElapsedTime tagTimer = new ElapsedTime();
+    public double p, i, d, f;
 
-    public TurretLock(DcMotorEx turretMotor) {
+
+    Telemetry tele;
+
+   // private final ElapsedTime tagTimer = new ElapsedTime();
+
+    public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele, double p, double i, double d, double f) {
         this.turret = turretMotor;
-        tagTimer.reset();
+        this.webcam = cam;
+        this.tele = tele;
+        this.p = p;
+        this.i = i;
+        this.d = d;
+        this.f = f;
+    }
+
+    public double getTurrPos() {
+        return turret.getCurrentPosition();
     }
 
-    /** Call this if you want driver trim like your triggers (degrees). */
     public void addManualTrimDeg(double deltaDeg){
         manualTrimDeg += deltaDeg;
     }
 
     public void resetManualTrim(){
-        manualTrimDeg = 0.0;
+        manualTrimDeg = 0;
+    }
+    private void limelightRead(LLResult result) { // only for tracking purposes, not general reads
+        if (result != null) {
+            if (result.isValid()) {
+                tx = result.getTx();
+                ty = result.getTy();
+                // MegaTag1 code for receiving position
+                Pose3D botpose = result.getBotpose();
+                if (botpose != null) {
+                    limelightPosX = botpose.getPosition().x;
+                    limelightPosY = botpose.getPosition().y;
                 }
 
-    public void resetVisionTrim() {
-        visionTrimDeg = 0.0;
-        filteredVisionTrimDeg = 0.0;
             }
+        }
+    }
+
+    public double getBearing() {
+     //   tx = 1000;
+        LLResult result = webcam.getLatestResult();
+        return result.getBotpose().getOrientation().getYaw();
+    }
+
+    public double getTy() {
+        return ty;
+    }
+
+    public double getLimelightX() {
+        return limelightPosX;
+    }
+
+    public double getLimelightY() {
+        return limelightPosY;
+    }
+
+    public int detectObelisk() {
+        com.qualcomm.hardware.limelightvision.LLResult result = webcam.getLatestResult();
+        if (result != null && result.isValid()) {
+            List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
+            for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                obeliskID = fiducial.getFiducialId();
+            }
+        }
+        return obeliskID;
+    }
+
+    public int getObeliskID() {
+        return obeliskID;
+    }
+
+
+    /*
+        Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
+     */
+
+    private double bearingAlign(LLResult llResult) {
+        double targetTx = 0;
+        if(llResult != null && llResult.isValid()){
+            targetTx = llResult.getTx();   // How far left or right the target is (degrees)
+
+        }
+        return llResult.getBotpose().getOrientation().getYaw();
+    }
+
+    public void Init(){
+        pid = new PersonalPID(p, i, d, f);
+    }
+
+    public String getStatus(){
+        return String.valueOf(webcam.getStatus());
+    }
+
 
     /** Main update: call every loop. */
-    public void update(Pose2d robotPose, List<AprilTagDetection> detections) {
+    public void update(Pose2d robotPose, LLResult result) {
         // 1) ODOM: compute turret angle needed to face goal (in degrees)
         double dx = goalX - robotPose.getX();
         double dy = goalY - robotPose.getY();
@@ -98,33 +192,22 @@ public class TurretLock {
         if (invertTurret) turretCmdDeg = -turretCmdDeg;
 
         // 2) VISION: find tag error and update vision trim
-        Double tagErrorDeg = getTagErrorDeg(detections);
+        limelightRead(result);
-        if (tagErrorDeg != null) {
+        filteredVisionTrimDeg = result.getTx();
-            // deadband
+        switch (TYPE){
-            if (Math.abs(tagErrorDeg) < visionDeadbandDeg) tagErrorDeg = 0.0;
+            case LIME:
-
+                finalDeg = turretCmdDeg + manualTrimDeg - filteredVisionTrimDeg;
-            // Convert tag error into turret trim change.
+                if (!result.isValid()) {
-            // Sign: if tag is to the right (+error), turret should move right or left?
+                    TYPE = MODE.ODOMETRY;
-            // If it goes the wrong way, flip the sign here (multiply by -1).
+                }
-            double deltaTrim = (tagErrorDeg * visionDegPerDegError) * visionBlend;
+                break;
-
+            case ODOMETRY:
-            // We want trim to counteract the observed error, so subtract.
+                finalDeg = turretCmdDeg + manualTrimDeg;
-            visionTrimDeg -= deltaTrim;
+                if (finalDeg < 12 || finalDeg > -12) {
-
+                    TYPE = MODE.LIME;
-            tagTimer.reset();
+                }
-        } else {
+                break;
-            // no tag: decay vision trim toward 0 over time
-            double dt = 0.02; // assume ~50Hz; fine for a simple decay
-            double decay = trimDecayPerSec * dt;
-            if (visionTrimDeg > 0) visionTrimDeg = Math.max(0, visionTrimDeg - decay);
-            else if (visionTrimDeg < 0) visionTrimDeg = Math.min(0, visionTrimDeg + decay);
         }
-
-        // Low-pass filter the trim so it doesn’t jerk
-        filteredVisionTrimDeg = filteredVisionTrimDeg * visionLowPass + visionTrimDeg * (1.0 - visionLowPass);
-
-        // 3) Combine
-        double finalDeg = turretCmdDeg + manualTrimDeg + filteredVisionTrimDeg;
 
         // Optional: keep within your allowed lock range (like ±90)
         finalDeg = clamp(finalDeg, -90, 90);
@@ -133,42 +216,23 @@ public class TurretLock {
         int targetTicks = (int) Math.round(finalDeg * ticksPerDegree);
         targetTicks = clamp(targetTicks, minTicks, maxTicks);
 
+//        double pow = pid.calculate(targetTicks);
+//        turret.setPower(pow);
+
         turret.setTargetPosition(targetTicks);
         turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
         turret.setPower(1.0);
       }
 
+    public MODE getTYPE() {
+        return TYPE;
+    }
+
+    public double getFinalDeg(){
+        return finalDeg;
+    }
+
     /** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */
-    private Double getTagErrorDeg(List<AprilTagDetection> detections) {
-        if (detections == null || detections.isEmpty()) return null;
-
-        AprilTagDetection best = null;
-
-        for (AprilTagDetection d : detections) {
-            if (d == null || d.metadata == null) continue; // metadata can be null on some setups
-            if (targetTagId != -1 && d.id != targetTagId) continue;
-
-            // pick the first good one (or you can pick closest by range if you want)
-            best = d;
-            break;
-        }
-
-        if (best == null) return null;
-
-        // FTC AprilTagDetection typically gives:
-        // best.ftcPose.yaw  (degrees)  // yaw ~ left-right rotation of tag relative to camera
-        // best.ftcPose.bearing (degrees) on some versions
-        //
-        // We'll prefer "bearing" if present; otherwise use yaw.
-        try {
-            // If your SDK has bearing:
-            // return best.ftcPose.bearing;
-            // Otherwise:
-            return best.ftcPose.yaw;
-        } catch (Exception e) {
-            return null;
-        }
-    }
 
     private static double normalizeDeg(double deg) {
         while (deg > 180) deg -= 360;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java

@@ -53,8 +53,8 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer
         lastEncPositions = lastTrackingEncPositions;
         lastEncVels = lastTrackingEncVels;
 
-        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
+        leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake"));
-        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake"));
+        rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
         frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight"));
 
         // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)