stash

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

@@ -1,7 +1,9 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Poses.*;
+
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodAuto;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
 import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
 
@@ -11,23 +13,24 @@ import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.ParallelAction;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.ftc.Actions;
 
 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.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
-@Autonomous
+@Autonomous(preselectTeleOp = "TeleopV2")
 public class Blue_V2 extends LinearOpMode {
 
     Robot robot;
@@ -38,9 +41,13 @@ public class Blue_V2 extends LinearOpMode {
 
     AprilTagWebcam aprilTag;
 
-    public static double intake1Time = 4.0;
+    Flywheel flywheel;
 
-    public static double intake2Time = 5.5;
+    double velo = 0.0;
+    double targetVelocity = 0.0;
+    public static double intake1Time = 2.9;
+
+    public static double intake2Time = 2.9;
 
     public static double colorDetect = 3.0;
 
@@ -59,6 +66,7 @@ public class Blue_V2 extends LinearOpMode {
     int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
 
     boolean spindexPosEqual(double spindexer) {
+        TELE.addData("Velocity", velo);
         TELE.addLine("spindex equal");
         TELE.update();
         return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
@@ -67,7 +75,6 @@ public class Blue_V2 extends LinearOpMode {
 
     public Action initShooter(int vel) {
         return new Action() {
-            double velo = 0.0;
             double initPos = 0.0;
             double stamp = 0.0;
             double stamp1 = 0.0;
@@ -80,34 +87,15 @@ public class Blue_V2 extends LinearOpMode {
                     stamp2 = getRuntime();
                 }
 
+                targetVelocity = (double) vel;
                 ticker++;
                 if (ticker % 16 == 0) {
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
                     stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
                     stamp1 = stamp;
                 }
 
-                if (vel - velo > 500) {
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
-                    powPID = 1.0;
+                velo = flywheel.getVelo();
-                } else {
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - 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));
-                }
-
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
                 robot.transfer.setPower(1);
@@ -119,6 +107,7 @@ public class Blue_V2 extends LinearOpMode {
                     stamp2 = getRuntime();
                     return true;
                 } else if (steady && getRuntime() - stamp2 > 1.5){
+                    TELE.addData("Velocity", velo);
                     TELE.addLine("finished init");
                     TELE.update();
                     return false;
@@ -131,49 +120,12 @@ public class Blue_V2 extends LinearOpMode {
 
     public Action steadyShooter(int vel, boolean last) {
         return new Action() {
-            double velo = 0.0;
-            double initPos = 0.0;
             double stamp = 0.0;
-            double stamp1 = 0.0;
-            double ticker = 0.0;
-            double stamp2 = 0.0;
-            double currentPos = 0.0;
             boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (ticker == 0) {
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
-                    stamp2 = getRuntime();
+                velo = flywheel.getVelo();
-                }
+                steady = flywheel.getSteady();
-
-                ticker++;
-                if (ticker % 8 == 0) {
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
-                    stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
-                    stamp1 = stamp;
-                }
-
-                if (vel - velo > 500) {
-                    powPID = 1.0;
-                } else if (velo - vel > 500){
-                    powPID = 0.0;
-                } else {
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - 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));
-                }
-
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
                 robot.transfer.setPower(1);
@@ -181,11 +133,14 @@ public class Blue_V2 extends LinearOpMode {
                 TELE.addData("Velocity", velo);
                 TELE.update();
 
-                if (Math.abs(vel - velo) < 100 && last && !steady){
+
+                if (last && !steady){
                     stamp = getRuntime();
-                    steady = true;
+                    drive.updatePoseEstimate();
+
+                    teleStart = drive.localizer.getPose();
                     return false;
-                } else if (steady && getRuntime() - stamp > 1.0) {
+                } else if (steady) {
                     stamp = getRuntime();
                     return true;
                 } else {
@@ -216,6 +171,7 @@ public class Blue_V2 extends LinearOpMode {
                 }
                 aprilTag.update();
 
+                TELE.addData("Velocity", velo);
                 TELE.addData("21", gpp);
                 TELE.addData("22", pgp);
                 TELE.addData("23", ppg);
@@ -232,11 +188,43 @@ public class Blue_V2 extends LinearOpMode {
         };
     }
 
+    public Action intakeReject() {
+        return new Action() {
+            double stamp = 0.0;
+            int ticker = 0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+
+                double position = 0.0;
+
+                if ((getRuntime() % 0.3) > 0.15) {
+                    position = spindexer_intakePos1 + 0.02;
+                } else {
+                    position = spindexer_intakePos1 - 0.02;
+                }
+
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1 - position);
+
+                if (ticker == 0) {
+                    stamp = getRuntime();
+                }
+                ticker++;
+
+                if (getRuntime() - stamp < 0.3){
+                    return true;
+                }else {
+                    robot.intake.setPower(0);
+                    return false;
+                }
+            }
+        };
+    }
+
     public Action spindex (double spindexer, double vel){
         return new Action() {
             double currentPos = 0.0;
             double stamp = 0.0;
-            double velo = 0.0;
             double initPos = 0.0;
             double stamp1 = 0.0;
             int ticker = 0;
@@ -272,12 +260,20 @@ public class Blue_V2 extends LinearOpMode {
                     powPID = Math.max(0, Math.min(1, powPID));
                 }
 
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
                 robot.spin1.setPosition(spindexer);
                 robot.spin2.setPosition(1-spindexer);
+                TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 return !spindexPosEqual(spindexer);
             }
         };
@@ -290,13 +286,14 @@ public class Blue_V2 extends LinearOpMode {
             boolean transferIn = false;
             double currentPos = 0.0;
             double stamp = 0.0;
-            double velo = 0.0;
             double initPos = 0.0;
             double stamp1 = 0.0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                TELE.addData("Velocity", velo);
                 TELE.addLine("shooting");
                 TELE.update();
+
                 if (ticker % 8 == 0) {
                     currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
                     stamp = getRuntime();
@@ -326,9 +323,15 @@ public class Blue_V2 extends LinearOpMode {
                     powPID = Math.max(0, Math.min(1, powPID));
                 }
 
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
 
                 if (ticker == 1) {
                     transferStamp = getRuntime();
@@ -336,12 +339,14 @@ public class Blue_V2 extends LinearOpMode {
                 }
                 if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
                     robot.transferServo.setPosition(transferServo_in);
+                    TELE.addData("Velocity", velo);
                     TELE.addData("ticker", ticker);
                     TELE.update();
                     transferIn = true;
                     return true;
                 } else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){
                     robot.transferServo.setPosition(transferServo_out);
+                    TELE.addData("Velocity", velo);
                     TELE.addLine("shot once");
                     TELE.update();
                     return false;
@@ -378,12 +383,16 @@ public class Blue_V2 extends LinearOpMode {
                 robot.spin1.setPosition(position);
                 robot.spin2.setPosition(1 - position);
 
+                TELE.addData("Velocity", velo);
                 TELE.addLine("Intaking");
                 TELE.update();
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 robot.intake.setPower(1);
                 if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
-                    robot.intake.setPower(0);
                     return false;
                 } else {
                     return true;
@@ -416,6 +425,10 @@ public class Blue_V2 extends LinearOpMode {
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
                 double s3D = robot.color3.getDistance(DistanceUnit.MM);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 if (s1D < 40) {
 
                     double green = robot.color1.getNormalizedColors().green;
@@ -463,6 +476,7 @@ public class Blue_V2 extends LinearOpMode {
                     }
                 }
 
+                TELE.addData("Velocity", velo);
                 TELE.addLine("Detecting");
                 TELE.addData("Distance 1", s1D);
                 TELE.addData("Distance 2", s2D);
@@ -482,6 +496,8 @@ public class Blue_V2 extends LinearOpMode {
 
         robot = new Robot(hardwareMap);
 
+        flywheel = new Flywheel();
+
         TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
         );
@@ -495,7 +511,7 @@ public class Blue_V2 extends LinearOpMode {
         TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                 .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
 
-        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, by1, bh1))
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
                 .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
                 .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
 
@@ -516,14 +532,14 @@ public class Blue_V2 extends LinearOpMode {
         while (opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()) {
-                hoodDefault -= 0.01;
+                hoodAuto-= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()) {
-                hoodDefault += 0.01;
+                hoodAuto += 0.01;
             }
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
 
             robot.turr1.setPosition(turret_detectBlue);
             robot.turr2.setPosition(1 - turret_detectBlue);
@@ -534,7 +550,7 @@ public class Blue_V2 extends LinearOpMode {
             robot.spin2.setPosition(1 - spindexer_intakePos1);
 
             aprilTag.update();
-
+            TELE.addData("Velocity", velo);
             TELE.update();
         }
 
@@ -544,7 +560,7 @@ public class Blue_V2 extends LinearOpMode {
 
         if (opModeIsActive()) {
 
-            robot.hood.setPosition(hoodStart);
+            robot.hood.setPosition(hoodAuto);
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -553,13 +569,22 @@ public class Blue_V2 extends LinearOpMode {
                             Obelisk()
                     )
             );
+            drive.updatePoseEstimate();
 
+            teleStart = drive.localizer.getPose();
+
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
             shootingSequence();
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -567,19 +592,32 @@ public class Blue_V2 extends LinearOpMode {
                             intake(intake1Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
                             shoot1.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
                     )
             );
 
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
             shootingSequence();
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -587,15 +625,21 @@ public class Blue_V2 extends LinearOpMode {
                             intake(intake2Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
                             shoot2.build(),
                             ColorDetect(),
-                            steadyShooter(AUTO_CLOSE_VEL, true)
+                            steadyShooter(AUTO_CLOSE_VEL, true),
+                            intakeReject()
                     )
             );
 
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
@@ -605,8 +649,8 @@ public class Blue_V2 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
+            TELE.addData("Velocity", velo);
             TELE.addLine("finished");
-
             TELE.update();
 
             sleep(2000);
@@ -616,6 +660,7 @@ public class Blue_V2 extends LinearOpMode {
     }
 
     public void shootingSequence() {
+        TELE.addData("Velocity", velo);
         if (gpp) {
             if (b1 + b2 + b3 == 4) {
                 if (b1 == 2 && b2 - b3 == 0) {