diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 0a944dc..f1b49bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/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; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java index c023c54..0625f6e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java index d8d0d23..306af37 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java +++ b/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); - TELE.update(); - } else { - if (obeliskPipe >= 3){ - obeliskPipe = 1; - } else { - obeliskPipe++; + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + int id = fiducial.getFiducialId(); + TELE.addData("ID", id); } + } } else if (mode == 2){ limelight.pipelineSwitch(4);