started updating the auto

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V3.java

@@ -17,16 +17,18 @@ import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.acmerobotics.roadrunner.ftc.Actions;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
-import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
+import org.firstinspires.ftc.teamcode.utils.FlywheelV2;
-import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.Servos;
-import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+
+import java.util.List;
 
 @Config
 @Autonomous(preselectTeleOp = "TeleopV3")
@@ -34,7 +36,7 @@ public class Red_V3 extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
     MecanumDrive drive;
-    Flywheel flywheel;
+    FlywheelV2 flywheel;
     Servos servo;
 
     double velo = 0.0;
@@ -49,38 +51,20 @@ public class Red_V3 extends LinearOpMode {
     int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
     int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
     int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
-    
+
     public Action initShooter(int vel) {
         return new Action() {
-            double ticker = 0.0;
-            double stamp = 0.0;
-            boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (ticker == 0) {
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                    stamp = getRuntime();
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                }
-                ticker++;
-                
-                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
-                velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
                 robot.transfer.setPower(1);
 
                 TELE.addData("Velocity", velo);
                 TELE.update();
-                if (vel < velo && getRuntime() - stamp > 3.0 && !steady){
+
-                    steady = true;
+                return !flywheel.getSteady();
-                    stamp = getRuntime();
-                    return true;
-                } else if (steady && getRuntime() - stamp > 1.5){
-                    TELE.addData("Velocity", velo);
-                    TELE.addLine("finished init");
-                    TELE.update();
-                    return false;
-                } else {
-                    return true;
-                }
             }
         };
     }
@@ -90,8 +74,8 @@ public class Red_V3 extends LinearOpMode {
             double stamp = 0.0;
             boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                velo = flywheel.getVelo();
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 steady = flywheel.getSteady();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
@@ -101,10 +85,9 @@ public class Red_V3 extends LinearOpMode {
                 TELE.update();
                 detectTag();
 
-                if (last && !steady){
+                if (last && !steady) {
                     stamp = getRuntime();
                     drive.updatePoseEstimate();
-
                     teleStart = drive.localizer.getPose();
                     return false;
                 } else if (steady) {
@@ -119,24 +102,27 @@ public class Red_V3 extends LinearOpMode {
 
     public Action Obelisk() {
         return new Action() {
-            double stamp = getRuntime();
+            int id = 0;
-            int ticker = 0;
-
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (ticker == 0) {
+                LLResult result = robot.limelight.getLatestResult();
-                    stamp = getRuntime();
+                if (result != null && result.isValid()) {
-                }
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
-                ticker++;
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
+                        id = fiducial.getFiducialId();
+                        TELE.addData("ID", id);
+                        TELE.update();
+                    }
 
-                if (aprilTag.getTagById(21) != null) {
+                }
+
+                if (id == 21){
                     gpp = true;
-                } else if (aprilTag.getTagById(22) != null) {
+                } else if (id == 22){
                     pgp = true;
-                } else if (aprilTag.getTagById(23) != null) {
+                } else if (id == 23){
                     ppg = true;
                 }
-                aprilTag.update();
 
                 TELE.addData("Velocity", velo);
                 TELE.addData("21", gpp);
@@ -144,10 +130,11 @@ public class Red_V3 extends LinearOpMode {
                 TELE.addData("23", ppg);
                 TELE.update();
 
-                if (gpp || pgp || ppg){
+                if (gpp || pgp || ppg) {
                     double turretPID = servo.setTurrPos(turret_red);
                     robot.turr1.setPower(turretPID);
                     robot.turr2.setPower(-turretPID);
+                    robot.limelight.pipelineSwitch(3);
                     return !servo.turretEqual(turret_red);
                 } else {
                     return true;
@@ -156,23 +143,24 @@ public class Red_V3 extends LinearOpMode {
         };
     }
 
-    public Action spindex (double spindexer, int vel){
+    public Action spindex(double spindexer, int vel) {
         return new Action() {
+            double spinPID = 0.0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
 
-                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                velo = flywheel.getVelo();
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
-                robot.spin1.setPower(spindexer);
+                spinPID = servo.setSpinPos(spindexer);
-                robot.spin2.setPower(1-spindexer);
+                robot.spin1.setPower(spinPID);
+                robot.spin2.setPower(-spinPID);
                 TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
 
                 drive.updatePoseEstimate();
-
                 teleStart = drive.localizer.getPose();
 
                 return !servo.spinEqual(spindexer);
@@ -185,14 +173,15 @@ public class Red_V3 extends LinearOpMode {
             double transferStamp = 0.0;
             int ticker = 1;
             boolean transferIn = false;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 TELE.addData("Velocity", velo);
                 TELE.addLine("shooting");
                 TELE.update();
 
-                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition());
+                powPID = flywheel.manageFlywheel(vel, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-                velo = flywheel.getVelo();
+                velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
@@ -212,7 +201,8 @@ public class Red_V3 extends LinearOpMode {
                     TELE.update();
                     transferIn = true;
                     return true;
-                } else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){
+                } else if (getRuntime() - transferStamp > waitTransfer + waitTransferOut &&
+                           transferIn) {
                     robot.transferServo.setPosition(transferServo_out);
                     TELE.addData("Velocity", velo);
                     TELE.addLine("shot once");
@@ -273,6 +263,7 @@ public class Red_V3 extends LinearOpMode {
         return new Action() {
             double stamp = 0.0;
             int ticker = 0;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
@@ -280,9 +271,9 @@ public class Red_V3 extends LinearOpMode {
                 }
                 ticker++;
 
-                if (getRuntime() - stamp < 0.3){
+                if (getRuntime() - stamp < 0.3) {
                     return true;
-                }else {
+                } else {
                     robot.intake.setPower(0);
                     return false;
                 }
@@ -295,6 +286,7 @@ public class Red_V3 extends LinearOpMode {
             double stamp = 0.0;
             int ticker = 0;
             double position = 0.0;
+
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
                 if (ticker == 0) {
@@ -326,8 +318,6 @@ public class Red_V3 extends LinearOpMode {
 
                     double gP = green / (green + red + blue);
 
-
-
                     if (gP >= 0.4) {
                         b1 = 2;
                     } else {
@@ -385,7 +375,7 @@ public class Red_V3 extends LinearOpMode {
 
         robot = new Robot(hardwareMap);
 
-        flywheel = new Flywheel();
+        flywheel = new FlywheelV2();
 
         TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -395,7 +385,8 @@ public class Red_V3 extends LinearOpMode {
                 0, 0, 0
         ));
 
-        aprilTag = new AprilTagWebcam();
+        robot.limelight.pipelineSwitch(1);
+        robot.limelight.start();
 
         TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                 .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
@@ -416,12 +407,10 @@ public class Red_V3 extends LinearOpMode {
         TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(rx3b, ry3b, rh3b))
                 .strafeToLinearHeading(new Vector2d(rx1, ry1), rh1);
 
-        aprilTag.init(robot, TELE);
-
         while (opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()) {
-                hoodAuto-= 0.01;
+                hoodAuto -= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()) {
@@ -435,13 +424,11 @@ public class Red_V3 extends LinearOpMode {
             robot.spin1.setPower(spindexer_intakePos1);
             robot.spin2.setPower(1 - spindexer_intakePos1);
 
-            aprilTag.update();
             TELE.addData("Velocity", velo);
             TELE.addData("Turret Pos", servo.getTurrPos());
             TELE.update();
         }
 
-
         waitForStart();
 
         if (isStopRequested()) return;
@@ -461,8 +448,8 @@ public class Red_V3 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-            velo = flywheel.getVelo();
+            velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
@@ -497,8 +484,8 @@ public class Red_V3 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-            velo = flywheel.getVelo();
+            velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
@@ -527,8 +514,8 @@ public class Red_V3 extends LinearOpMode {
                     )
             );
 
-            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition());
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
-            velo = flywheel.getVelo();
+            velo = flywheel.getVelo(robot.shooter1.getCurrentPosition(), robot.shooter2.getCurrentPosition());
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
@@ -547,19 +534,13 @@ public class Red_V3 extends LinearOpMode {
         }
 
     }
-
+    //TODO: adjust this
-    public void detectTag(){
+    public void detectTag() {
-        AprilTagDetection d20 = aprilTag.getTagById(20);
+        LLResult result = robot.limelight.getLatestResult();
-        AprilTagDetection d24 = aprilTag.getTagById(24);
+        if (result != null) {
-
+            if (result.isValid()) {
-        if (d20 != null) {
+                bearing = result.getTx();
-            bearing = d20.ftcPose.bearing;
+            }
-            TELE.addData("Bear", bearing);
-        }
-
-        if (d24 != null) {
-            bearing = d24.ftcPose.bearing;
-            TELE.addData("Bear", bearing);
         }
         double turretPos = servo.getTurrPos() - (bearing / 1300);
         double turretPID = servo.setTurrPos(turretPos);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/FlywheelV2.java

@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.utils;
+
+import com.acmerobotics.dashboard.config.Config;
+
+@Config
+public class FlywheelV2 {
+    public static double kP = 0.001;           // small proportional gain (tune this)
+    public static double maxStep = 0.06;         // prevents sudden jumps
+    double initPos1 = 0.0;
+    double initPos2 = 0.0;
+    double stamp = 0.0;
+    double stamp1 = 0.0;
+    double ticker = 0.0;
+    double currentPos1 = 0.0;
+    double currentPos2 = 0.0;
+    double velo = 0.0;
+    double velo1 = 0.0;
+    double velo1a = 0.0;
+    double velo1b = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
+    double targetVelocity = 0.0;
+    double powPID = 0.0;
+    boolean steady = false;
+
+    public FlywheelV2() {
+        //robot = new Robot(hardwareMap);
+    }
+
+    public double getVelo(double shooter1CurPos, double shooter2CurPos) {
+        ticker++;
+        if (ticker % 2 == 0) {
+            velo5 = velo4;
+            velo4 = velo3;
+            velo3 = velo2;
+            velo2 = velo1;
+
+            currentPos1 = shooter1CurPos / 28;
+            currentPos2 = shooter2CurPos / 28;
+            stamp = getTimeSeconds(); //getRuntime();
+            velo1a = 60 * ((currentPos1 - initPos1) / (stamp - stamp1));
+            velo1b = 60 * ((currentPos2 - initPos2) / (stamp - stamp1));
+            initPos1 = currentPos1;
+            initPos2 = currentPos2;
+            stamp1 = stamp;
+
+            if (velo1a < 200){
+                velo1 = velo1b;
+            } else if (velo1b < 200){
+                velo1 = velo1a;
+            } else {
+                velo1 = (velo1a + velo1b) / 2;
+            }
+        }
+        return ((velo1 + velo2 + velo3 + velo4 + velo5) / 5);
+    }
+
+    public double getVelo1() { return (velo1a + velo2 + velo3 + velo4 + velo5) / 5; }
+
+    public double getVelo2() { return (velo1b + velo2 + velo3 + velo4 + velo5) / 5; }
+
+    public boolean getSteady() {
+        return steady;
+    }
+
+    private double getTimeSeconds() {
+        return (double) System.currentTimeMillis() / 1000.0;
+    }
+
+    public double manageFlywheel(int commandedVelocity, double shooter1CurPos, double shooter2CurPos) {
+        targetVelocity = commandedVelocity;
+        velo = getVelo(shooter1CurPos, shooter2CurPos);
+        // Flywheel PID code here
+        if (targetVelocity - velo > 500) {
+            powPID = 1.0;
+        } else if (velo - targetVelocity > 500) {
+            powPID = 0.0;
+        } else {
+            double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
+
+            // --- PROPORTIONAL CORRECTION ---
+            double error = targetVelocity - velo;
+            double correction = kP * error;
+
+            // limit how fast power changes (prevents oscillation)
+            correction = Math.max(-maxStep, Math.min(maxStep, correction));
+
+            // --- FINAL MOTOR POWER ---
+            powPID = feed + correction;
+
+            // clamp to allowed range
+            powPID = Math.max(0, Math.min(1, powPID));
+        }
+
+        steady = (Math.abs(targetVelocity - velo) < 100.0);
+
+        return powPID;
+    }
+
+    public void update() {
+    }
+}