diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java index 17832ad..bcf89a0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java @@ -454,10 +454,6 @@ public class Red_V2 extends LinearOpMode { robot.hood.setPosition(hoodAuto); - double turretPID = servo.setTurrPos(turret_detectRed); - robot.turr1.setPower(turretPID); - robot.turr2.setPower(-turretPID); - robot.transferServo.setPosition(transferServo_out); robot.spin1.setPower(spindexer_intakePos1); @@ -465,6 +461,7 @@ public class Red_V2 extends LinearOpMode { aprilTag.update(); TELE.addData("Velocity", velo); + TELE.addData("Turret Pos", servo.getTurrPos()); TELE.update(); } 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 new file mode 100644 index 0000000..3fe29b3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/LimelightTest.java @@ -0,0 +1,76 @@ +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.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.utils.Robot; + +import java.util.List; + +public class LimelightTest extends LinearOpMode { + Robot robot; + 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 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 { + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + robot.limelight.pipelineSwitch(pipeline); + + waitForStart(); + if (isStopRequested()) return; + robot.limelight.start(); + while (opModeIsActive()){ + if (mode == 0){ + robot.limelight.pipelineSwitch(pipeline); + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + TELE.addData("tx", result.getTx()); + TELE.addData("ty", result.getTy()); + TELE.update(); + } + } + } else if (mode == 1){ + robot.limelight.pipelineSwitch(obeliskPipe); + LLResult result = robot.limelight.getLatestResult(); + if (result != null && result.isValid()){ + TELE.addData("ID", obeliskPipe+20); + TELE.update(); + } else { + if (obeliskPipe >= 3){ + obeliskPipe = 1; + } else { + obeliskPipe++; + } + } + } else if (mode == 2){ + robot.limelight.pipelineSwitch(4); + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + TELE.addData("tx", result.getTx()); + TELE.addData("ty", result.getTy()); + TELE.update(); + } + } + } else if (mode == 3){ + robot.limelight.pipelineSwitch(5); + LLResult result = robot.limelight.getLatestResult(); + if (result != null) { + if (result.isValid()) { + TELE.addData("tx", result.getTx()); + TELE.addData("ty", result.getTy()); + TELE.update(); + } + } + } else { + robot.limelight.pipelineSwitch(0); + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index 2848835..6f67a59 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -51,7 +51,7 @@ public class Flywheel { velo3 = velo2; velo2 = velo1; - currentPos = shooter1CurPos / 3072; + currentPos = shooter1CurPos / 2048; stamp = getTimeSeconds(); //getRuntime(); velo1 = -60 * ((currentPos - initPos) / (stamp - stamp1)); initPos = currentPos; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 0d2fcb0..821597d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.utils; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; @@ -81,6 +82,8 @@ public class Robot { public RevColorSensorV3 color3; + public Limelight3A limelight; + public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map @@ -163,5 +166,7 @@ public class Robot { color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); + + limelight = hardwareMap.get(Limelight3A.class,"Limelight"); } }