From e7056812b4df72183888895d807f1b11b701458f Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 4 Jun 2026 17:29:14 -0500 Subject: [PATCH] shooting is ok but NOT PERFECT --- .../ftc/teamcode/teleop/TeleopV4.java | 7 +- .../ftc/teamcode/tests/NewShooterTest.java | 186 ++++++++---------- .../ftc/teamcode/utilsv2/Flywheel.java | 7 +- .../ftc/teamcode/utilsv2/Shooter.java | 58 ++++-- .../utilsv2/SpindexerTransferIntake.java | 2 + .../teamcode/utilsv2/VelocityCommander.java | 32 +-- 6 files changed, 152 insertions(+), 140 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index bb07e40..db8c8c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -55,6 +55,7 @@ public class TeleopV4 extends LinearOpMode { shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); shooter.setState(Shooter.ShooterState.TRACK_GOAL); + shooter.setRedAlliance(Color.redAlliance); spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); @@ -73,7 +74,8 @@ public class TeleopV4 extends LinearOpMode { follower.update(); - shooter.update(); + + shooter.update(robot.voltage.getVoltage()); spindexerTransferIntake.update(); SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); @@ -114,7 +116,8 @@ public class TeleopV4 extends LinearOpMode { TELE.addData("Distance From Goal", commander.getDistance()); TELE.addData("Hood Position", commander.getHoodPredicted()); TELE.addData("Transfer Power", robot.transfer.getPower()); - TELE.addData("Velocity RPM", commander.getPredictedRPM()); + TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); + TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java index ec91447..883eb44 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -1,53 +1,38 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.follower.Follower; import com.pedropathing.geometry.Pose; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.constants.ServoPositions; +import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.pedroPathing.Constants; -import org.firstinspires.ftc.teamcode.utilsv2.Flywheel; -import org.firstinspires.ftc.teamcode.utilsv2.Robot; -import org.firstinspires.ftc.teamcode.utilsv2.Shooter; -import org.firstinspires.ftc.teamcode.utilsv2.Turret; -import org.firstinspires.ftc.teamcode.utilsv2.VelocityCommander; +import org.firstinspires.ftc.teamcode.utilsv2.*; -@Config @TeleOp +@Config public class NewShooterTest extends LinearOpMode { - Robot robot; - Flywheel flywheel; - Turret turret; + Drivetrain drivetrain; Shooter shooter; MultipleTelemetry TELE; Follower follower; + SpindexerTransferIntake spindexerTransferIntake; + Turret turret; + Flywheel flywheel; VelocityCommander commander; + ParkTilter parkTilter; - - public static boolean intake = true; - public static boolean shoot = false; - public static double intakePower = 1.0; - public static double transferShootPower = -1; - public static double transferIntakePower = -1; - public static double turretPos = 0.51; - public static double hoodPos = 0.51; - public static double flywheel_velo = 0; - - public static double shooterP = 500, shooterI = 1, shooterD = 0, shooterF = 93; - - private enum ShootState { - IDLE, - WAIT_GATE, - WAIT_PUSH - } - - private ShootState shootState = ShootState.IDLE; - private long timestamp = 0; + public static int flywheelVelo = 0; + public static double hoodPos = 0.5; +// public static double turretPos = 0.51; @Override public void runOpMode() throws InterruptedException { @@ -56,105 +41,90 @@ public class NewShooterTest extends LinearOpMode { robot = Robot.getInstance(hardwareMap); - TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + TELE = new MultipleTelemetry( + FtcDashboard.getInstance().getTelemetry(), telemetry + ); + commander = new VelocityCommander(); + drivetrain = new Drivetrain(robot, TELE); follower = Constants.createFollower(hardwareMap); - follower.setStartingPose(new Pose(72, 72, 0)); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); flywheel = new Flywheel(robot); turret = new Turret(robot); - commander = new VelocityCommander(); - shooter = new Shooter( - robot, - TELE, - follower, - true, - turret, - flywheel, - commander - ); + parkTilter = new ParkTilter(robot); - shooter.setState(Shooter.ShooterState.MANUAL); + shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel, commander); + shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); + shooter.setRedAlliance(Color.redAlliance); + spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); waitForStart(); if (isStopRequested()) return; while (opModeIsActive()) { + //Drivetrain + drivetrain.drive( + -gamepad1.right_stick_y, + gamepad1.right_stick_x, + gamepad1.left_stick_x + ); follower.update(); + shooter.setFlywheelVelocity(flywheelVelo); robot.setHoodPos(hoodPos); - shooter.setTurretPosition(turretPos); - shooter.setFlywheelVelocity(flywheel_velo); - double voltage = robot.voltage.getVoltage(); - flywheel.setPIDF(shooterP, shooterI, shooterD, shooterF / voltage); +// shooter.setTurretPosition(turretPos); + shooter.update(robot.voltage.getVoltage()); + spindexerTransferIntake.update(); - robot.setSpinPos(ServoPositions.spindexer_A2); + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); - if (intake && !shoot) { + if (gamepad1.leftBumperWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || + state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_IN || + state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { - shootState = ShootState.IDLE; - - robot.setRapidFireBlockerPos( - ServoPositions.rapidFireBlocker_Closed); - - robot.setTransferPower(transferIntakePower); - robot.setIntakePower(intakePower); - robot.setTransferServoPos( - ServoPositions.transferServo_out); - } else if (shoot) { - robot.setIntakePower(intakePower); - - - switch (shootState) { - - case IDLE: - - robot.setTransferPower(transferShootPower); - - timestamp = System.currentTimeMillis(); - shootState = ShootState.WAIT_GATE; - - break; - - case WAIT_GATE: - - if (System.currentTimeMillis() - timestamp >= 300) { - - robot.setRapidFireBlockerPos( - ServoPositions.rapidFireBlocker_Open); - - timestamp = System.currentTimeMillis(); - shootState = ShootState.WAIT_PUSH; - } - - break; - - case WAIT_PUSH: - - if (System.currentTimeMillis() - timestamp >= 100) { - - robot.setTransferServoPos( - ServoPositions.transferServo_in); - - shootState = ShootState.IDLE; - } - - break; - } + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); } - TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28); - TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28); - TELE.addData("Flywheel Average Velocity", flywheel.getAverageVelocity()); - TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF); - TELE.addData("Power", flywheel.getShooterPower()); - TELE.addData("Distance", shooter.getDistance()); - TELE.update(); + if (gamepad1.right_trigger > 0.5 && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { - shooter.update(); + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + if (gamepad1.rightBumperWasPressed() + && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.INTAKE + ); + } + + if (gamepad1.dpad_down){ + parkTilter.park(); + } else if (gamepad1.dpad_up) { + parkTilter.unpark(); + } + + TELE.addData("Distance From Goal", commander.getDistance()); + TELE.addData("Hood Position", commander.getHoodPredicted()); + TELE.addData("Transfer Power", robot.transfer.getPower()); + TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); + TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); + + TELE.update(); } + } -} \ No newline at end of file +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index a8e8d64..c4ef6b7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -9,6 +10,7 @@ import org.firstinspires.ftc.teamcode.utilsv2.Robot; import java.util.LinkedList; +@Config public class Flywheel { Robot robot; @@ -70,8 +72,9 @@ public class Flywheel { private double prevF = 0; - public static double voltagePIDFDifference = 0.8; - public void setF(double f){ + public static double voltagePIDFDifference = 1; + public void setF(double voltage){ + double f = shooterPIDF_F / voltage; if (Math.abs(prevF - f) > voltagePIDFDifference) { shooterPIDF.f = f; robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 9f76ae4..a1d0acc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -1,9 +1,12 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import static org.firstinspires.ftc.teamcode.utilsv2.Flywheel.*; + +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.follower.Follower; - +@Config public class Shooter { Robot robot; @@ -15,6 +18,8 @@ public class Shooter { double goalY = 0.0; double obeliskX = 72; double obeliskY = 144; + double turretGoalX = 0; + double turretGoalY = 0; private boolean red = true; @@ -37,10 +42,13 @@ public class Shooter { if (this.red) { goalX = 144; + turretGoalX = 136; } else { goalX = 0; + turretGoalX = 8; } goalY = 144; + turretGoalY = 136; } private double flywheelVelocity = 0.0; @@ -75,7 +83,7 @@ public class Shooter { } private final double shooterDistFromCenter = 1.545; - public void update() { + public void update(double voltage) { switch (state) { case NOTHING: @@ -87,16 +95,18 @@ public class Shooter { follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), - follow.getAcceleration().getYComponent() + follow.getAcceleration().getYComponent(), + voltage ); fly.manageFlywheel(flywheelVelocity); + fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage); turr.manual(turretPosition); break; case TRACK_GOAL: turr.trackGoal( - (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), - (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), + (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), @@ -105,17 +115,21 @@ public class Shooter { follow.getAcceleration().getYComponent() ); - flywheelVelocity = commander.getVeloPredictive( + commander.getVeloPredictive( (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), - follow.getAcceleration().getYComponent() + follow.getAcceleration().getYComponent(), + voltage ); + flywheelVelocity = commander.getPredictedRPM(); + robot.setHoodPos(commander.getHoodPredicted()); fly.manageFlywheel(flywheelVelocity); + fly.setF(voltage); break; case READ_OBELISK: turr.trackObelisk( @@ -124,28 +138,35 @@ public class Shooter { follow.getHeading() ); - flywheelVelocity = commander.getVeloPredictive( + commander.getVeloPredictive( (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), - follow.getAcceleration().getYComponent() + follow.getAcceleration().getYComponent(), + voltage ); + flywheelVelocity = commander.getPredictedRPM(); + fly.manageFlywheel(flywheelVelocity); + fly.setF(voltage); break; case MANUAL_TURRET_TRACK_FLY: turr.manual(turretPosition); - flywheelVelocity = commander.getVeloPredictive( + commander.getVeloPredictive( (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getVelocity().getXComponent(), follow.getAcceleration().getXComponent(), follow.getVelocity().getYComponent(), - follow.getAcceleration().getYComponent() + follow.getAcceleration().getYComponent(), + voltage ); + + flywheelVelocity = commander.getPredictedRPM(); robot.setHoodPos(commander.getHoodPredicted()); fly.manageFlywheel(flywheelVelocity); @@ -153,8 +174,8 @@ public class Shooter { case MANUAL_FLYWHEEL_TRACK_TURR: turr.trackGoal( - (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), - (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), + (turretGoalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (turretGoalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), follow.getHeading(), follow.getAngularVelocity(), follow.getVelocity().getXComponent(), @@ -162,7 +183,18 @@ public class Shooter { follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent() ); + commander.getVeloPredictive( + (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), + (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent(), + voltage + ); fly.manageFlywheel(flywheelVelocity); + fly.setPIDF(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F / voltage); + fly.setF(voltage); break; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java index 8f5109d..e619532 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -2,11 +2,13 @@ package org.firstinspires.ftc.teamcode.utilsv2; import android.health.connect.datatypes.units.Velocity; +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.constants.ServoPositions; +@Config public class SpindexerTransferIntake { private final Robot robot; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java index 5d99897..790c3c2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -1,14 +1,16 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import com.acmerobotics.dashboard.config.Config; + +@Config public class VelocityCommander { - private final double goalH = 20.0; //TODO: Tune - private final double xVelK = 0; // TODO: Tune - private final double xAccK = 0; // TODO: Tune - private final double yVelK = 0; // TODO: Tune - private final double yAccK = 0; // TODO: Tune + public static double xVelK = 0.05; // TODO: Tune + public static double xAccK = 0.025; // TODO: Tune + public static double yVelK = 0.05; // TODO: Tune + public static double yAccK = 0.025; // TODO: Tune private double hoodPos = 0.88; private double transferPow = -1; - private double veloRPM = 2000; + private double velo = 0; public VelocityCommander() {} @@ -24,7 +26,6 @@ public class VelocityCommander { private final double veloJ = 272005.7124; private final double veloK = -2474581.713; private double distToRPM (double dist){ - double velo = 0; if (dist < 49) { velo = 2000; } else if (dist > 165){ @@ -78,7 +79,7 @@ public class VelocityCommander { return hoodPos; } - private void distToTransferPow(double dist){ + private void distToTransferPow(double dist, double voltage){ if (dist < 118){ transferPow = -1; } else if (dist < 125){ @@ -86,6 +87,8 @@ public class VelocityCommander { } else { transferPow = -0.5; } + +// transferPow = Math.max(-0.5, Math.min(-1, transferPow * (14/voltage))); } public double getTransferPow(){return transferPow;} @@ -94,22 +97,21 @@ public class VelocityCommander { return distToRPM(distance); } - private final double goalHeight = 28; double predictedDist = 0; - public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) { + public void getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc, double voltage) { double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot - predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight); + double goalHeight = 28; + predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight * goalHeight); distToHood(predictedDist); - distToTransferPow(predictedDist); - veloRPM = distToRPM(predictedDist); - return veloRPM; + distToTransferPow(predictedDist, voltage); + distToRPM(predictedDist); } - public double getPredictedRPM(){return veloRPM;} + public double getPredictedRPM(){return velo;} public double getDistance(){return predictedDist;} }