Progress update: some changes to auto and voltage added to flywheel (tested briefly, need more tests to verify)

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

@@ -72,6 +72,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate;
+import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed;
 import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault;
 
 import androidx.annotation.NonNull;
@@ -137,7 +138,7 @@ public class Auto_LT_Close extends LinearOpMode {
     public static double intake3Time = 4.2;
 
     public static double flywheel0Time = 3.5;
-    public static double pickup1Speed = 23;
+    public static double pickup1Speed = 15;
     // ---- SECOND SHOT / PICKUP ----
     public static double shoot1Vel = 2300;
     public static double shootAllVelocity = 2500;
@@ -214,12 +215,28 @@ public class Auto_LT_Close extends LinearOpMode {
                 ticker++;
                 robot.transferServo.setPosition(transferServo_out);
 
-                turret.manualSetTurret(turretShootPos);
-
                 drive.updatePoseEstimate();
 
                 teleStart = drive.localizer.getPose();
 
+                double robX = drive.localizer.getPose().position.x;
+                double robY = drive.localizer.getPose().position.y;
+                double robotHeading = drive.localizer.getPose().heading.toDouble();
+
+                double goalX = -15;
+                double goalY = 0;
+
+                double dx = robX - goalX;  // delta x from robot to goal
+                double dy = robY - goalY;  // delta y from robot to goal
+                Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
+
+                double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
+
+                targetingSettings = targeting.calculateSettings
+                        (robX, robY, robotHeading, 0.0, turretInterpolate);
+
+                turret.trackGoal(deltaPose);
+
                 TELE.addData("Velocity", flywheel.getVelo());
                 TELE.addData("Hood", robot.hood.getPosition());
                 TELE.addData("motif", motif_id);
@@ -504,6 +521,9 @@ public class Auto_LT_Close extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
+                TELE.addData("Full?", spindexer.isFull());
+                TELE.update();
+
                 return ((System.currentTimeMillis() - stamp) < (intakeTime * 1000)) && !spindexer.isFull();
             }
         };
@@ -780,11 +800,6 @@ public class Auto_LT_Close extends LinearOpMode {
 
         servos = new Servos(hardwareMap);
 
-        robot.limelight.start();
-
-        robot.limelight.pipelineSwitch(1);
-
-
         turret = new Turret(robot, TELE, robot.limelight);
 
         turret.manualSetTurret(turrDefault);
@@ -796,6 +811,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
         robot.transferServo.setPosition(transferServo_out);
 
+        limelightUsed = false;
+
         TrajectoryActionBuilder shoot0 = null;
         TrajectoryActionBuilder pickup1 = null;
         TrajectoryActionBuilder shoot1 = null;
@@ -831,6 +848,11 @@ public class Auto_LT_Close extends LinearOpMode {
                 ballCycles--;
             }
 
+            if (gamepad2.squareWasPressed()){
+                robot.limelight.start();
+                robot.limelight.pipelineSwitch(1);
+            }
+
             if (redAlliance) {
                 robot.light.setPosition(0.28);
 
@@ -1177,6 +1199,8 @@ public class Auto_LT_Close extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
+                flywheel.manageFlywheel(0);
+
                 TELE.addLine("finished");
                 TELE.update();
             }

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

@@ -763,6 +763,8 @@ public class Auto_LT_Far extends LinearOpMode {
 
                 teleStart = drive.localizer.getPose();
 
+                flywheel.manageFlywheel(0);
+
                 TELE.addLine("finished");
                 TELE.update();
             }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java

@@ -8,45 +8,40 @@ public class Front_Poses {
 
 
     public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
-    public static double rx2a = 41, ry2a = 18, rh2a = 140;
-    public static double rx2b = 19, ry2b = 40, rh2b = 140;
-
-
-    public static double rx3a = 55, ry3a = 39, rh3a = 140;
-
-    public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
-
-    public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
-
-    public static double rx4a = 80, ry4a = 48, rh4a = 140;
-    public static double rx4b = 45, ry4b = 83, rh4b = 140.1;
-
     public static double bx1 = 20, by1 = 0.5, bh1 = 0.1;
+
+    public static double rx2a = 41, ry2a = 18, rh2a = 140;
     public static double bx2a = 41, by2a = -18, bh2a = -140;
+
+    public static double rx2b = 23, ry2b = 36, rh2b = 140.1;
     public static double bx2b = 19, by2b = -40, bh2b = -140.1;
 
+    public static double rx3a = 55, ry3a = 39, rh3a = 140;
     public static double bx3a = 55, by3a = -39, bh3a = -140;
-    public static double bx3b = 41, by3b = -59, bh3b = -140.1;
+
+    public static double rx3aG = 60, ry3aG = 34, rh3aG = 140;
     public static double bx3aG = 55, by3aG = -43, bh3aG = -140;
 
+    public static double rx3b = 36, ry3b = 58, rh3b = 140.1;
+    public static double bx3b = 41, by3b = -59, bh3b = -140.1;
+
+    public static double rx4a = 75, ry4a = 53, rh4a = 140;
     public static double bx4a = 75, by4a = -53, bh4a = -140;
+
+    public static double rx4b = 50, ry4b = 78, rh4b = 140.1;
     public static double bx4b = 47, by4b = -85, bh4b = -140.1;
+
     public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
 
     public static double rShootX = 40, rShootY = 10, rShootH = 50;
-    public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
-
-    public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
-
-    public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
-
     public static double bShootX = 40, bShootY = 0, bShootH = -50;
+
+    public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
     public static double bxPrep = 45, byPrep = -10, bhPrep = -50;
 
+    public static double rLeaveX = 40, rLeaveY = -7, rLeaveH = 50;
+    public static double bLeaveX = 40, bLeaveY = 7, bLeaveH = -50;
 
     public static Pose2d teleStart = new Pose2d(0, 0, 0);
-
     public static Pose2d teleEnd = new Pose2d(0, 0, 0);
-
-
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java

@@ -97,7 +97,10 @@ public class TeleopV3 extends LinearOpMode {
         tController.setTolerance(0.001);
 
         Turret turret = new Turret(robot, TELE, robot.limelight);
+        limelightUsed = true;
+
         robot.light.setPosition(1);
+
         while (opModeInInit()) {
             robot.limelight.start();
             if (redAlliance) {
@@ -107,9 +110,6 @@ public class TeleopV3 extends LinearOpMode {
             }
         }
 
-        limelightUsed= true;
-
-
         waitForStart();
         if (isStopRequested()) return;
 
@@ -186,6 +186,8 @@ public class TeleopV3 extends LinearOpMode {
             }
 
             //SHOOTER:
+            double voltage = robot.voltage.getVoltage();
+            flywheel.setPIDF(robot.shooterPIDF_P, robot.shooterPIDF_I, robot.shooterPIDF_D, robot.shooterPIDF_F / voltage);
             flywheel.manageFlywheel(vel);
 
             //HOOD:
@@ -225,8 +227,6 @@ public class TeleopV3 extends LinearOpMode {
                 drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
             }
 
-
-
             if (enableSpindexerManager) {
                 //if (!shootAll) {
                 spindexer.processIntake();
@@ -328,7 +328,7 @@ public class TeleopV3 extends LinearOpMode {
 //            TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
 //            TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
 //            TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
-            TELE.addData("Voltage", robot.voltage.getVoltage());
+            TELE.addData("Voltage", robot.voltage.getVoltage()); // returns alleged recorded voltage (not same as driver hub)
 
             TELE.update();
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -26,12 +26,11 @@ public class ShooterTest extends LinearOpMode {
     public static double parameter = 0.0;
     // --- CONSTANTS YOU TUNE ---
 
-    //TODO: @Daniel FIX THE BELOW CONSTANTS A LITTLE IF NEEDED
     public static double Velocity = 0.0;
     public static double P = 255.0;
     public static double I = 0.0;
     public static double D = 0.0;
-    public static double F = 7.5;
+    public static double F = 90;
     public static double transferPower = 1.0;
     public static double hoodPos = 0.501;
     public static double turretPos = 0.501;
@@ -73,11 +72,13 @@ public class ShooterTest extends LinearOpMode {
 
         while (opModeIsActive()) {
 
+            double voltage = robot.voltage.getVoltage();
+
             if (mode == 0) {
                 rightShooter.setPower(parameter);
                 leftShooter.setPower(parameter);
             } else if (mode == 1) {
-                flywheel.setPIDF(P, I, D, F);
+                flywheel.setPIDF(P, I, D, F / voltage);
                 flywheel.manageFlywheel((int) Velocity);
             }
 
@@ -109,7 +110,6 @@ public class ShooterTest extends LinearOpMode {
                 //intake = false;
                 //reject = false;
 
-                // TODO: Change starting position based on desired order to shoot green ball
                 //spindexPos = spindexer_intakePos1;
                 if (getRuntime() - shootStamp < 3.5) {
 
@@ -150,6 +150,7 @@ public class ShooterTest extends LinearOpMode {
             TELE.addData("Power", robot.shooter1.getPower());
             TELE.addData("Steady?", flywheel.getSteady());
             TELE.addData("Position", robot.shooter1.getCurrentPosition());
+            TELE.addData("Voltage", voltage);
 
             TELE.update();
 

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

@@ -34,7 +34,7 @@ public class Robot {
     public double shooterPIDF_P = 255.0;
     public double shooterPIDF_I = 0.0;
     public double shooterPIDF_D = 0.0;
-    public double shooterPIDF_F = 7.5;
+    public double shooterPIDF_F = 90;
     public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001};
     public DcMotorEx shooter1;
     public DcMotorEx shooter2;