From 2f04fc0aeac1aeae1d79ec72f96f934ace0fea58 Mon Sep 17 00:00:00 2001 From: Arkm20 Date: Sun, 8 Mar 2026 11:03:01 -0500 Subject: [PATCH] staged --- .../ftc/teamcode/tuning/PIDFTuning.java | 84 +--- .../ftc/teamcode/tuning/flywheelTesting.java | 370 ------------------ .../ftc/teamcode/tuning/flywheelTuning.java | 90 +++++ 3 files changed, 92 insertions(+), 452 deletions(-) mode change 100755 => 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java delete mode 100755 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java create mode 100755 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java old mode 100755 new mode 100644 index a0d4ea2..faff252 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java @@ -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)); - } -} \ No newline at end of file +public class PIDFTuning { +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java deleted file mode 100755 index 29886ce..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java +++ /dev/null @@ -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 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); - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java new file mode 100755 index 0000000..8c24189 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTuning.java @@ -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 + } +} \ No newline at end of file