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

Merge remote-tracking branch 'origin/master' into Targeting

Browse Source 
# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/TurretTest.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java
This commit is contained in:
Matt9150
2026-01-22 22:13:07 -06:00
parent c2e9d8fa87 fbdeb6e291
commit f20e640c62
5 changed files with 138 additions and 198 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -12,7 +12,7 @@ public class Poses {
public static double relativeGoalHeight = goalHeight - turretHeight;
public static Pose2d goalPose = new Pose2d(-15, 0, 0);
public static Pose2d goalPose = new Pose2d(-10, 0, 0);
public static double rx1 = 40, ry1 = -7, rh1 = 0;
public static double rx2a = 41, ry2a = 18, rh2a = Math.toRadians(140);
@@ -38,6 +38,6 @@ public class Poses {
public static double bx4b = 48, by4b = -79, bh4b = Math.toRadians(-140);
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0; //TODO: test this
public static Pose2d teleStart = new Pose2d(rx1, ry1, rh1);
public static Pose2d teleStart = new Pose2d(0, 0, 0);
}

168
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -1,11 +1,9 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinD;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinF;
import static org.firstinspires.ftc.teamcode.utils.Servos.spinI;
@@ -21,7 +19,6 @@ import com.acmerobotics.roadrunner.TranslationalVelConstraint;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.Actions;
import com.arcrobotics.ftclib.controller.PIDFController;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -29,11 +26,13 @@ import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.utils.Flywheel;
import org.firstinspires.ftc.teamcode.utils.Robot;
import org.firstinspires.ftc.teamcode.utils.Servos;
import org.firstinspires.ftc.teamcode.utils.Spindexer;
import org.firstinspires.ftc.teamcode.utils.Targeting;
import org.firstinspires.ftc.teamcode.utils.Turret;
import java.util.ArrayList;
import java.util.List;
@@ -50,8 +49,6 @@ public class TeleopV3 extends LinearOpMode {
public static double spindexPos = spindexer_intakePos1;
public static double spinPow = 0.09;
public static double bMult = 1, bDiv = 2200;
public static double limelightKp = 0.001; // Proportional gain for limelight auto-aim
public static double limelightDeadband = 0.5; // Ignore tx values smaller than this
public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0;
public static boolean manualTurret = true;
public double vel = 3000;
@@ -169,6 +166,12 @@ public class TeleopV3 extends LinearOpMode {
// robot.limelight.start();
AprilTagWebcam webcam = new AprilTagWebcam();
webcam.init(robot, TELE);
Turret turret = new Turret(robot, TELE, webcam);
waitForStart();
waitForStart();
if (isStopRequested()) return;
@@ -382,40 +385,22 @@ public class TeleopV3 extends LinearOpMode {
double robotY = robY - yOffset;
double robotHeading = drive.localizer.getPose().heading.toDouble();
double goalX = -10;
double goalX = -15;
double goalY = 0;
double dx = goalX - robotX; // delta x from robot to goal
double dy = goalY - robotY; // delta y from robot to goal
double dx = robotX - goalX; // delta x from robot to goal
double dy = robotY - goalY; // delta y from robot to goal
Pose2d deltaPose = new Pose2d(dx, dy, robotHeading);
double distanceToGoal = Math.sqrt(dx * dx + dy * dy);
desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360;
desiredTurretAngle += manualOffset + error;
offset = desiredTurretAngle - 180 - (Math.toDegrees(robotHeading - headingOffset));
if (offset > 135) {
offset -= 360;
}
double pos = turrDefault;
TELE.addData("offset", offset);
pos -= offset * ((double) 1 / 360);
if (pos < 0.13) {
pos = 0.13;
} else if (pos > 0.83) {
pos = 0.83;
}
targetingSettings = targeting.calculateSettings
(robotX,robotY,robotHeading,0.0, turretInterpolate);
turret.trackGoal(deltaPose);
webcam.update();
//VELOCITY AUTOMATIC
if (targetingVel) {
vel = targetingSettings.flywheelRPM;
@@ -446,105 +431,28 @@ public class TeleopV3 extends LinearOpMode {
//TODO: test the camera teleop code
// TODO: TEST THIS CODE
TELE.addData("posS2", pos);
LLResult result = robot.limelight.getLatestResult();
boolean limelightActive = false;
double turretMin = 0.13;
double turretMax = 0.83;
if (result != null && result.isValid()) {
double tx = result.getTx();
double ty = result.getTy();
if (Math.abs(tx) > limelightDeadband) {
limelightActive = true;
overrideTurr = true;
double currentTurretPos = servo.getTurrPos();
// + tx means tag is right, so rotate right
double adjustment = -tx * limelightKp;
// calculate new position
double newTurretPos = currentTurretPos + adjustment;
if (newTurretPos < turretMin) {
double forwardDist = turretMin - newTurretPos;
double backwardDist = (currentTurretPos - turretMin) + (turretMax - newTurretPos);
// check path distance
if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
newTurretPos = turretMax - (turretMin - newTurretPos);
} else {
newTurretPos = turretMin;
}
} else if (newTurretPos > turretMax) {
double forwardDist = newTurretPos - turretMax;
double backwardDist = (turretMax - currentTurretPos) + (newTurretPos - turretMin);
if (backwardDist < forwardDist && backwardDist < (turretMax - turretMin) / 2) {
newTurretPos = turretMin + (newTurretPos - turretMax);
} else {
newTurretPos = turretMax;
}
}
// Final clamp
if (newTurretPos < turretMin) {
newTurretPos = turretMin;
} else if (newTurretPos > turretMax) {
newTurretPos = turretMax;
}
pos = newTurretPos;
turretPos = pos;
camTicker++;
TELE.addData("tx", tx);
TELE.addData("ty", ty);
TELE.addData("limelightAdjustment", adjustment);
TELE.addData("limelightActive", true);
} else {
limelightActive = true;
overrideTurr = true;
TELE.addData("tx", tx);
TELE.addData("ty", ty);
TELE.addData("limelightActive", true);
TELE.addData("limelightStatus", "Centered");
}
} else {
if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) {
TELE.addData("limelightActive", false);
TELE.addData("limelightStatus", "No target");
} else {
camTicker = 0;
overrideTurr = false;
limelightActive = false;
}
}
if (!limelightActive && !overrideTurr) {
turretPos = pos;
}
TELE.addData("posS3", turretPos);
if (manualTurret && !limelightActive) {
pos = turrDefault + (manualOffset / 100) + error;
}
if (!overrideTurr && !limelightActive) {
turretPos = pos;
}
if (Math.abs(gamepad2.left_stick_x)>0.2 && !limelightActive) {
manualOffset += 1.35 * gamepad2.left_stick_x;
}
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1 - pos);
// if (y < 0.3 && y > -0.3 && x < 0.3 && x > -0.3 && rx < 0.3 && rx > -0.3) { //not moving
// double bearing;
//
// LLResult result = robot.light.getLatestResult();
// if (result != null) {
// if (result.isValid()) {
// bearing = result.getTx() * bMult;
//
// double bearingCorrection = bearing / bDiv;
//
// error += bearingCorrection;
//
// camTicker++;
// TELE.addData("tx", bearingCorrection);
// TELE.addData("ty", result.getTy());
// }
// }
//
// } else {
// camTicker = 0;
// overrideTurr = false;
// }
//HOOD:

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

@@ -1,12 +1,9 @@
package org.firstinspires.ftc.teamcode.tests;
import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
@@ -33,33 +30,19 @@ public class TurretTest extends LinearOpMode {
Turret turret = new Turret(robot, TELE, webcam);
waitForStart();
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(10, 0,0));
MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(15, 0,0));
while(opModeIsActive()){
drive.updatePoseEstimate();
Pose2d robotPose = drive.localizer.getPose();
turret.trackGoal(drive.localizer.getPose());
double dx = goalPose.position.x - robotPose.position.x;
double dy = goalPose.position.y - robotPose.position.y;
double heading = robotPose.heading.toDouble();
// field vector -> robot frame... avoids double calculation
double relX = dx * Math.cos(-heading) - dy * Math.sin(-heading);
double relY = dx * Math.sin(-heading) + dy * Math.cos(-heading);
Pose2d deltaPos = new Pose2d(
new Vector2d(relX, relY),
robotPose.heading
);
webcam.update();
webcam.displayAllTelemetry();
turret.trackGoal(deltaPos);
TELE.addData("Robot Pose", robotPose);
TELE.addData("Goal Pose", goalPose);
TELE.addData("Delta Pos", deltaPos);
TELE.update();
}

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

@@ -79,8 +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);
shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF);
shooter2.setVelocity(1400);
hood = hardwareMap.get(Servo.class, "hood");

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

@@ -5,61 +5,83 @@ 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.robotcore.util.Range;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
@Config
public class Turret {
public static double turretTolerance = 0.02;
public static double turrPosScalar = 1.009;
public static double turret180Range = 0.4;
public static double turrDefault = 0.4;
public static double turretRange = 0.6;
public static double cameraBearingEqual = 1;
public static double errorLearningRate = 0.15;
public static double turrMin = 0.2;
public static double turrMax = 0.8;
public static double turretTolerance = 0.02;
public static double cameraBearingEqual = 1.5;
public static double errorLearningRate = 0.02; // must be low
public static double maxOffsetDeg = 30.0;
private final Robot robot;
private final MultipleTelemetry TELE;
private final AprilTagWebcam webcam;
public static double deltaAngleThreshold = 0.02;
public static double angleMultiplier = 0.0;
Robot robot;
MultipleTelemetry TELE;
AprilTagWebcam webcam;
private int obeliskID = 0;
private double offsetDeg = 0.0;
private double turrPos = 0.0;
private double offset = 0.0;
private double bearing = 0.0;
public Turret(Robot rob, MultipleTelemetry tele, AprilTagWebcam cam) {
this.robot = rob;
this.TELE = tele;
this.robot = rob;
this.webcam = cam;
}
public double getTurretPos() {
return robot.turr1Pos.getVoltage() / 3.3;
public double getTurrPos() {
return turrPosScalar * (robot.turr1Pos.getVoltage() / 3.3);
}
public void manualSetTurret(double pos){
pos = Range.clip(pos, turrMin, turrMax);
robot.turr1.setPosition(pos);
robot.turr2.setPosition(1.0 - pos);
robot.turr2.setPosition(1-pos);
}
public boolean turretAt(double pos) {
return Math.abs(pos - getTurretPos()) < turretTolerance;
public boolean turretEqual(double pos) {
return Math.abs(pos - this.getTurrPos()) < turretTolerance;
}
public double getBearingDeg() {
AprilTagDetection tag =
redAlliance ? webcam.getTagById(24) : webcam.getTagById(20);
return (tag != null) ? tag.ftcPose.bearing : Double.NaN;
public double getBearing() {
if (redAlliance) {
AprilTagDetection d24 = webcam.getTagById(24);
if (d24 != null) {
bearing = d24.ftcPose.bearing;
return bearing;
} else {
return 1000.0;
}
} else {
AprilTagDetection d20 = webcam.getTagById(20);
if (d20 != null) {
bearing = d20.ftcPose.bearing;
return bearing;
} else {
return 1000.0;
}
}
}
public int detectObelisk() {
if (webcam.getTagById(21) != null) obeliskID = 21;
else if (webcam.getTagById(22) != null) obeliskID = 22;
else if (webcam.getTagById(23) != null) obeliskID = 23;
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;
}
return obeliskID;
}
@@ -67,34 +89,59 @@ public class Turret {
return obeliskID;
}
/*
Param @deltaPos = Pose2d when subtracting robot x, y, heading from goal x, y, heading
*/
public void trackGoal(Pose2d deltaPos) {
double turretAngleDeg = Math.toDegrees(
/* ---------------- FIELD → TURRET GEOMETRY ---------------- */
// Angle from robot to goal in robot frame
double desiredTurretAngleDeg = Math.toDegrees(
Math.atan2(deltaPos.position.y, deltaPos.position.x)
);
double bearingDeg = getBearingDeg();
// Robot heading (field → robot)
double robotHeadingDeg = Math.toDegrees(deltaPos.heading.toDouble());
if (!Double.isNaN(bearingDeg) &&
Math.abs(bearingDeg) < cameraBearingEqual) {
// Turret angle needed relative to robot
double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg;
offsetDeg -= bearingDeg * errorLearningRate;
offsetDeg = Range.clip(offsetDeg, -maxOffsetDeg, maxOffsetDeg);
turretAngleDeg = -turretAngleDeg;
// Normalize to [-180, 180]
while (turretAngleDeg > 180) turretAngleDeg -= 360;
while (turretAngleDeg < -180) turretAngleDeg += 360;
/* ---------------- APRILTAG CORRECTION ---------------- */
//
double tagBearingDeg = getBearing(); // + = target is to the left
if (tagBearingDeg != 1000.0 && Math.abs(tagBearingDeg) > cameraBearingEqual) {
// Slowly learn turret offset (persistent calibration)
offset -= tagBearingDeg * errorLearningRate;
}
turretAngleDeg += offsetDeg;
turretAngleDeg += offset;
double turretPos =
turrDefault + (turretAngleDeg / 180.0) * turretRange;
/* ---------------- ANGLE → SERVO ---------------- */
turretPos = Range.clip(turretPos, turrMin, turrMax);
double turretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
// Clamp to servo range
turretPos = Math.max(turrMin, Math.min(turretPos, turrMax));
robot.turr1.setPosition(turretPos);
robot.turr2.setPosition(1.0 - turretPos);
TELE.addData("Turret Angle (deg)", turretAngleDeg);
TELE.addData("Offset (deg)", offsetDeg);
TELE.addData("Tag Bearing (deg)", bearingDeg);
TELE.addData("Turret Servo", turretPos);
/* ---------------- TELEMETRY ---------------- */
TELE.addData("Turret Angle", turretAngleDeg);
TELE.addData("Bearing", tagBearingDeg);
TELE.addData("Offset", offset);
}
}