From: WesleyWong-972 Date: Sat, 18 Apr 2026 18:54:24 +0000 (-0700) Subject: new interpolation map with hood extension X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=43a6138930fac63699e046c8908131b6bd68f4d6;p=FRC2026.git new interpolation map with hood extension --- diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java index 84e86d9..c7301b7 100644 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ b/src/main/java/frc/robot/constants/ShotInterpolation.java @@ -44,25 +44,24 @@ public class ShotInterpolation { 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(2.00, 12.3); - shooterVelocityMap.put(3.00, 14.0); - shooterVelocityMap.put(4.00, 15.5); - shooterVelocityMap.put(5.00, 17.0); - shooterVelocityMap.put(5.60, 18.0); - shooterVelocityMap.put(25.0, 43.44); + shooterVelocityMap.put(0.0, 9.55 * 1.05); + shooterVelocityMap.put(1.00, 11.5 * 1.05); + shooterVelocityMap.put(2.00, 12.3 * 1.1); + shooterVelocityMap.put(3.00, 14.0 * 1.075); + shooterVelocityMap.put(4.00, 15.5 * 1.075); + shooterVelocityMap.put(5.00, 17.0 * 1.05); + shooterVelocityMap.put(5.60, 18.0 * 1.05); + shooterVelocityMap.put(25.0, 43.44 * 1.05); - newHoodMap.put(1.00, 78.0); - newHoodMap.put(1.49, 72.0); - newHoodMap.put(2.09, 70.0); - newHoodMap.put(2.95, 68.0); - newHoodMap.put(5.05, 60.0); - newHoodMap.put(5.79, 59.0); - newHoodMap.put(4.07, 65.0); newHoodMap.put(0.0, 75.9); + newHoodMap.put(1.00, 78.0); + newHoodMap.put(1.49, 72.0 - 2.0 - 1.0); + newHoodMap.put(2.09, 70.0 - 4.0 - 2.0); + newHoodMap.put(2.95, 68.0 - 4.0 - 2.0); + newHoodMap.put(4.07, 65.0 - 3.0 - 2.0); + newHoodMap.put(5.05, 60.0 - 1.0 - 1.0); + newHoodMap.put(5.79, 59.0 - 2.0); newHoodMap.put(27.99, 0.0); - } } diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java index d373bb9..12d4ab3 100644 --- a/src/main/java/frc/robot/subsystems/Intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -110,7 +110,7 @@ public class Intake extends SubsystemBase implements IntakeIO{ // config Slot 0 PID params var rollerSlot0Configs = config.Slot0; - rollerSlot0Configs.kP = 5.0; + rollerSlot0Configs.kP = 0.5; rollerSlot0Configs.kI = 0.0; rollerSlot0Configs.kD = 0.0; rollerSlot0Configs.kV = 0.0; diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index e0b8bdd..15cfe5b 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -116,6 +116,7 @@ public class Hood extends SubsystemBase implements HoodIO { goalVelocityRadPerSec = 0.0; } + double setpointRad = goalAngle.getRadians(); // calculate shortest angular delta diff --git a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java index 8d199f0..c170a0d 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodConstants.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodConstants.java @@ -12,8 +12,8 @@ public class HoodConstants { public static final double MAX_VELOCITY = 25; // rad/s public static final double MAX_ACCELERATION = 160; // rad/s^2 - public static final double MAX_ANGLE = 82; // degrees - public static final double MIN_ANGLE = 58.5; // degrees + public static final double MAX_ANGLE = 78.0; // degrees + public static final double MIN_ANGLE = 54.5; // degrees public static final double FEEDFORWARD_KV = 0.12; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ab2b34d..81eeb2a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -35,8 +35,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged(); - - LoggedNetworkNumber powerModifier = new LoggedNetworkNumber("OPERATOR: Shooter Power Modifier", 1.05); + double powerModifier = 1.00; public Shooter() { updateInputs(); @@ -83,7 +82,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { // Convert to RPS - double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)) * powerModifier.get(); + double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)) * powerModifier; if (!Constants.DISABLE_SMART_DASHBOARD) { SmartDashboard.putNumber("Target Velocity RPS", targetVelocityRPS); @@ -106,7 +105,9 @@ public class Shooter extends SubsystemBase implements ShooterIO { SmartDashboard.putBoolean("Shooter At Speed", atTargetSpeed()); SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0); } - + powerModifier = SmartDashboard.getNumber("OPERATOR: Shooter Power Modifier", powerModifier); + SmartDashboard.putNumber("OPERATOR: Shooter Power Modifier", powerModifier); + // keep this SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST"); } @@ -144,11 +145,11 @@ public class Shooter extends SubsystemBase implements ShooterIO { } public void bumpUpShooterModifier() { - powerModifier.set(powerModifier.get() + 0.025); + powerModifier += 0.025; } public void bumpDownShooterModifier() { - powerModifier.set(powerModifier.get() - 0.025); + powerModifier -= 0.025; } /**