From e0f5a398cc68f7dd29ebbf54d12e7aa2ab8c4760 Mon Sep 17 00:00:00 2001 From: WesleyWong-972 Date: Sat, 28 Mar 2026 06:03:29 -0700 Subject: [PATCH] Shuttling implemented and map finnished --- .../robot/commands/gpm/Superstructure.java | 39 ++++++++++--------- .../robot/constants/ShotInterpolation.java | 6 +-- .../robot/constants/ShuttleInterpolation.java | 36 +++++++++++++++-- 3 files changed, 56 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index d3cce24..44fc659 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -219,33 +219,36 @@ public class Superstructure extends Command { stowEverything(); } else { turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); - - // if(phaseManager.getCurrentState() == CurrentState.UNDER_TRENCH){ - // hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); - // } else{ - //hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.hoodAngleMap.get(hoodSetpoint)), hoodVelocity); - // } - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); + boolean shuttling = target.getZ() != FieldConstants.getHubTranslation().getZ(); // if we're aiming upward we are aiming at hub, thus not shuttling + + // shuttling will move the hood so its angles very close to max (less arch) + if (shuttling) { + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); + } else { + hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); + } + + double x = drivepose.getX(); // compared as meters double y = drivepose.getY(); - System.out.println("X: " + Units.metersToInches(x) + "Y: " + Units.metersToInches(y)); // if (FieldConstants.underTrench(x, y)) { - // hood.forceHoodDown(true); // System.out.println("Hood forced down"); // } else { // hood.forceHoodDown(false); - // System.out.println("Hood forced up"); // } - shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); - Logger.recordOutput("Distance From Target", distanceFromTarget); - //shooter.setShooter(-ShotInterpolation.exitVelocityMap.get(goalState.exitVel())); - // if (phaseManager.shouldFeed()) { - // spindexer.maxSpindexer(); - // } else { - // spindexer.stopSpindexer(); - // } + // different maps for shuttling vs shooting. Less powerful when shuttling. + if (shuttling) { + shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget)); + } else { + shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); + } + + // record when shuttling + Logger.recordOutput("Shuttling", shuttling); + // record distance for tuning if needed + Logger.recordOutput("Distance From Target", distanceFromTarget); } Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint); diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java index 0c0da26..1aba33c 100644 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -43,15 +43,15 @@ public class ShotInterpolation { exitVelocityMap.put(11.0, 26.0); exitVelocityMap.put(25.0, 25.0* 3.2); + // currently regresses to y = 1.34959x + 9.79618 + shooterVelocityMap.put(0.0, 9.55); shooterVelocityMap.put(1.00, 11.5); shooterVelocityMap.put(1.49, 11.5); shooterVelocityMap.put(2.09, 12.5); shooterVelocityMap.put(2.95, 13.5); + shooterVelocityMap.put(4.07, 15.5); shooterVelocityMap.put(5.05, 16.7); shooterVelocityMap.put(5.79, 18.0); - shooterVelocityMap.put(4.07, 15.5); - - shooterVelocityMap.put(0.0, 9.55); shooterVelocityMap.put(25.0, 43.44); newHoodMap.put(1.00, 78.0); diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java index f5ad80c..1eb358b 100644 --- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java +++ b/src/main/java/frc/robot/constants/ShuttleInterpolation.java @@ -1,11 +1,39 @@ // i'm going to get so many lines of code from this :) - Wesley public class ShuttleInterpolation { - public static final IntpolationDoubleTreeMap timeOfFlightMap = new InterpolationDoubleTreeMap(); - public static final IntpolationDoubleTreeMap timeOfFlightMap = new InterpolationDoubleTreeMap(); - + public static final IntpolationDoubleTreeMap hoodAngleMap = new InterpolationDoubleTreeMap(); + public static final IntpolationDoubleTreeMap shooterVelocityMap = new InterpolationDoubleTreeMap(); + /* + guide to tuning: + modify the left values in the parenthesis to change the distance of the shot. + setup the robot at that distance and shoot. Then modifty the right value to be more or less until it hits target perfectly. + Repeat + OR + Run robot, estimate distance, and tweak values when nobodies looking until it works. + */ static { - + // we can be less aggressive: y = 0.65 * (1.34959x + 9.79618) + // will likely be this that requires tuning. + shooterVelocityMap.put(0.0, 6.2075); + shooterVelocityMap.put(1.00, 7.475); + shooterVelocityMap.put(1.49, 7.475); + shooterVelocityMap.put(2.09, 8.125); + shooterVelocityMap.put(2.95, 8.775); + shooterVelocityMap.put(4.07, 10.075); + shooterVelocityMap.put(5.05, 10.855); + shooterVelocityMap.put(5.79, 11.7); + shooterVelocityMap.put(25.0, 28.236); + + // always shoot at low angle to ground. + newHoodMap.put(0.0, 80.0); + newHoodMap.put(1.00, 80.0); + newHoodMap.put(1.49, 80.0); + newHoodMap.put(2.09, 80.0); + newHoodMap.put(2.95, 80.0); + newHoodMap.put(5.05, 80.0); + newHoodMap.put(5.79, 80.0); + newHoodMap.put(4.07, 80.0); + newHoodMap.put(27.99, 80.0); } } \ No newline at end of file -- 2.39.5