Improve Spindexer shaking. Upgrade shooterTest to control the spindexer and fix flywheel real time pidf coef updates..

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

@@ -5,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.19;
+    public static double spindexer_intakePos1 = 0.18;
 
-    public static double spindexer_intakePos2 = 0.35;//0.5;
+    public static double spindexer_intakePos2 = 0.36;//0.5;
 
-    public static double spindexer_intakePos3 = 0.51;//0.66;
+    public static double spindexer_intakePos3 = 0.54;//0.66;
 
     public static double spindexer_outtakeBall3 = 0.47;
 

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.tests;
 
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
 
@@ -12,12 +13,13 @@ import com.qualcomm.robotcore.hardware.DcMotorEx;
 
 import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
+import org.firstinspires.ftc.teamcode.utils.Spindexer;
 
 @Config
 @TeleOp
 public class ShooterTest extends LinearOpMode {
 
-    public static int mode = 0;
+    public static int mode = 1;
     public static double parameter = 0.0;
     // --- CONSTANTS YOU TUNE ---
 
@@ -31,9 +33,18 @@ public class ShooterTest extends LinearOpMode {
     public static double hoodPos = 0.501;
     public static double turretPos = 0.501;
     public static boolean shoot = false;
+
+    public static boolean intake = false;
     Robot robot;
     Flywheel flywheel;
 
+    double shootStamp = 0.0;
+    boolean shootAll = false;
+
+    public double spinPow = 0.09;
+
+    Spindexer spindexer ;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -41,6 +52,7 @@ public class ShooterTest extends LinearOpMode {
         DcMotorEx leftShooter = robot.shooter1;
         DcMotorEx rightShooter = robot.shooter2;
         flywheel = new Flywheel(hardwareMap);
+        spindexer = new Spindexer(hardwareMap);
 
         MultipleTelemetry TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
@@ -64,12 +76,53 @@ public class ShooterTest extends LinearOpMode {
                 robot.hood.setPosition(hoodPos);
             }
 
-            robot.transfer.setPower(transferPower);
+            if (intake) {
-            if (shoot) {
+                robot.intake.setPower(1);
-                robot.transferServo.setPosition(transferServo_in);
+
             } else {
-                robot.transferServo.setPosition(transferServo_out);
+                robot.intake.setPower(0);
             }
+
+
+            if (shoot) {
+                shootStamp = getRuntime();
+                shootAll = true;
+                shoot = false;
+                robot.transfer.setPower(transferPower);
+            }
+            if (shootAll) {
+
+                //intake = false;
+                //reject = false;
+
+                // TODO: Change starting position based on desired order to shoot green ball
+                //spindexPos = spindexer_intakePos1;
+
+                if (getRuntime() - shootStamp < 3.5) {
+
+                    robot.transferServo.setPosition(transferServo_in);
+
+                    robot.spin1.setPower(-spinPow);
+                    robot.spin2.setPower(spinPow);
+
+                } else {
+                    robot.transferServo.setPosition(transferServo_out);
+                    //spindexPos = spindexer_intakePos1;
+
+                    shootAll = false;
+
+                    robot.transferServo.setPosition(transferServo_out);
+                    robot.transfer.setPower(0);
+                    robot.spin1.setPower(0);
+                    robot.spin2.setPower(0);
+
+                    spindexer.resetSpindexer();
+                    spindexer.processIntake();
+                }
+            } else {
+                spindexer.processIntake();
+            }
+
             TELE.addData("Velocity", flywheel.getVelo());
             TELE.addData("Velocity 1", flywheel.getVelo1());
             TELE.addData("Velocity 2", flywheel.getVelo2());

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

@@ -38,10 +38,14 @@ public class Flywheel {
 
     // Set the robot PIDF for the next cycle.
     public void setPIDF(double p, double i, double d, double f) {
-        robot.shooterPIDF.p = p;
+        shooterPIDF1.p = p;
-        robot.shooterPIDF.i = i;
+        shooterPIDF1.i = i;
-        robot.shooterPIDF.d = d;
+        shooterPIDF1.d = d;
-        robot.shooterPIDF.f = f;
+        shooterPIDF1.f = f;
+        shooterPIDF2.p = p;
+        shooterPIDF2.i = i;
+        shooterPIDF2.d = d;
+        shooterPIDF2.f = f;
     }
 
     // Convert from RPM to Ticks per Second

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

@@ -21,7 +21,7 @@ public class Robot {
     //Initialize Public Components
 
     public static boolean usingLimelight = true;
-    public static boolean usingCamera = true;
+    public static boolean usingCamera = false;
     public DcMotorEx frontLeft;
     public DcMotorEx frontRight;
     public DcMotorEx backLeft;

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

@@ -8,7 +8,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 public class Servos {
     //PID constants
     // TODO: get PIDF constants
-    public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
+    public static double spinP = 2.0, spinI = 0, spinD = 0.3, spinF = 0.02;
     public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0;
     public static double spin_scalar = 1.0086;
     public static double spin_restPos = 0.0;
@@ -40,7 +40,7 @@ public class Servos {
     }
 
     public boolean spinEqual(double pos) {
-        return Math.abs(pos - this.getSpinPos()) < 0.02;
+        return Math.abs(pos - this.getSpinPos()) < 0.03;
     }
 
     public double getTurrPos() {

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

@@ -277,7 +277,6 @@ public class Spindexer {
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
                     currentIntakeState = Spindexer.IntakeState.UNKNOWN_DETECT;
                     stopSpindexer();
-                    detectBalls(true, true);
                     unknownColorDetect = 0;
                 } else {
                     // Keep moving the spindexer
@@ -288,7 +287,7 @@ public class Spindexer {
                 if (unknownColorDetect >5) {
                     currentIntakeState = Spindexer.IntakeState.FINDNEXT;
                 } else {
-                    detectBalls(true, true);
+                    //detectBalls(true, true);
                     unknownColorDetect++;
                 }
                 break;
@@ -343,7 +342,7 @@ public class Spindexer {
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
                     currentIntakeState = Spindexer.IntakeState.INTAKE;
                     stopSpindexer();
-                    detectBalls(false, false);
+                    //detectBalls(false, false);
                 } else {
                     // Keep moving the spindexer
                     moveSpindexerToPos(intakePositions[commandedIntakePosition]);
@@ -374,7 +373,7 @@ public class Spindexer {
 
             case SHOOT_ALL_READY:
                 // Double Check Colors
-                detectBalls(false, false); // Minimize hardware calls
+                //detectBalls(false, false); // Minimize hardware calls
                 if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) {
                     // All ball shot move to intake state
                     currentIntakeState = Spindexer.IntakeState.FINDNEXT;
@@ -422,7 +421,7 @@ public class Spindexer {
                 if (servos.spinEqual(intakePositions[commandedIntakePosition])) {
                     currentIntakeState = Spindexer.IntakeState.INTAKE;
                     stopSpindexer();
-                    detectBalls(true, false);
+                    //detectBalls(true, false);
                 } else {
                     // Keep moving the spindexer
                     moveSpindexerToPos(intakePositions[commandedIntakePosition]);