spindex pid tuned

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

@@ -7,9 +7,9 @@ public class ServoPositions {
 
     public static double spindexer_intakePos1 = 0.34;
 
-    public static double spindexer_intakePos3 = 0.5;
+    public static double spindexer_intakePos2 = 0.5;
 
-    public static double spindexer_intakePos2 = 0.66;
+    public static double spindexer_intakePos3 = 0.66;
 
     public static double spindexer_outtakeBall3 = 0.42;
 

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

@@ -94,11 +94,12 @@ public class IntakeTest extends LinearOpMode {
                     }
                 }
 
-            } else if (mode == 2){ // switch to this mode before switching modes
+            } else if (mode == 2){ // switch to this mode before switching modes or to reset balls
                 powPID = 0;
                 spindexerPos = spindexer_intakePos1;
                 stamp = getRuntime();
                 ticker = 0;
+                spindexer();
             }
             TELE.addData("Manual Power", manualPow);
             TELE.addData("PID Power", powPID);

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

@@ -3,16 +3,17 @@ package org.firstinspires.ftc.teamcode.tests;
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
+import java.util.List;
+
 //TODO: fix to get the apriltag that it is reading
 public class LimelightTest extends LinearOpMode {
     MultipleTelemetry TELE;
-    public static int pipeline = 0; //0 is for test; 1, 2, and 3 are for obelisk; 4 is for blue track; 5 is for red track
+    public static int pipeline = 0; //0 is for test; 1 for obelisk; 2 is for blue track; 3 is for red track
     public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
-    int obeliskPipe = 1;
     @Override
     public void runOpMode() throws InterruptedException {
         Limelight3A limelight = hardwareMap.get(Limelight3A.class, "Limelight");
@@ -33,17 +34,15 @@ public class LimelightTest extends LinearOpMode {
                     }
                 }
             } else if (mode == 1){
-                limelight.pipelineSwitch(obeliskPipe);
+                limelight.pipelineSwitch(1);
                 LLResult result = limelight.getLatestResult();
                 if (result != null && result.isValid()) {
-                    TELE.addData("ID", obeliskPipe+20);
+                    List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
-                    TELE.update();
+                    for (LLResultTypes.FiducialResult fiducial : fiducials) {
-                } else {
+                        int id = fiducial.getFiducialId();
-                    if (obeliskPipe >= 3){
+                        TELE.addData("ID", id);
-                        obeliskPipe = 1;
-                    } else {
-                        obeliskPipe++;
                     }
+
                 }
             } else if (mode == 2){
                 limelight.pipelineSwitch(4);

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

@@ -4,8 +4,6 @@ import com.acmerobotics.dashboard.config.Config;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
-import org.firstinspires.ftc.teamcode.utils.Robot;
-
 @Config
 public class Servos {
     Robot robot;
@@ -16,7 +14,7 @@ public class Servos {
 
     //PID constants
     // TODO: get PIDF constants
-    public static double spinP = 1.8, spinI = 0, spinD = 0.03, spinF = 0.03;
+    public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02;
     public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
 
     public static double spin_scalar = 1.0086;