From 8b07ed55791a70382101abc7192f54200d122c84 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 6 Jan 2026 19:52:26 -0600 Subject: [PATCH] stash --- .../ftc/teamcode/tests/ShooterTest.java | 62 +--------------- .../ftc/teamcode/utils/Flywheel.java | 73 +++++++++++-------- .../ftc/teamcode/utils/Robot.java | 4 +- 3 files changed, 50 insertions(+), 89 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 12f3663..0c250f8 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 @@ -18,28 +18,6 @@ public class ShooterTest extends LinearOpMode { public static double parameter = 0.0; // --- CONSTANTS YOU TUNE --- public static double hoodPos = 0.501; - double initPos = 0.0; - double stamp = 0.0; - double stamp1 = 0.0; - double ticker = 0.0; - double currentPos = 0.0; - double velo = 0.0; - double velo1 = 0.0; - double velo2 = 0.0; - double velo3 = 0.0; - double velo4 = 0.0; - double velo5 = 0.0; - double initPosq = 0.0; - double stampq = 0.0; - double stamp1q = 0.0; - double tickerq = 0.0; - double currentPosq = 0.0; - double veloq = 0.0; - double velo1q = 0.0; - double velo2q = 0.0; - double velo3q = 0.0; - double velo4q = 0.0; - double velo5q = 0.0; Robot robot; Flywheel flywheel; @@ -64,40 +42,8 @@ public class ShooterTest extends LinearOpMode { if (mode == 0) { rightShooter.setPower(parameter); leftShooter.setPower(parameter); - leftShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - rightShooter.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - if (ticker % 2 == 0) { - velo5 = velo4; - velo4 = velo3; - velo3 = velo2; - velo2 = velo1; - - currentPos = (double) leftShooter.getCurrentPosition() / 28; - stamp = getRuntime(); - velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1)); - initPos = currentPos; - stamp1 = stamp; - - velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5; - } - if (tickerq % 2 == 0) { - velo5q = velo4q; - velo4q = velo3q; - velo3q = velo2q; - velo2q = velo1q; - - currentPosq = (double) rightShooter.getCurrentPosition() / 28; - stampq = getRuntime(); - velo1q = 60 * ((currentPosq - initPosq) / (stampq - stamp1q)); - initPosq = currentPosq; - stamp1q = stampq; - - veloq = (velo1q + velo2q + velo3q + velo4q + velo5q) / 5; - } } else if (mode == 1) { - leftShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - rightShooter.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - double powPID = flywheel.manageFlywheel((int) parameter, robot.shooter1.getCurrentPosition()); + double powPID = flywheel.manageFlywheel1((int) parameter, leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition()); rightShooter.setPower(powPID); leftShooter.setPower(powPID); TELE.addData("PIDPower", powPID); @@ -107,9 +53,9 @@ public class ShooterTest extends LinearOpMode { robot.hood.setPosition(hoodPos); } - TELE.addData("Velocity1", velo); - TELE.addData("Velocity2", veloq); - TELE.addData("Encoder Velocity", flywheel.getVelo()); + TELE.addData("Used Velocity", flywheel.getVelo(leftShooter.getCurrentPosition(), rightShooter.getCurrentPosition())); + TELE.addData("Velocity1", flywheel.getVelo1()); + TELE.addData("Velocity2", flywheel.getVelo2()); TELE.addData("Power", robot.shooter1.getPower()); TELE.addData("Steady?", flywheel.getSteady()); TELE.addData("Position1", robot.shooter1.getCurrentPosition()/28); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java index a58c6ca..30340a3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Flywheel.java @@ -6,13 +6,17 @@ import com.acmerobotics.dashboard.config.Config; public class Flywheel { public static double kP = 0.001; // small proportional gain (tune this) public static double maxStep = 0.06; // prevents sudden jumps - double initPos = 0.0; + double initPos1 = 0.0; + double initPos2 = 0.0; double stamp = 0.0; double stamp1 = 0.0; double ticker = 0.0; - double currentPos = 0.0; + double currentPos1 = 0.0; + double currentPos2 = 0.0; double velo = 0.0; double velo1 = 0.0; + double velo1a = 0.0; + double velo1b = 0.0; double velo2 = 0.0; double velo3 = 0.0; double velo4 = 0.0; @@ -20,27 +24,12 @@ public class Flywheel { double targetVelocity = 0.0; double powPID = 0.0; boolean steady = false; - public Flywheel () { + + public Flywheel() { //robot = new Robot(hardwareMap); } - public double getVelo () { - return velo; - } - - public boolean getSteady() { - return steady; - } - - private double getTimeSeconds () - { - return (double) System.currentTimeMillis()/1000.0; - } - - - public double manageFlywheel(int commandedVelocity, double shooter1CurPos) { - targetVelocity = commandedVelocity; - + public double getVelo(double shooter1CurPos, double shooter2CurPos) { ticker++; if (ticker % 2 == 0) { velo5 = velo4; @@ -48,18 +37,46 @@ public class Flywheel { velo3 = velo2; velo2 = velo1; - currentPos = shooter1CurPos / 2048; + currentPos1 = shooter1CurPos / 28; + currentPos2 = shooter2CurPos / 28; stamp = getTimeSeconds(); //getRuntime(); - velo1 = 60 * ((currentPos - initPos) / (stamp - stamp1)); - initPos = currentPos; + velo1a = 60 * ((currentPos1 - initPos1) / (stamp - stamp1)); + velo1b = 60 * ((currentPos2 - initPos2) / (stamp - stamp1)); + initPos1 = currentPos1; stamp1 = stamp; - velo = (velo1 + velo2 + velo3 + velo4 + velo5) / 5; + if (Math.abs(velo1a - velo1b) > 200) { + if (velo1a < 200) { + velo1 = velo1b; + } else { + velo1 = velo1a; + } + } else { + velo1 = (velo1a + velo1b) / 2; + } } - // Flywheel control code here + return ((velo1 + velo2 + velo3 + velo4 + velo5) / 5); + } + + public double getVelo1() { return (velo1a + velo2 + velo3 + velo4 + velo5) / 5; } + + public double getVelo2() { return (velo1b + velo2 + velo3 + velo4 + velo5) / 5; } + + public boolean getSteady() { + return steady; + } + + private double getTimeSeconds() { + return (double) System.currentTimeMillis() / 1000.0; + } + + public double manageFlywheel1(int commandedVelocity, double shooter1CurPos, double shooter2CurPos) { + targetVelocity = commandedVelocity; + velo = getVelo(shooter1CurPos, shooter2CurPos); + // Flywheel PID code here if (targetVelocity - velo > 500) { powPID = 1.0; - } else if (velo - targetVelocity > 500){ + } else if (velo - targetVelocity > 500) { powPID = 0.0; } else { double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18; @@ -78,13 +95,11 @@ public class Flywheel { powPID = Math.max(0, Math.min(1, powPID)); } - // really should be a running average of the last 5 steady = (Math.abs(targetVelocity - velo) < 100.0); return powPID; } - public void update() - { + public void update() { } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 2562a9d..88f5278 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -35,8 +35,8 @@ public class Robot { shooter2 = hardwareMap.get(DcMotorEx.class, "s2"); shooter1.setDirection(DcMotorSimple.Direction.REVERSE); shooter2.setDirection(DcMotorSimple.Direction.REVERSE); - shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); hood = hardwareMap.get(Servo.class, "hood");