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 2ab1b16..c8e698d 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 @@ -4,32 +4,33 @@ import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; 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 com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import java.util.List; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Turret; @Config @TeleOp //TODO: fix to get the apriltag that it is reading public class LimelightTest extends LinearOpMode { MultipleTelemetry TELE; + Turret turret; + Robot robot; 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 @Override public void runOpMode() throws InterruptedException { - Limelight3A limelight = hardwareMap.get(Limelight3A.class, "limelight"); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); - limelight.pipelineSwitch(pipeline); + robot = new Robot(hardwareMap); + turret = new Turret(robot, TELE, robot.limelight); + robot.limelight.pipelineSwitch(pipeline); waitForStart(); if (isStopRequested()) return; - limelight.start(); while (opModeIsActive()){ if (mode == 0){ - limelight.pipelineSwitch(pipeline); - LLResult result = limelight.getLatestResult(); + robot.limelight.pipelineSwitch(pipeline); + LLResult result = robot.limelight.getLatestResult(); if (result != null) { if (result.isValid()) { TELE.addData("tx", result.getTx()); @@ -38,39 +39,21 @@ public class LimelightTest extends LinearOpMode { } } } else if (mode == 1){ - limelight.pipelineSwitch(1); - LLResult result = limelight.getLatestResult(); - if (result != null && result.isValid()) { - List fiducials = result.getFiducialResults(); - for (LLResultTypes.FiducialResult fiducial : fiducials) { - int id = fiducial.getFiducialId(); - TELE.addData("ID", id); - TELE.update(); - } - - } - } else if (mode == 2){ - limelight.pipelineSwitch(4); - LLResult result = limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - TELE.addData("tx", result.getTx()); - TELE.addData("ty", result.getTy()); - TELE.update(); - } - } - } else if (mode == 3){ - limelight.pipelineSwitch(5); - LLResult result = limelight.getLatestResult(); - if (result != null) { - if (result.isValid()) { - TELE.addData("tx", result.getTx()); - TELE.addData("ty", result.getTy()); - TELE.update(); - } - } + int obeliskID = turret.detectObelisk(); + TELE.addData("Limelight ID", obeliskID); + TELE.update(); + } else if (mode == 2 || mode == 3){ // Use redAlliance variable to switch between red and blue + double tx = turret.getBearing(); + double ty = turret.getTy(); + double x = turret.getLimelightX(); + double y = turret.getLimelightY(); + TELE.addData("tx", tx); + TELE.addData("ty", ty); + TELE.addData("x", x); + TELE.addData("y", y); + TELE.update(); } else { - limelight.pipelineSwitch(0); + robot.limelight.pipelineSwitch(0); } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java index 6f17fef..781f1a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java @@ -8,10 +8,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; -import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Turret; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; @Autonomous @Config @@ -35,7 +33,6 @@ public class TurretTest extends LinearOpMode { drive.updatePoseEstimate(); turret.trackGoal(drive.localizer.getPose()); - TELE.update(); } 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 bb11edf..9dcc104 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 @@ -79,10 +79,10 @@ public class Robot { shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter1.setVelocity(1400); + shooter1.setVelocity(0); shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); - shooter2.setVelocity(1400); + shooter2.setVelocity(0); hood = hardwareMap.get(Servo.class, "hood"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 39cb80a..75cf6d1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -5,9 +5,13 @@ import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.Limelight3A; -import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; @Config public class Turret { @@ -29,6 +33,10 @@ public class Turret { private double turrPos = 0.0; private double offset = 0.0; private double bearing = 0.0; + double tx = 0.0; + double ty = 0.0; + double limelightPosX = 0.0; + double limelightPosY = 0.0; @@ -36,6 +44,7 @@ public class Turret { this.TELE = tele; this.robot = rob; this.webcam = cam; + webcam.start(); } public double getTurrPos() { @@ -52,36 +61,58 @@ public class Turret { return Math.abs(pos - this.getTurrPos()) < turretTolerance; } - public double getBearing() { - if (redAlliance) { - AprilTagDetection d24 = webcam.getTagById(24); - if (d24 != null) { - bearing = d24.ftcPose.bearing; - return bearing; - } else { - return 1000.0; - } + private void limelightRead(){ // only for tracking purposes, not general reads + if (redAlliance){ + webcam.pipelineSwitch(3); } else { - AprilTagDetection d20 = webcam.getTagById(20); - if (d20 != null) { - bearing = d20.ftcPose.bearing; - return bearing; - } else { - return 1000.0; + webcam.pipelineSwitch(2); + } + + LLResult result = webcam.getLatestResult(); + if (result != null) { + if (result.isValid()) { + tx = result.getTx(); + ty = result.getTy(); + // MegaTag1 code for receiving position + Pose3D botpose = result.getBotpose(); + if (botpose != null){ + limelightPosX = botpose.getPosition().x; + limelightPosY = botpose.getPosition().y; + } + } } } + public double getBearing() { + tx = 1000; + limelightRead(); + return tx; + } + + public double getTy(){ + limelightRead(); + return ty; + } + + public double getLimelightX(){ + limelightRead(); + return limelightPosX; + } + + public double getLimelightY(){ + limelightRead(); + return limelightPosY; + } + public int detectObelisk() { - AprilTagDetection id21 = webcam.getTagById(21); - AprilTagDetection id22 = webcam.getTagById(22); - AprilTagDetection id23 = webcam.getTagById(23); - if (id21 != null) { - obeliskID = 21; - } else if (id22 != null) { - obeliskID = 22; - } else if (id23 != null) { - obeliskID = 23; + webcam.pipelineSwitch(1); + LLResult result = webcam.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fiducial : fiducials) { + obeliskID = fiducial.getFiducialId(); + } } return obeliskID; } @@ -90,8 +121,6 @@ public class Turret { return obeliskID; } - - /* Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading */ diff --git a/build.gradle b/build.gradle index e70f209..02aa304 100644 --- a/build.gradle +++ b/build.gradle @@ -25,5 +25,9 @@ allprojects { } repositories { - mavenCentral() + repositories { + mavenCentral() + google() + maven { url 'https://maven.pedropathing.com' } + } }