KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

need to add zero code to properly test

Browse Source
This commit is contained in:
DanTheMan-byte
2026-01-23 22:50:33 -06:00
parent 78d38481a7
commit 5922f4e935
5 changed files with 86 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
for (LLResultTypes.FiducialResult fiducial : fiducials) {
int id = fiducial.getFiducialId();
TELE.addData("ID", id);
int obeliskID = turret.detectObelisk();
TELE.addData("Limelight ID", obeliskID);
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());
} 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 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();
}
}
} else {
limelight.pipelineSwitch(0);
robot.limelight.pipelineSwitch(0);
}
}
}

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
View File

@@ -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();
}

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

@@ -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");

81
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
View File

@@ -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;
private void limelightRead(){ // only for tracking purposes, not general reads
if (redAlliance){
webcam.pipelineSwitch(3);
} else {
return 1000.0;
webcam.pipelineSwitch(2);
}
} else {
AprilTagDetection d20 = webcam.getTagById(20);
if (d20 != null) {
bearing = d20.ftcPose.bearing;
return bearing;
} else {
return 1000.0;
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<LLResultTypes.FiducialResult> 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
*/

4
build.gradle
View File

@@ -25,5 +25,9 @@ allprojects {
}
repositories {
repositories {
mavenCentral()
google()
maven { url 'https://maven.pedropathing.com' }
}
}