From 88e1c428a5f9034ffaed3798a9a9d5797e9534a7 Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sat, 29 Nov 2025 14:11:37 -0600 Subject: [PATCH] Auto --- .../ftc/teamcode/tests/ShooterTest.java | 132 +++++++++++------- 1 file changed, 83 insertions(+), 49 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 960cd57..1de70ac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -26,6 +26,8 @@ public class ShooterTest extends LinearOpMode { public static String flyMode = "VEL"; + public static boolean AutoTrack = false; + double initPos = 0.0; double velo1 = 0.0; @@ -48,6 +50,7 @@ public class ShooterTest extends LinearOpMode { public static double kP = 0.0005; // small proportional gain (tune this) public static double maxStep = 0.06; // prevents sudden jumps + public static double distance = 50; MultipleTelemetry TELE; @@ -75,72 +78,103 @@ public class ShooterTest extends LinearOpMode { while (opModeIsActive()) { - shooter.setShooterMode(flyMode); - - shooter.setManualPower(pow); - - robot.hood.setPosition(hoodPos); - robot.turr1.setPosition(turretPos); - robot.turr2.setPosition(1-turretPos); - if (intake){ - robot.transfer.setPower(0); - robot.intake.setPower(0.75); - robot.spin1.setPosition(spindexer_intakePos); - robot.spin2.setPosition(1-spindexer_intakePos); - } else { - robot.transfer.setPower(1); - robot.intake.setPower(0); - if (spindexPos == 1){ - robot.spin1.setPosition(spindexer_outtakeBall1); - robot.spin2.setPosition(1-spindexer_outtakeBall1); - } else if (spindexPos == 2){ - robot.spin1.setPosition(spindexer_outtakeBall2); - robot.spin2.setPosition(1-spindexer_outtakeBall2); - } else if (spindexPos == 3){ - robot.spin1.setPosition(spindexer_outtakeBall3); - robot.spin2.setPosition(1-spindexer_outtakeBall3); - } + if (AutoTrack){ + hoodPos = hoodAnglePrediction(distance); + vel = velPrediction(distance); } - velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); - stamp1 = getRuntime(); - initPos1 = shooter.getECPRPosition(); - double feed = vel / maxVel; // Example: vel=2500 → feed=0.5 + + shooter.setShooterMode(flyMode); + + shooter.setManualPower(pow); + + robot.hood.setPosition(hoodPos); + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(1 - turretPos); + if (intake) { + robot.transfer.setPower(0); + robot.intake.setPower(0.75); + robot.spin1.setPosition(spindexer_intakePos); + robot.spin2.setPosition(1 - spindexer_intakePos); + } else { + robot.transfer.setPower(1); + robot.intake.setPower(0); + if (spindexPos == 1) { + robot.spin1.setPosition(spindexer_outtakeBall1); + robot.spin2.setPosition(1 - spindexer_outtakeBall1); + } else if (spindexPos == 2) { + robot.spin1.setPosition(spindexer_outtakeBall2); + robot.spin2.setPosition(1 - spindexer_outtakeBall2); + } else if (spindexPos == 3) { + robot.spin1.setPosition(spindexer_outtakeBall3); + robot.spin2.setPosition(1 - spindexer_outtakeBall3); + } + } + + velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); + stamp1 = getRuntime(); + initPos1 = shooter.getECPRPosition(); + + double feed = vel / maxVel; // Example: vel=2500 → feed=0.5 // --- PROPORTIONAL CORRECTION --- - double error = vel - velo1; - double correction = kP * error; + double error = vel - velo1; + double correction = kP * error; // limit how fast power changes (prevents oscillation) - correction = Math.max(-maxStep, Math.min(maxStep, correction)); + correction = Math.max(-maxStep, Math.min(maxStep, correction)); // --- FINAL MOTOR POWER --- - powPID = feed + correction; + powPID = feed + correction; // clamp to allowed range - powPID = Math.max(0, Math.min(1, powPID)); - shooter.setVelocity(powPID); + powPID = Math.max(0, Math.min(1, powPID)); + shooter.setVelocity(powPID); + if (shoot) { + robot.transferServo.setPosition(transferServo_in); + } else { + robot.transferServo.setPosition(transferServo_out); + } - if (shoot){ - robot.transferServo.setPosition(transferServo_in); - } else { - robot.transferServo.setPosition(transferServo_out); - } + shooter.update(); - shooter.update(); + TELE.addData("Revolutions", shooter.getECPRPosition()); + TELE.addData("hoodPos", shooter.gethoodPosition()); + TELE.addData("turretPos", shooter.getTurretPosition()); + TELE.addData("Power Fly 1", robot.shooter1.getPower()); + TELE.addData("Power Fly 2", robot.shooter2.getPower()); + TELE.addData("powPID", shooter.getpowPID()); + TELE.addData("Velocity", velo1); + TELE.update(); - TELE.addData("Revolutions", shooter.getECPRPosition()); - TELE.addData("hoodPos", shooter.gethoodPosition()); - TELE.addData("turretPos", shooter.getTurretPosition()); - TELE.addData("Power Fly 1", robot.shooter1.getPower()); - TELE.addData("Power Fly 2", robot.shooter2.getPower()); - TELE.addData("powPID", shooter.getpowPID()); - TELE.addData("Velocity", velo1); - TELE.update(); } } + + public double hoodAnglePrediction(double distance) { + double L = 0.298317; + double A = 1.02124; + double k = 0.0157892; + double n = 3.39375; + + double dist = Math.sqrt(distance*distance+24*24); + + return L + A * Math.exp(-Math.pow(k * dist, n)); + } + public static double velPrediction(double distance) { + + double x = Math.sqrt(distance*distance+24*24); + + + + double A = -211149.992; + double B = -1.19943; + double C = 3720.15909; + + return A * Math.pow(x, B) + C; + } + }