Arkm/Ftc-12926
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

staged

Browse Source
This commit is contained in:
Arkm20
2026-03-08 11:03:01 -05:00
parent 4d934dee5d
commit 2f04fc0aea
3 changed files with 92 additions and 452 deletions
Download Patch File Download Diff File Expand all files Collapse all files

84
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java Executable file → Normal file
View File

@@ -1,84 +1,4 @@
package org.firstinspires.ftc.teamcode.tuning;
import com.bylazar.configurables.annotations.Configurable;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
// Make sure your import points to your actual Constants file
import org.firstinspires.ftc.teamcode.teleOp.Constants;
@Configurable
@TeleOp
public class PIDFTuning extends OpMode {
private Limelight3A limelight;
public DcMotorEx flywheelMotorR;
public DcMotorEx flywheelMotorL;
public Servo hoodServo;
public static double curTargetVelocity = 0;
public static double targetPOS = 0;
public static int pipeline = 1;
double distance = 0;
@Override
public void init() {
flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
flywheelMotorL = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
flywheelMotorR.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE);
flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
hoodServo = hardwareMap.servo.get("hoodServo");
hoodServo.setDirection(Servo.Direction.REVERSE);
limelight = hardwareMap.get(Limelight3A.class, "limelight");
limelight.pipelineSwitch(pipeline);
limelight.start();
telemetry.addLine("Init complete");
}
@Override
public void loop() {
limelight.pipelineSwitch(pipeline);
LLResult result = limelight.getLatestResult();
if (result.isValid()) {
double targetY = result.getTy();
telemetry.addData("Ty", targetY);
distance = getTrigBasedDistance(targetY);
}
if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
flywheelMotorL.setVelocity(curTargetVelocity);}
hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
telemetry.addData("Distance", distance);
telemetry.update();
}
private double getTrigBasedDistance(double targetOffsetAngleVertical) {
return (14) / Math.tan(Math.toRadians(2.0 + targetOffsetAngleVertical));
}
}
public class PIDFTuning {
}

370
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java
View File

@@ -1,370 +0,0 @@
package org.firstinspires.ftc.teamcode.tuning;
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_HEIGHT_INCHES;
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_MOUNT_ANGLE_DEGREES;
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.GOAL_HEIGHT_INCHES;
import com.bylazar.configurables.annotations.Configurable;
import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;
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;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import com.qualcomm.hardware.lynx.LynxModule;
import java.util.List;
@Configurable
@TeleOp(name = "Flywheel Testing", group = "All")
public class flywheelTesting extends LinearOpMode {
// --- PID Constants ---
public static double F = 15.6;
public static double P = 22.5;
public static double D = 0.001;
// Turret PID
public static double kP_LL = 0.02;
public static double kP_ODO = 0.035;
public static double kI = 0.0;
public static double kD_LL = 0.001;
public static double kD_ODO = 0.0007;
public static double MAX_POWER = 0.5;
public static double MAX_INTEGRAL = 0.5;
public static double TICKS_PER_REV_MOTOR = 384.5;
public static double EXTERNAL_GEAR_RATIO = 1.315994798439532;
// FIX: Static Motor Direction & Logic Multiplier
public static boolean MOTOR_REVERSED = false;
public static double LL_LOGIC_MULTIPLIER = -1.0;
public static double LL_TARGET_OFFSET_DEGREES = 4;
public static double RED_TARGET_OFFSET_DEGREES = 14;
public static double BLUE_TARGET_OFFSET_DEGREES = 17;
private double TARGET_OFFSET_DEGREES = BLUE_TARGET_OFFSET_DEGREES;
public static double SOFT_LIMIT_NEG = -250.0;
public static double SOFT_LIMIT_POS = 250.0;
public static double WRAP_THRESHOLD_DEGREES = 180.0;
// --- Goal Coordinates for Odometry ---
public static double GOAL_RED_X = 138;
public static double GOAL_RED_Y = 138;
public static double GOAL_BLUE_X = 6;
public static double GOAL_BLUE_Y = 138;
private double targetTicks = 0;
private double ticksPerDegree = 0;
// --- Hardware Constants ---
public static double SERVO_MIN = 0.4;
public static double SERVO_MAX = 1.0;
public static double OFFSET_GAIN = -0.0005;
private Limelight3A limelight;
private DcMotorEx turretRotation;
private DcMotorEx leftTurret, rightTurret;
private Servo hoodServo;
private Follower follower;
private TelemetryManager telemetryM;
private double integralSum = 0;
private double lastError = 0;
private boolean launch = false;
private int pipelineIndex = 1;
private final ElapsedTime loopTimer = new ElapsedTime();
private final ElapsedTime limelightFailureTimer = new ElapsedTime();
private double loopTimeSum = 0;
private int loopCount = 0;
private enum Alliance { RED, BLUE }
private Alliance currentAlliance = Alliance.BLUE;
private enum TrackingSource { LIMELIGHT, ODOMETRY }
private TrackingSource activeSource = TrackingSource.ODOMETRY;
private TrackingSource lastActiveSource = TrackingSource.ODOMETRY;
List<LynxModule> allHubs;
public static double start_x = 72;
public static double start_y = 8.5;
public static double start_heading = 90;
@Override
public void runOpMode() {
telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
}
// --- Hardware Map ---
turretRotation = hardwareMap.get(DcMotorEx.class, "turretRotation");
// STATIC DIRECTION CONFIGURATION
turretRotation.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
turretRotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
turretRotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
turretRotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
leftTurret = hardwareMap.get(DcMotorEx.class, "lturret");
rightTurret = hardwareMap.get(DcMotorEx.class, "rturret");
hoodServo = hardwareMap.get(Servo.class, "hoodServo");
// PedroPathing Follower Setup
follower = Constants.createFollower(hardwareMap);
follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
limelight = hardwareMap.get(Limelight3A.class, "limelight");
leftTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rightTurret.setDirection(DcMotorSimple.Direction.REVERSE);
PIDFCoefficients pidfCoefficients = new PIDFCoefficients(P, 0, D, F);
rightTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
leftTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
recalculateTicksPerDegree();
limelight.pipelineSwitch(pipelineIndex);
limelight.start();
telemetryM.debug("Status", "Initialized");
telemetryM.update(telemetry);
waitForStart();
loopTimer.reset();
limelightFailureTimer.reset();
follower.startTeleopDrive(true);
follower.update();
loopTimeSum = 0;
loopCount = 0;
while (opModeIsActive()) {
update();
}
limelight.stop();
}
private void update() {
for (LynxModule hub : allHubs) {
hub.clearBulkCache();
}
follower.update();
double dt = loopTimer.seconds();
loopTimer.reset();
loopTimeSum += dt;
loopCount++;
if (dt < 0.001) dt = 0.001;
// Pass movement to follower for pose updates (Robot Centric)
follower.setTeleOpDrive(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x,
true
);
if (gamepad1.startWasPressed()) {
pipelineIndex = (pipelineIndex + 1) % 3;
limelight.pipelineSwitch(pipelineIndex);
}
if (gamepad1.psWasPressed()) {
currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
}
if (gamepad1.yWasPressed()) launch = !launch;
// --- TURRET TRACKING LOGIC ---
if (currentAlliance == Alliance.BLUE) limelight.pipelineSwitch(1);
else limelight.pipelineSwitch(2);
LLResult result = limelight.getLatestResult();
double rawTicks = turretRotation.getCurrentPosition();
double currentDegrees = rawTicks / ticksPerDegree;
double calculatedTargetTicks = Double.NaN;
// Hybrid Logic
calculatedTargetTicks = calculateLimelightTargetTicks(result, currentDegrees, rawTicks);
if (Double.isNaN(calculatedTargetTicks)) {
if (limelightFailureTimer.seconds() > 0.5) {
calculatedTargetTicks = calculateOdometryTargetTicks();
activeSource = TrackingSource.ODOMETRY;
} else {
calculatedTargetTicks = targetTicks;
}
} else {
limelightFailureTimer.reset();
activeSource = TrackingSource.LIMELIGHT;
}
if (!Double.isNaN(calculatedTargetTicks)) {
targetTicks = calculatedTargetTicks;
}
if (activeSource != lastActiveSource) {
integralSum = 0;
lastError = 0;
}
lastActiveSource = activeSource;
double kP_use = (activeSource == TrackingSource.ODOMETRY) ? kP_ODO : kP_LL;
double kD_use = (activeSource == TrackingSource.ODOMETRY) ? kD_ODO : kD_LL;
double error = targetTicks - rawTicks;
double derivative = (error - lastError) / dt;
if (Math.abs(error) < (15 * ticksPerDegree)) {
integralSum += (error * dt);
} else {
integralSum = 0;
}
double integralTerm = kI * integralSum;
if (Math.abs(integralTerm) > MAX_INTEGRAL) {
integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
}
double output = (kP_use * error) + integralTerm + (kD_use * derivative);
output = Range.clip(output, -MAX_POWER, MAX_POWER);
turretRotation.setPower(output);
lastError = error;
// --- SHOOTER CALCULATIONS (Using TY) ---
double calculatedDistance = 0;
double calculatedVelocity = 0;
double calculatedHoodPos = 0;
if (result != null && result.isValid()) {
double ty = result.getTy();
calculatedDistance = getDistanceFromTag(ty);
calculatedVelocity = getFlywheelVelocity(calculatedDistance);
calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity);
if (launch) {
leftTurret.setVelocity(calculatedVelocity);
rightTurret.setVelocity(calculatedVelocity);
hoodServo.setPosition(calculatedHoodPos);
}
}
if (launch) {
if (leftTurret.getVelocity() * 0.9 <= calculatedVelocity) {
// unlockTurret();
}
}
// Telemetry
telemetryM.debug("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
telemetryM.debug("Source", activeSource);
telemetryM.debug("Turret Angle", currentDegrees);
telemetryM.debug("Hood Target", calculatedHoodPos);
telemetryM.debug("Distance", calculatedDistance);
telemetryM.debug("Velocity", calculatedVelocity);
if (loopCount > 0) {
telemetryM.debug("▰▰▰▰▰▰▰ FPS ▰▰▰▰▰▰▰");
telemetryM.debug("FPS", "%.2f", 1000 / (dt * 1000));
}
telemetryM.update(telemetry);
}
private double calculateLimelightTargetTicks(LLResult result, double currentDegrees, double currentTicks) {
if (result != null && result.isValid()) {
double tx = result.getTx();
double calculatedTargetAngle = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
TARGET_OFFSET_DEGREES = LL_TARGET_OFFSET_DEGREES;
double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
double wrapLimitTicks = WRAP_THRESHOLD_DEGREES * ticksPerDegree;
if (potentialTargetTicks < -wrapLimitTicks) {
potentialTargetTicks = SOFT_LIMIT_POS;
} else if (potentialTargetTicks > wrapLimitTicks) {
potentialTargetTicks = SOFT_LIMIT_NEG;
}
return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
}
return Double.NaN;
}
private double calculateOdometryTargetTicks() {
Pose robotPose = follower.getPose();
double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
double dx = targetX - robotPose.getX();
double dy = targetY - robotPose.getY();
double targetFieldHeading = Math.atan2(dy, dx);
double robotHeading = robotPose.getHeading();
TARGET_OFFSET_DEGREES = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
double relativeRad = targetFieldHeading - robotHeading;
while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
double calculatedTargetAngle = Math.toDegrees(relativeRad);
double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
}
private void recalculateTicksPerDegree() {
ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
}
private double getDistanceFromTag(double x) {
// Uses Trigonometry (TY) instead of Area (TA)
return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + x));
}
private double calculateHoodOffset(double target) {
return (target - leftTurret.getVelocity()) * OFFSET_GAIN;
}
private double getFlywheelVelocity(double dist) {
return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
}
private double getHoodPOS(double dist, double targetVelocity) {
return Range.clip(((-(5.15893 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000954719 * Math.pow(dist, 3) - (0.000349083 * Math.pow(dist, 2)) + (-0.0153896 * dist) + 1.37618 + calculateHoodOffset(targetVelocity)), SERVO_MIN, SERVO_MAX);
}
}

90
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java Executable file
View File

@@ -0,0 +1,90 @@
package org.firstinspires.ftc.teamcode.tuning;
import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_MOUNT_ANGLE_DEGREES;
import com.bylazar.configurables.annotations.Configurable;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.Range;
// Make sure your import points to your actual Constants file
import org.firstinspires.ftc.teamcode.teleOp.Constants;
@Configurable
@TeleOp
public class PIDFTuning extends OpMode {
private Limelight3A limelight;
public DcMotorEx flywheelMotorR;
public DcMotorEx flywheelMotorL;
public Servo hoodServo;
public static double curTargetVelocity = 0;
public static double targetPOS = 0;
public static int pipeline = 1;
public static boolean reverse = false;
double distance = 0;
@Override
public void init() {
flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
flywheelMotorL = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
flywheelMotorR.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
flywheelMotorR.setDirection(reverse ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
flywheelMotorL.setDirection(reverse ? DcMotorSimple.Direction.FORWARD : DcMotorSimple.Direction.REVERSE);
hoodServo = hardwareMap.servo.get("hoodservo");
hoodServo.setDirection(Servo.Direction.REVERSE);
limelight = hardwareMap.get(Limelight3A.class, "limelight");
limelight.pipelineSwitch(pipeline);
limelight.start();
telemetry.addLine("Init complete");
}
@Override
public void loop() {
limelight.pipelineSwitch(pipeline);
LLResult result = limelight.getLatestResult();
if (result.isValid()) {
double targetY = result.getTy();
telemetry.addData("Ty", targetY);
distance = getTrigBasedDistance(targetY);
}
if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
flywheelMotorL.setVelocity(curTargetVelocity);}
hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
telemetry.addData("Distance", distance);
telemetry.update();
}
private double getTrigBasedDistance(double targetOffsetAngleVertical) {
return (17.1) / Math.tan(Math.toRadians(7.9 + targetOffsetAngleVertical));
// ^ GOAL - CAMERA HEIGHT ^ Offset Degrees
}
}