added redtele, minor tweaks on TurretLock

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

@@ -4,6 +4,7 @@ import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PE
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.fasterxml.jackson.databind.node.POJONode;
 import com.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -15,9 +16,10 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 import org.firstinspires.ftc.robotcore.external.Telemetry;
 import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
 import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
 
 @Config
-@TeleOp
+@TeleOp (name = "TeleOp BLUE")
 public class DriverControlsV1 extends LinearOpMode {
     enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
     Limelight3A limelight;
@@ -37,7 +39,8 @@ public class DriverControlsV1 extends LinearOpMode {
         Boolean shooting = false;
 
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-        drive.setPoseEstimate(new Pose2d(0,0,0));
+        drive.setPoseEstimate(PoseStorage.currentPose);
+        drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
 
         Pose2d driveDirection;
 
@@ -49,7 +52,7 @@ public class DriverControlsV1 extends LinearOpMode {
         PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
 
         drive.setPoseEstimate(new Pose2d(0, 0, 0));
-        TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry);
+        TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE);
 
         waitForStart();
 

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

@@ -0,0 +1,198 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PER_LOOP;
+
+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;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+@Config
+@TeleOp (name = "TeleOp RED")
+public class DriverControlsV1Red extends LinearOpMode {
+    enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
+    Limelight3A limelight;
+    LLResult result = null;
+    public static double visionMult = 1.5;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Hware robot = new Hware();
+        robot.initialize(hardwareMap);
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+
+        double SPEEDCONTROL = 1;
+
+        Boolean adjust = false;
+        Boolean shooting = false;
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        drive.setPoseEstimate(new Pose2d(0,0,0));
+
+        Pose2d driveDirection;
+
+        ShooterSubsystem shooterMap = new ShooterSubsystem();
+
+        ShootState shootStatus = ShootState.IDLE;
+        ElapsedTime shootTimer = new ElapsedTime();
+
+        PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
+
+        drive.setPoseEstimate(new Pose2d(0, 0, 0));
+        TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED);
+
+        waitForStart();
+
+        limelight.pipelineSwitch(1);
+        limelight.start();
+
+        robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
+        robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
+        robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        while (opModeIsActive()) {
+            robot.leftPTO.setPosition(0.2);
+            robot.rightPTO.setPosition(0.2);
+
+            double currentVoltage = robot.batteryVoltageSensor.getVoltage();
+            if (currentVoltage > 13.5) {
+                pidf = new PIDFCoefficients(3, 0, 0, 13.8
+                );
+            } else if (currentVoltage > 12.7) {
+                pidf = new PIDFCoefficients(3, 0, 0, 14);
+            } else if (currentVoltage > 11.2) {
+                pidf = new PIDFCoefficients(3, 0, 0, 14.5 );
+            } else {
+                pidf = new PIDFCoefficients(3, 0, 0, 14 );
+
+            }
+            Pose2d myPose = drive.getPoseEstimate();
+            double turrPos = robot.turret.getCurrentPosition();
+
+            result = limelight.getLatestResult();
+            double vision = result.getTx() * visionMult;
+//            if((result.getTx() < 2 || result.getTy() > -2) && result.isValid()){
+//                drive.setPoseEstimate(new Pose2d(drive.getPoseEstimate().getX(), drive.getPoseEstimate().getY(), turretLock.updateHeading(myPose, result, turrPos)));
+//            }
+            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
+//            telemetry.update();\
+//
+            double targetTicks = turretLock.update(myPose, result, vision, turrPos);
+            robot.turret.setTargetPosition((int) targetTicks);
+            robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+            robot.turret.setPower(1);
+
+            if (gamepad1.right_bumper) {
+                SPEEDCONTROL= 0.5;
+            }
+            if (!shooting) {
+                if (gamepad1.right_trigger > 0.3) {
+                    robot.intake.set(0.8);
+                    robot.activeTransfer.setPosition(0.7);
+                } else if (gamepad1.left_trigger > 0.5) {
+                    robot.intake.set(-0.8);
+                    robot.activeTransfer.setPosition(0.7);
+                } else {
+                    robot.intake.set(0);
+                    robot.activeTransfer.setPosition(0.7);
+                }
+            }
+
+            driveDirection = new Pose2d(
+                    -gamepad1.left_stick_y * SPEEDCONTROL,
+                    -gamepad1.left_stick_x * SPEEDCONTROL,
+                    -gamepad1.right_stick_x * SPEEDCONTROL
+            );
+
+            // Arm
+
+
+            // 2. Get Targets based on current X, Y
+            double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
+            double targetVelocity = targets[0];
+            double targetHood = targets[1];
+
+
+            if (shooting) {
+                robot.launchHood.setPosition(targetHood);
+            }
+
+            if(gamepad2.dpadUpWasPressed()) {
+                adjust = true;
+            } else if (gamepad2.dpadDownWasPressed()){
+                adjust = false;
+            }
+
+            if (adjust) {
+                robot.bottomFly.setVelocity(targetVelocity);
+                robot.topFly.setVelocity(targetVelocity);
+            } else {
+                robot.bottomFly.setVelocity(0);
+                robot.topFly.setVelocity(0);
+            }
+
+
+            // 1. TRIGGER: Start the sequence
+            if (gamepad2.xWasPressed()) {
+                shooting = true;
+                shootStatus = ShootState.START_SHOOT;
+                shootTimer.reset();
+            }
+
+            switch (shootStatus) {
+                case START_SHOOT:
+                    // Initial actions
+                    robot.activeTransfer.setPosition(0.95);
+                    robot.intake.set(0.55);
+                    robot.launchHood.setPosition(targetHood);
+
+                    // Wait 250ms for the shot to physically fire before moving the hood
+                    if (shootTimer.milliseconds() > 100) {
+                        shootStatus = ShootState.LOWER_HOOD;
+                    }
+                    break;
+
+                case LOWER_HOOD:
+                    targetHood = Math.max(0.34, targetHood - 0.2);
+                    targetVelocity = targetVelocity + 100;
+                    robot.launchHood.setPosition(targetHood);
+                    robot.bottomFly.setVelocity(targetVelocity);
+                    robot.topFly.setVelocity(targetVelocity);
+                    shootStatus = ShootState.RESET;
+                    break;
+
+                case RESET:
+                    if (shootTimer.milliseconds() > 3000) {
+                        shootStatus = ShootState.IDLE;
+                        shooting = false;
+                    }
+                    break;
+            }
+
+            if (gamepad2.yWasPressed()) {
+                shooting = false;
+                robot.activeTransfer.setPosition(.7);
+            }
+
+            drive.setWeightedDrivePower(driveDirection);
+            drive.update();
+
+            telemetry.addData("Vision", vision);
+            telemetry.addData("Voltage Sensor", currentVoltage);
+            telemetry.update();
+        }
+
+    }
+}

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

@@ -70,7 +70,7 @@ public class TeleOpTurretLock extends LinearOpMode {
 
         // Start pose (you said 0,0)
         drive.setPoseEstimate(new Pose2d(0, 0, 0));
-        TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry);
+        TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.BLUE);
 
 
         // limelight

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

@@ -19,7 +19,7 @@ import java.util.List;
 
 public class TurretLock {
 
-    public enum MODE{
+    public enum MODE {
         LIME,
         ODOMETRY
     }
@@ -40,7 +40,7 @@ public class TurretLock {
 
     // Mechanical limits (ticks). Keep your existing numbers.
     public int minTicks = (int) (ticksPerDegree * (-90));
-    public int maxTicks = (int) (ticksPerDegree * ( 90 ));
+    public int maxTicks = (int) (ticksPerDegree * (90));
 
     public double finalDeg = 0;
 
@@ -50,6 +50,11 @@ public class TurretLock {
     public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
     public static double cameraBearingEqual = 0.5;      // Deadband
 
+    public enum SIDE {
+        RED,
+        BLUE
+    }
+
     // TODO: tune these values for limelight
     Hware robot;
     Limelight3A webcam;
@@ -76,25 +81,35 @@ public class TurretLock {
 
     Telemetry tele;
 
-   // private final ElapsedTime tagTimer = new ElapsedTime();
+    // private final ElapsedTime tagTimer = new ElapsedTime();
 
-    public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele) {
+    public TurretLock(DcMotorEx turretMotor, Limelight3A cam, SIDE side) {
         this.turret = turretMotor;
         this.webcam = cam;
-        this.tele = tele;
+        switch (side) {
+            case RED:
+                goalX = -75;
+                goalY = -75;
+                break;
+            case BLUE:
+                goalX = 75;
+                goalY = 75;
+                break;
+        }
     }
 
     public double getTurrPos() {
         return turret.getCurrentPosition();
     }
 
-    public void addManualTrimDeg(double deltaDeg){
+    public void addManualTrimDeg(double deltaDeg) {
         manualTrimDeg += deltaDeg;
     }
 
-    public void resetManualTrim(){
+    public void resetManualTrim() {
         manualTrimDeg = 0;
     }
+
     private void limelightRead(LLResult result) { // only for tracking purposes, not general reads
         if (result != null) {
             if (result.isValid()) {
@@ -112,7 +127,7 @@ public class TurretLock {
     }
 
     public double getBearing() {
-     //   tx = 1000;
+        //   tx = 1000;
         LLResult result = webcam.getLatestResult();
         return result.getBotpose().getOrientation().getYaw();
     }
@@ -151,23 +166,25 @@ public class TurretLock {
 
     private double bearingAlign(LLResult llResult) {
         double targetTx = 0;
-        if(llResult != null && llResult.isValid()){
+        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(){
+    public void Init() {
         pid = new PersonalPID(p, i, d, f);
     }
 
-    public String getStatus(){
+    public String getStatus() {
         return String.valueOf(webcam.getStatus());
     }
 
 
-    /** Main update: call every loop. */
+    /**
+     * Main update: call every loop.
+     */
     public double update(Pose2d robotPose, LLResult result, double visionCorrect, double turrPos) {
         // 1) ODOM: compute turret angle needed to face goal (in degrees)
         double dx = goalX - robotPose.getX();
@@ -191,7 +208,7 @@ public class TurretLock {
         finalDeg = manualTrimDeg + turretCmdDeg;
         limelightRead(result);
         filteredVisionTrimDeg = -visionCorrect;
-        switch (TYPE){
+        switch (TYPE) {
             case LIME:
                 finalDeg = manualTrimDeg + turrPos / ticksPerDegree + filteredVisionTrimDeg;
                 if (result != null && (finalDeg > 20 || finalDeg < -20)) {
@@ -221,13 +238,13 @@ public class TurretLock {
 //        turret.setTargetPosition(targetTicks);
 //        turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
 //        turret.setPower(1.0);
-      }
+    }
 
     public double updateHeading(Pose2d robotPose, LLResult result, double turrPos) {
         double robotHeadingDeg = Math.toDegrees(robotPose.getHeading());
 
-        if(result.getTx() < 1 || result.getTy() > -1){
+        if (result.getTx() < 1 || result.getTy() > -1) {
-            robotHeadingDeg = - (turrPos / ticksPerDegree - 45);
+            robotHeadingDeg = -(turrPos / ticksPerDegree - 45);
         }
 
         return robotHeadingDeg;
@@ -237,11 +254,13 @@ public class TurretLock {
         return TYPE;
     }
 
-    public double getFinalDeg(){
+    public double getFinalDeg() {
         return finalDeg;
     }
 
-    /** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */
+    /**
+     * Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag.
+     */
 
     private static double normalizeDeg(double deg) {
         while (deg > 180) deg -= 360;