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.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.limelightvision.LLResult; 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.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp; 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 @Config
@TeleOp @TeleOp
//TODO: fix to get the apriltag that it is reading //TODO: fix to get the apriltag that it is reading
public class LimelightTest extends LinearOpMode { public class LimelightTest extends LinearOpMode {
MultipleTelemetry TELE; 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 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 public static int mode = 0; //0 for bare testing, 1 for obelisk, 2 for blue track, 3 for red track
@Override @Override
public void runOpMode() throws InterruptedException { public void runOpMode() throws InterruptedException {
Limelight3A limelight = hardwareMap.get(Limelight3A.class, "limelight");
TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); 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(); waitForStart();
if (isStopRequested()) return; if (isStopRequested()) return;
limelight.start();
while (opModeIsActive()){ while (opModeIsActive()){
if (mode == 0){ if (mode == 0){
limelight.pipelineSwitch(pipeline); robot.limelight.pipelineSwitch(pipeline);
LLResult result = limelight.getLatestResult(); LLResult result = robot.limelight.getLatestResult();
if (result != null) { if (result != null) {
if (result.isValid()) { if (result.isValid()) {
TELE.addData("tx", result.getTx()); TELE.addData("tx", result.getTx());
@@ -38,39 +39,21 @@ public class LimelightTest extends LinearOpMode {
} }
} }
} else if (mode == 1){ } else if (mode == 1){
limelight.pipelineSwitch(1); int obeliskID = turret.detectObelisk();
LLResult result = limelight.getLatestResult(); TELE.addData("Limelight ID", obeliskID);
if (result != null && result.isValid()) {
List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
for (LLResultTypes.FiducialResult fiducial : fiducials) {
int id = fiducial.getFiducialId();
TELE.addData("ID", id);
TELE.update(); 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();
} else if (mode == 2){ double x = turret.getLimelightX();
limelight.pipelineSwitch(4); double y = turret.getLimelightY();
LLResult result = limelight.getLatestResult(); TELE.addData("tx", tx);
if (result != null) { TELE.addData("ty", ty);
if (result.isValid()) { TELE.addData("x", x);
TELE.addData("tx", result.getTx()); TELE.addData("y", y);
TELE.addData("ty", result.getTy());
TELE.update(); 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 { } 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 com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; 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.Robot;
import org.firstinspires.ftc.teamcode.utils.Turret; import org.firstinspires.ftc.teamcode.utils.Turret;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@Autonomous @Autonomous
@Config @Config
@@ -35,7 +33,6 @@ public class TurretTest extends LinearOpMode {
drive.updatePoseEstimate(); drive.updatePoseEstimate();
turret.trackGoal(drive.localizer.getPose()); turret.trackGoal(drive.localizer.getPose());
TELE.update(); 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); shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F);
shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter1.setVelocity(1400); shooter1.setVelocity(0);
shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter2.setVelocity(1400); shooter2.setVelocity(0);
hood = hardwareMap.get(Servo.class, "hood"); 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.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A; 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 @Config
public class Turret { public class Turret {
@@ -29,6 +33,10 @@ public class Turret {
private double turrPos = 0.0; private double turrPos = 0.0;
private double offset = 0.0; private double offset = 0.0;
private double bearing = 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.TELE = tele;
this.robot = rob; this.robot = rob;
this.webcam = cam; this.webcam = cam;
webcam.start();
} }
public double getTurrPos() { public double getTurrPos() {
@@ -52,36 +61,58 @@ public class Turret {
return Math.abs(pos - this.getTurrPos()) < turretTolerance; return Math.abs(pos - this.getTurrPos()) < turretTolerance;
} }
public double getBearing() { private void limelightRead(){ // only for tracking purposes, not general reads
if (redAlliance) { if (redAlliance){
AprilTagDetection d24 = webcam.getTagById(24); webcam.pipelineSwitch(3);
if (d24 != null) {
bearing = d24.ftcPose.bearing;
return bearing;
} else { } else {
return 1000.0; webcam.pipelineSwitch(2);
} }
} else {
AprilTagDetection d20 = webcam.getTagById(20); LLResult result = webcam.getLatestResult();
if (d20 != null) { if (result != null) {
bearing = d20.ftcPose.bearing; if (result.isValid()) {
return bearing; tx = result.getTx();
} else { ty = result.getTy();
return 1000.0; // 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() { public int detectObelisk() {
AprilTagDetection id21 = webcam.getTagById(21); webcam.pipelineSwitch(1);
AprilTagDetection id22 = webcam.getTagById(22); LLResult result = webcam.getLatestResult();
AprilTagDetection id23 = webcam.getTagById(23); if (result != null && result.isValid()) {
if (id21 != null) { List<LLResultTypes.FiducialResult> fiducials = result.getFiducialResults();
obeliskID = 21; for (LLResultTypes.FiducialResult fiducial : fiducials) {
} else if (id22 != null) { obeliskID = fiducial.getFiducialId();
obeliskID = 22; }
} else if (id23 != null) {
obeliskID = 23;
} }
return obeliskID; return obeliskID;
} }
@@ -90,8 +121,6 @@ public class Turret {
return obeliskID; return obeliskID;
} }
/* /*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading 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 {
repositories {
mavenCentral() mavenCentral()
google()
maven { url 'https://maven.pedropathing.com' }
}
} }