From deefa19be473322fb0fe8b6332c103a702f679c2 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 4 Jun 2026 15:18:08 -0500 Subject: [PATCH] added regression --- .../ftc/teamcode/teleop/TeleopV4.java | 5 +- .../ftc/teamcode/tests/NewShooterTest.java | 2 +- .../ftc/teamcode/utilsv2/Flywheel.java | 6 +- .../ftc/teamcode/utilsv2/Robot.java | 2 +- .../ftc/teamcode/utilsv2/Shooter.java | 3 +- .../utilsv2/SpindexerTransferIntake.java | 14 ++- .../teamcode/utilsv2/VelocityCommander.java | 87 +++++++++++++++++-- 7 files changed, 102 insertions(+), 17 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 249d594..f030251 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 @@ -27,6 +27,7 @@ public class TeleopV4 extends LinearOpMode { SpindexerTransferIntake spindexerTransferIntake; Turret turret; Flywheel flywheel; + VelocityCommander commander; ParkTilter parkTilter; @@ -41,6 +42,7 @@ public class TeleopV4 extends LinearOpMode { FtcDashboard.getInstance().getTelemetry(), telemetry ); + commander = new VelocityCommander(); drivetrain = new Drivetrain(robot, TELE); follower = Constants.createFollower(hardwareMap); Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); @@ -51,10 +53,9 @@ public class TeleopV4 extends LinearOpMode { parkTilter = new ParkTilter(robot); - shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel); shooter.setState(Shooter.ShooterState.TRACK_GOAL); - spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE); + spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE, commander); spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); 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 702840b..a9e6519 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 @@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode { public static double hoodPos = 0.51; public static double flywheel_velo = 0; - public static double shooterP = 255, shooterI = 0, shooterD = 0, shooterF = 75; + public static double shooterP = 500, shooterI = 1, shooterD = 0, shooterF = 93; private enum ShootState { IDLE, 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 ec5a462..a8e8d64 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 @@ -14,10 +14,10 @@ public class Flywheel { // public PIDFCoefficients shooterPIDF1, shooterPIDF2; public static PIDFCoefficients shooterPIDF; - public static double shooterPIDF_P = 255; - public static double shooterPIDF_I = 0.0; + public static double shooterPIDF_P = 500; + public static double shooterPIDF_I = 1; public static double shooterPIDF_D = 0.0; - public static double shooterPIDF_F = 75; + public static double shooterPIDF_F = 93; private double velo = 0.0; private double velo1 = 0.0; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java index 7cbedc0..cabc895 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -169,7 +169,7 @@ public class Robot { // Voids below are used to minimize hardware calls to minimize loop times // Used to cut off digits that are negligible - private final int maxDigits = 5; + private final int maxDigits = 3; private final int roundingFactor = (int) Math.pow(10, maxDigits); private double prevFrontLeftPower = -10.501; 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 e68ff91..6849d11 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 @@ -81,7 +81,6 @@ public class Shooter { case NOTHING: break; case MANUAL: - commander.getVeloPredictive( (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())), (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())), @@ -115,6 +114,7 @@ public class Shooter { follow.getAcceleration().getYComponent() ); + robot.setHoodPos(commander.getHoodPredicted()); fly.manageFlywheel(flywheelVelocity); break; case READ_OBELISK: @@ -146,6 +146,7 @@ public class Shooter { follow.getVelocity().getYComponent(), follow.getAcceleration().getYComponent() ); + robot.setHoodPos(commander.getHoodPredicted()); fly.manageFlywheel(flywheelVelocity); 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 5896484..ed1e318 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 @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.utilsv2; +import android.health.connect.datatypes.units.Velocity; + import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -9,8 +11,11 @@ public class SpindexerTransferIntake { private final Robot robot; - public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) { + VelocityCommander commander; + + public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE, VelocityCommander com) { this.robot = rob; + this.commander = com; } private final double sensorDistanceThreshold = 6.0; @@ -61,6 +66,10 @@ public class SpindexerTransferIntake { public void update() { + if (mode == SpindexerMode.RAPID && rapidMode == RapidMode.INTAKE){ + robot.setTransferPower(-0.7); + } + switch (mode) { case RAPID: @@ -74,7 +83,6 @@ public class SpindexerTransferIntake { case INTAKE: robot.setIntakePower(1); - robot.setTransferPower(-0.7); robot.setRapidFireBlockerPos( ServoPositions.rapidFireBlocker_Closed ); @@ -95,8 +103,6 @@ public class SpindexerTransferIntake { case TRANSFER_OFF: - robot.setTransferPower(-0.7); - if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) { setRapidMode(RapidMode.BEFORE_PULSE_OUT); } 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 6490e2f..c2484c8 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 @@ -6,15 +6,90 @@ public class VelocityCommander { private final double xAccK = 0; // TODO: Tune private final double yVelK = 0; // TODO: Tune private final double yAccK = 0; // TODO: Tune + private double hoodPos = 0.88; + private double transferPow = -1; - public VelocityCommander() { - - } + public VelocityCommander() {} + private final double veloA = -2.703087757*Math.pow(10, -14); + private final double veloB = 2.904756341*Math.pow(10, -11); + private final double veloC = -1.381814293*Math.pow(10, -8); + private final double veloD = 0.000003829224585; + private final double veloE = -0.000684090204; + private final double veloF = 0.0822754689; + private final double veloG = -6.743119277; + private final double veloH = 371.7359504; + private final double veloI = -13189.70958; + private final double veloJ = 272005.7124; + private final double veloK = -2474581.713; private double distToRPM (double dist){ - return Math.sqrt(dist*dist + goalH*goalH) * 20; - //TODO: Add regression here using goalH + double velo = 0; + if (dist < 49) { + velo = 2000; + } else if (dist > 165){ + velo = 3760; + } else { + velo = veloA*Math.pow(dist, 10) + + veloB*Math.pow(dist, 9) + + veloC*Math.pow(dist, 8) + + veloD*Math.pow(dist, 7) + + veloE*Math.pow(dist, 6) + + veloF*Math.pow(dist, 5) + + veloG*Math.pow(dist, 4) + + veloH*Math.pow(dist, 3) + + veloI*Math.pow(dist, 2) + + veloJ*Math.pow(dist, 1) + + veloK; + velo = Math.max(2000, Math.min(3760, velo)); + } + return velo; } + + private final double hoodA = -4.3276177*Math.pow(10, -13); + private final double hoodB = 2.68062979*Math.pow(10, -11); + private final double hoodC = -7.12859632*Math.pow(10, -8); + private final double hoodD = 0.0000106010785; + private final double hoodE = -0.000960693973; + private final double hoodF = 0.0540375808; + private final double hoodG = -1.82724027; + private final double hoodH = 33.4797545; + private final double hoodI = -246.888632; + private void distToHood (double dist){ + if (dist > 112){ + hoodPos = 0.35; + } else if (dist < 49){ + hoodPos = 0.88; + } else { + hoodPos = hoodA*Math.pow(dist, 8) + + hoodB*Math.pow(dist, 7) + + hoodC*Math.pow(dist, 6) + + hoodD*Math.pow(dist, 5) + + hoodE*Math.pow(dist, 4) + + hoodF*Math.pow(dist, 3) + + hoodG*Math.pow(dist, 2) + + hoodH*Math.pow(dist, 1) + + hoodI; + + hoodPos = Math.max(0.35, Math.min(0.88, hoodPos)); + } + } + + private void distToTransferPow(double dist){ + if (dist < 118){ + transferPow = -1; + } else if (dist < 125){ + transferPow = -0.7; + } else { + transferPow = -0.5; + } + } + + public double getTransferPow(){return transferPow;} + + public double getHoodPredicted(){ + return hoodPos; + } + // 27 public double getVeloStationary (double distance){ return distToRPM(distance); @@ -29,6 +104,8 @@ public class VelocityCommander { predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight); + distToHood(predictedDist); + return distToRPM(predictedDist); }