ripark2/FTC-Code
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2/11 changes

Browse Source
This commit is contained in:
Plano East Robotics
2026-02-11 16:28:54 -06:00
parent 28d98c41d9
commit f701ff884f
5 changed files with 219 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/FlyOp.java
View File

@@ -3,39 +3,50 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
@Config
@TeleOp(name="FlyOpTuner", group="Test")
@TeleOp
public class FlyOp extends LinearOpMode {
private Hware robot;
// Dashboard-editable
public static double TARGET = 1500.0; // ticks/sec (flip sign if needed)
public static double launchPos = 1.0; // servo position (0..1 usually)
public static double TARGET = 1500; // ticks/sec (flip sign if needed)
public static double launchPos = 0.7; // servo position (0..1 usually)
public static double intake = 0.6;
@Override
public void runOpMode() throws InterruptedException {
robot = new Hware();
robot.initialize(hardwareMap);
robot.activeTransfer.setPosition(0.7);
robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
waitForStart();
while (opModeIsActive() && !isStopRequested()) {
robot.bottomFly.setVelocity(TARGET);
robot.topFly.setVelocity(TARGET);
robot.launchHood.setPosition(launchPos);
robot.intake.set(intake);
robot.turret.setPower(0);
if (gamepad1.dpadUpWasPressed()) {
robot.intake.set(.8);
robot.activeTransfer.setPosition(.95);
}
if (gamepad1.dpadDownWasPressed()) {
robot.intake.set(0);
robot.activeTransfer.setPosition(.7);
}
telemetry.addData("Target", (int)Math.round(TARGET));

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java
View File

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.OUTDATED;
import com.arcrobotics.ftclib.hardware.SimpleServo;
import com.arcrobotics.ftclib.hardware.motors.Motor;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
@@ -16,12 +17,16 @@ public class Hware {
public Motor frontRight, frontLeft, backRight, backLeft, intake;
public DcMotorEx turret, bottomFly, topFly;
public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
public Limelight3A limelight;
WebcamName webcam;
public void initialize(HardwareMap hwMap) {
hardwareMap = hwMap;
/** MOTORS **/
// Chassis Motors
frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435);
frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435);
@@ -58,10 +63,14 @@ public class Hware {
backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
PIDFCoefficients pidf = new PIDFCoefficients(0.45, 0, 0, 16.3);
PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14
);
bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
bottomFly.setDirection(DcMotorSimple.Direction.REVERSE);
//Vision
limelight = hardwareMap.get(Limelight3A.class, "limelight");
}
}

83
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpTurretLock.java
View File

@@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -22,8 +24,16 @@ public class TeleOpTurretLock extends LinearOpMode {
// Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system.
// You said your start is (0,0). So measure these from that same origin.
public static double GOAL_X = 64.0;
public static double GOAL_Y = 75.0;
public static double GOAL_X = 75;
public static double GOAL_Y = 75;
public static double p = 1, i = 0, d = 0, f = 1;
public static double ticksperdegree = 9.738888 * 0.29969;
Limelight3A limelight;
// Set to a specific tag ID if you want (ex: 5). Use -1 to lock to any visible tag.
public static int TARGET_TAG_ID = -1;
@@ -35,13 +45,10 @@ public class TeleOpTurretLock extends LinearOpMode {
public static double DRIVE_SPEED = 1.0;
public static double SLOW_SPEED = 0.45;
// If your webcam name in RC config is different, change this
public static String WEBCAM_NAME = "Webcam 1";
LLResult result = null;
/* ----------------------------------------------------- */
private VisionPortal visionPortal;
private AprilTagProcessor aprilTag;
@Override
public void runOpMode() {
@@ -49,25 +56,17 @@ public class TeleOpTurretLock extends LinearOpMode {
// Hardware
Hware robot = new Hware();
robot.initialize(hardwareMap);
limelight = hardwareMap.get(Limelight3A.class, "limelight");
// Drive
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// Start pose (you said 0,0)
drive.setPoseEstimate(new Pose2d(0, 0, 0));
TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry, p, i, d, f);
// AprilTag
aprilTag = AprilTagProcessor.easyCreateWithDefaults();
visionPortal = new VisionPortal.Builder()
.setCamera(hardwareMap.get(WebcamName.class, WEBCAM_NAME))
.addProcessor(aprilTag)
.build();
// limelight
// TurretLock subsystem
TurretLock turretLock = new TurretLock(robot.turret);
turretLock.goalX = GOAL_X;
turretLock.goalY = GOAL_Y;
turretLock.targetTagId = TARGET_TAG_ID;
// Optional: if you want these controlled from dashboard too, you can,
// but you already have them inside TurretLock.
@@ -80,8 +79,14 @@ public class TeleOpTurretLock extends LinearOpMode {
waitForStart();
// turretLock.cameraInit();
limelight.pipelineSwitch(1);
limelight.start();
while (opModeIsActive()) {
result = robot.limelight.getLatestResult();
/* ---------------- DRIVE ---------------- */
double speed = DRIVE_SPEED;
if (gamepad1.left_bumper || gamepad1.right_bumper) speed = SLOW_SPEED;
@@ -95,49 +100,37 @@ public class TeleOpTurretLock extends LinearOpMode {
drive.setWeightedDrivePower(drivePower);
drive.update();
Pose2d pose = drive.getPoseEstimate();
/* ---------------- TOGGLE AUTOLOCK ---------------- */
boolean xNow = gamepad2.x;
if (xNow && !xPrev) autoLockEnabled = !autoLockEnabled;
xPrev = xNow;
Pose2d pose = drive.getPoseEstimate();
/* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
turretLock.goalX = GOAL_X;
turretLock.goalY = GOAL_Y;
turretLock.targetTagId = TARGET_TAG_ID;
/* ---------------- MANUAL TRIM ---------------- */
// Left trigger = +trim, Right trigger = -trim (same style as your old code)
double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger);
if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg);
if (gamepad2.y) turretLock.resetManualTrim(); // reset driver trim
if (gamepad2.b) turretLock.resetVisionTrim(); // reset vision trim
// telemetry.update();\
//
turretLock.update(pose, result);
/* ---------------- AUTOLOCK UPDATE ---------------- */
List<AprilTagDetection> detections = aprilTag.getDetections();
telemetry.addData("VisionAngle", turretLock.getBearing());
telemetry.addData("Angle", turretLock.getFinalDeg());
telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543));
telemetry.addData("MODE", turretLock.getTYPE());
telemetry.addData("Tag ID", turretLock.getObeliskID());
telemetry.addData("Does Lime have Result", result.isValid());
if (autoLockEnabled) {
turretLock.update(pose, detections);
} else {
// If autolock is off, just stop turret motor where it is
// (or you can put your own manual turret code here)
robot.turret.setPower(0);
}
telemetry.addData("Botpose", result.getBotpose());
telemetry.addData("Yaw", result.getTx());
telemetry.addData("Status", turretLock.getStatus());
telemetry.addData("LimelightInfo", robot.limelight.getStatus());
/* ---------------- TELEMETRY ---------------- */
telemetry.addData("AutoLock", autoLockEnabled);
telemetry.addData("Pose", "x=%.1f y=%.1f h=%.1fdeg",
pose.getX(), pose.getY(), Math.toDegrees(pose.getHeading()));
telemetry.addData("Goal", "x=%.1f y=%.1f", GOAL_X, GOAL_Y);
telemetry.addData("TargetTag", TARGET_TAG_ID);
telemetry.addData("TagCount", (detections == null) ? 0 : detections.size());
telemetry.addData("TurretTicks", robot.turret.getCurrentPosition());
telemetry.addLine("Controls: GP2 X=toggle, triggers=trim, Y=reset trim, B=reset vision");
telemetry.update();
}
if (visionPortal != null) visionPortal.close();
}
}

238
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TurretLock.java
View File

@@ -1,50 +1,65 @@
package org.firstinspires.ftc.teamcode;
import static java.lang.Math.abs;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
import org.firstinspires.ftc.teamcode.util.PersonalPID;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import java.util.List;
public class TurretLock {
public enum MODE{
LIME,
ODOMETRY
}
public MODE TYPE = MODE.ODOMETRY;
/* ------------------- TUNABLES ------------------- */
// Field target (in RoadRunner field units; usually inches)
// Set this to the AprilTag/backdrop location you want to aim at.
public double goalX = 64.0;
public double goalY = 75.0;
public double goalX = 75;
public double goalY = 75;
PersonalPID pid;
// Turret conversion
// If you already trust your TICKS_PER_DEGREE, keep it.
public double ticksPerDegree = 9.738888;
public double ticksPerDegree = 2.543; //9.738888 * 0.29969;
// Mechanical limits (ticks). Keep your existing numbers.
public int minTicks = (int) (ticksPerDegree * (-90));
public int maxTicks = (int) (ticksPerDegree * ( 90));
public int maxTicks = (int) (ticksPerDegree * ( 90 ));
// Which AprilTag to lock to (set -1 to accept any visible tag)
public int targetTagId = -1;
public double finalDeg = 0;
// Vision correction: degrees of turret change per degree of tag "bearing"/yaw error.
// Start small (0.6–1.2) and tune.
public double visionDegPerDegError = 0.9;
public static boolean limelightUsed = true;
// Deadband so it doesn’t twitch
public double visionDeadbandDeg = 0.6;
public static double visionCorrectionGain = 0.08; // Single tunable gain
public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
public static double cameraBearingEqual = 0.5; // Deadband
// How hard we trust vision vs odom.
// 0 = ignore vision, 1 = full correction.
public double visionBlend = 1.0;
// TODO: tune these values for limelight
Hware robot;
Limelight3A webcam;
double tx = 0.0;
double ty = 0.0;
double limelightPosX = 0.0;
double limelightPosY = 0.0;
private int obeliskID = 0;
// Smooth the vision correction so it doesn’t jump
// 0.0 = no smoothing, 0.8 = heavy smoothing
public double visionLowPass = 0.65;
// If we haven't seen a tag in this many seconds, slowly decay vision trim back to 0
public double trimDecayPerSec = 12.0; // deg/sec back toward 0
double limelightOffset;
// Optional: flip direction if your turret sign is backwards
public boolean invertTurret = false;
@@ -56,29 +71,108 @@ public class TurretLock {
private double visionTrimDeg = 0.0; // learned trim from camera
private double filteredVisionTrimDeg = 0.0;
private final ElapsedTime tagTimer = new ElapsedTime();
public double p, i, d, f;
public TurretLock(DcMotorEx turretMotor) {
Telemetry tele;
// private final ElapsedTime tagTimer = new ElapsedTime();
public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele, double p, double i, double d, double f) {
this.turret = turretMotor;
tagTimer.reset();
this.webcam = cam;
this.tele = tele;
this.p = p;
this.i = i;
this.d = d;
this.f = f;
}
/** Call this if you want driver trim like your triggers (degrees). */
public void addManualTrimDeg(double deltaDeg) {
public double getTurrPos() {
return turret.getCurrentPosition();
}
public void addManualTrimDeg(double deltaDeg){
manualTrimDeg += deltaDeg;
}
public void resetManualTrim() {
manualTrimDeg = 0.0;
public void resetManualTrim(){
manualTrimDeg = 0;
}
private void limelightRead(LLResult result) { // only for tracking purposes, not general reads
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 void resetVisionTrim() {
visionTrimDeg = 0.0;
filteredVisionTrimDeg = 0.0;
}
}
}
public double getBearing() {
// tx = 1000;
LLResult result = webcam.getLatestResult();
return result.getBotpose().getOrientation().getYaw();
}
public double getTy() {
return ty;
}
public double getLimelightX() {
return limelightPosX;
}
public double getLimelightY() {
return limelightPosY;
}
public int detectObelisk() {
com.qualcomm.hardware.limelightvision.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;
}
public int getObeliskID() {
return obeliskID;
}
/*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
private double bearingAlign(LLResult llResult) {
double targetTx = 0;
if(llResult != null && llResult.isValid()){
targetTx = llResult.getTx(); // How far left or right the target is (degrees)
}
return llResult.getBotpose().getOrientation().getYaw();
}
public void Init(){
pid = new PersonalPID(p, i, d, f);
}
public String getStatus(){
return String.valueOf(webcam.getStatus());
}
/** Main update: call every loop. */
public void update(Pose2d robotPose, List<AprilTagDetection> detections) {
public void update(Pose2d robotPose, LLResult result) {
// 1) ODOM: compute turret angle needed to face goal (in degrees)
double dx = goalX - robotPose.getX();
double dy = goalY - robotPose.getY();
@@ -98,33 +192,22 @@ public class TurretLock {
if (invertTurret) turretCmdDeg = -turretCmdDeg;
// 2) VISION: find tag error and update vision trim
Double tagErrorDeg = getTagErrorDeg(detections);
if (tagErrorDeg != null) {
// deadband
if (Math.abs(tagErrorDeg) < visionDeadbandDeg) tagErrorDeg = 0.0;
// Convert tag error into turret trim change.
// Sign: if tag is to the right (+error), turret should move right or left?
// If it goes the wrong way, flip the sign here (multiply by -1).
double deltaTrim = (tagErrorDeg * visionDegPerDegError) * visionBlend;
// We want trim to counteract the observed error, so subtract.
visionTrimDeg -= deltaTrim;
tagTimer.reset();
} else {
// no tag: decay vision trim toward 0 over time
double dt = 0.02; // assume ~50Hz; fine for a simple decay
double decay = trimDecayPerSec * dt;
if (visionTrimDeg > 0) visionTrimDeg = Math.max(0, visionTrimDeg - decay);
else if (visionTrimDeg < 0) visionTrimDeg = Math.min(0, visionTrimDeg + decay);
limelightRead(result);
filteredVisionTrimDeg = result.getTx();
switch (TYPE){
case LIME:
finalDeg = turretCmdDeg + manualTrimDeg - filteredVisionTrimDeg;
if (!result.isValid()) {
TYPE = MODE.ODOMETRY;
}
break;
case ODOMETRY:
finalDeg = turretCmdDeg + manualTrimDeg;
if (finalDeg < 12 || finalDeg > -12) {
TYPE = MODE.LIME;
}
break;
}
// Low-pass filter the trim so it doesn’t jerk
filteredVisionTrimDeg = filteredVisionTrimDeg * visionLowPass + visionTrimDeg * (1.0 - visionLowPass);
// 3) Combine
double finalDeg = turretCmdDeg + manualTrimDeg + filteredVisionTrimDeg;
// Optional: keep within your allowed lock range (like ±90)
finalDeg = clamp(finalDeg, -90, 90);
@@ -133,42 +216,23 @@ public class TurretLock {
int targetTicks = (int) Math.round(finalDeg * ticksPerDegree);
targetTicks = clamp(targetTicks, minTicks, maxTicks);
// double pow = pid.calculate(targetTicks);
// turret.setPower(pow);
turret.setTargetPosition(targetTicks);
turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
turret.setPower(1.0);
}
public MODE getTYPE() {
return TYPE;
}
public double getFinalDeg(){
return finalDeg;
}
/** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */
private Double getTagErrorDeg(List<AprilTagDetection> detections) {
if (detections == null || detections.isEmpty()) return null;
AprilTagDetection best = null;
for (AprilTagDetection d : detections) {
if (d == null || d.metadata == null) continue; // metadata can be null on some setups
if (targetTagId != -1 && d.id != targetTagId) continue;
// pick the first good one (or you can pick closest by range if you want)
best = d;
break;
}
if (best == null) return null;
// FTC AprilTagDetection typically gives:
// best.ftcPose.yaw (degrees) // yaw ~ left-right rotation of tag relative to camera
// best.ftcPose.bearing (degrees) on some versions
//
// We'll prefer "bearing" if present; otherwise use yaw.
try {
// If your SDK has bearing:
// return best.ftcPose.bearing;
// Otherwise:
return best.ftcPose.yaw;
} catch (Exception e) {
return null;
}
}
private static double normalizeDeg(double deg) {
while (deg > 180) deg -= 360;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
View File

@@ -53,8 +53,8 @@ public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer
lastEncPositions = lastTrackingEncPositions;
lastEncVels = lastTrackingEncVels;
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake"));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)