Further speed up loop times. We are now running 50% faster but need to retune the turret PID and update the shoot all speed everywhere.

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

@@ -12,6 +12,7 @@ public class Flywheel {
     public double velo1 = 0.0;
     public double velo2 = 0.0;
     double targetVelocity = 0.0;
+    double previousTargetVelocity = 0.0;
     double powPID = 0.0;
     boolean steady = false;
     public Flywheel (HardwareMap hardwareMap) {
@@ -55,20 +56,21 @@ public class Flywheel {
     private double TPS_to_RPM (double TPS) { return (TPS*60.0)/28.0;}
 
     public double manageFlywheel(double commandedVelocity) {
+
+        if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) {
             targetVelocity = commandedVelocity;
-
             // Add code here to set PIDF based on desired RPM
-
+            //robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
-        robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1);
+            //robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
-        robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2);
             robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity));
             robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity));
 
             // Record Current Velocity
             velo1 = TPS_to_RPM(robot.shooter1.getVelocity());
             velo2 = TPS_to_RPM(robot.shooter2.getVelocity());
-        velo = Math.max(velo1,velo2);
+            velo = Math.max(velo1, velo2);
 
+        }
         // really should be a running average of the last 5
         steady = (Math.abs(targetVelocity - velo) < 200.0);
 

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

@@ -44,7 +44,7 @@ public class Servos {
         return prevSpinPos;
     }
 
-    public boolean servoPosEqual(double pos1, double pos2) {
+    public static boolean servoPosEqual(double pos1, double pos2) {
         return (Math.abs(pos1 - pos2) < 0.005);
     }
 

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

@@ -49,6 +49,7 @@ public class Spindexer {
     public double spindexerOuttakeWiggle = 0.01;
     private double prevPos = 0.0;
     public double spindexerPosOffset = 0.00;
+    double shootWaitMax = 4;
     public Types.Motif desiredMotif = Types.Motif.NONE;
     // For Use
     enum RotatedBallPositionNames {
@@ -467,7 +468,6 @@ public class Spindexer {
                 break;
 
             case SHOOTWAIT:
-                double shootWaitMax = 4;
                 // Stopping when we get to the new position
                 if (prevIntakeState != currentIntakeState) {
                     if (commandedIntakePosition==2) {
@@ -505,8 +505,8 @@ public class Spindexer {
                 if (servos.getSpinPos() > spinEndPos){
                     currentIntakeState = IntakeState.FINDNEXT;
                 } else {
-                    double spinPos = servos.getSpinCmdPos() + shootAllSpindexerSpeedIncrease;
+                    double spinPos = servos.getSpinCmdPos() + 0.005; //shootAllSpindexerSpeedIncrease;
-                    if (spinPos > spinEndPos + 0.03){
+                    if (spinPos > spinEndPos + 0.03) { // 0.03 @ 48ms loops times
                         spinPos = spinEndPos + 0.03;
                     }
                     servos.setSpinPos(spinPos);

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

@@ -38,7 +38,8 @@ public class Turret {
     // TODO: tune these values for limelight
 
     public static double clampTolerance = 0.03;
-    public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
+    //public static double B_PID_P = 0.105, B_PID_I = 0.0, B_PID_D = 0.0125;
+    public static double B_PID_P = 0.095, B_PID_I = 0.0, B_PID_D = 0.0090;
     Robot robot;
     MultipleTelemetry TELE;
     Limelight3A webcam;
@@ -56,6 +57,9 @@ public class Turret {
     private double permanentOffset = 0.0;
     private PIDController bearingPID;
 
+    private double prevTurretPos = 0.0;
+    private boolean firstTurretPos = true;
+
     public Turret(Robot rob, MultipleTelemetry tele, Limelight3A cam) {
         this.TELE = tele;
         this.robot = rob;
@@ -74,8 +78,18 @@ public class Turret {
     }
 
     public void manualSetTurret(double pos) {
+        setTurretPos(pos);
+    }
+
+    public double setTurretPos(double pos) {
+        if (firstTurretPos || !Servos.servoPosEqual(pos, prevTurretPos)) {
             robot.turr1.setPosition(pos);
             robot.turr2.setPosition(1 - pos);
+            firstTurretPos = false;
+        }
+
+        prevTurretPos = pos;
+        return pos;
     }
 
     public boolean turretEqual(double pos) {
@@ -273,8 +287,7 @@ public class Turret {
         }
 
         // Set servo positions
-        robot.turr1.setPosition(turretPos + manualOffset);
+        setTurretPos(turretPos + manualOffset);
-        robot.turr2.setPosition(1.0 - turretPos - manualOffset);
 
 
         /* ---------------- TELEMETRY ---------------- */