diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 3c76dc5b..fb6f84e0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -451,7 +451,7 @@ private void configureButtonBindings() { Trigger nearSpeaker = new Trigger(robotState::inShootingZone); driver .a() - .and(nearSpeaker) + .and(nearSpeaker.or(shootPresets)) .whileTrue( driveAimCommand .get() @@ -459,7 +459,7 @@ private void configureButtonBindings() { .withName("Prepare Shot")); driver .a() - .and(nearSpeaker.negate()) + .and(nearSpeaker.negate().and(shootPresets.negate())) .whileTrue( Commands.startEnd( () -> @@ -476,7 +476,7 @@ private void configureButtonBindings() { driver .rightTrigger() .and(driver.a()) - .and(nearSpeaker) + .and(nearSpeaker.or(shootPresets)) .and(readyToShoot.debounce(0.2, DebounceType.kRising)) .onTrue( Commands.parallel( @@ -488,7 +488,7 @@ private void configureButtonBindings() { driver .rightTrigger() .and(driver.a()) - .and(nearSpeaker.negate()) + .and(nearSpeaker.negate().and(shootPresets.negate())) .and(readyToShoot.debounce(0.3, DebounceType.kFalling)) .onTrue( Commands.parallel( diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index d30f4551..2c28467f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -57,7 +57,7 @@ public record AimingParameters( private static final LoggedTunableNumber lookahead = new LoggedTunableNumber("RobotState/lookaheadS", 0.35); private static final LoggedTunableNumber superPoopLookahead = - new LoggedTunableNumber("RobotState/SuperPoopLookahead", 1.0); + new LoggedTunableNumber("RobotState/SuperPoopLookahead", 0.1); private static final LoggedTunableNumber closeShootingZoneFeet = new LoggedTunableNumber("RobotState/CloseShootingZoneFeet", 10.0); private static final double poseBufferSizeSeconds = 2.0; @@ -73,18 +73,21 @@ public record AimingParameters( new InterpolatingDoubleTreeMap(); static { - superPoopArmAngleMap.put(Units.feetToMeters(30.0), 35.0); - superPoopArmAngleMap.put(Units.feetToMeters(25.0), 37.0); - superPoopArmAngleMap.put(Units.feetToMeters(22.0), 45.0); + superPoopArmAngleMap.put(Units.feetToMeters(33.52713263758169), 35.0); + superPoopArmAngleMap.put(Units.feetToMeters(28.31299227120627), 39.0); + superPoopArmAngleMap.put(Units.feetToMeters(25.587026383435525), 48.0); } private static final InterpolatingTreeMap superPoopFlywheelSpeedsMap = new InterpolatingTreeMap<>(InverseInterpolator.forDouble(), FlywheelSpeeds::interpolate); static { - superPoopFlywheelSpeedsMap.put(Units.feetToMeters(30.0), new FlywheelSpeeds(3500, 4500)); - superPoopFlywheelSpeedsMap.put(Units.feetToMeters(25.0), new FlywheelSpeeds(4100, 4100)); - superPoopFlywheelSpeedsMap.put(Units.feetToMeters(22.0), new FlywheelSpeeds(2700, 3700)); + superPoopFlywheelSpeedsMap.put( + Units.feetToMeters(33.52713263758169), new FlywheelSpeeds(3500, 4500)); + superPoopFlywheelSpeedsMap.put( + Units.feetToMeters(28.31299227120627), new FlywheelSpeeds(2800, 3500)); + superPoopFlywheelSpeedsMap.put( + Units.feetToMeters(25.587026383435525), new FlywheelSpeeds(2500, 3200)); } private static final double autoFarShotCompensationDegrees = 0.0; // 0.6 at NECMP @@ -282,7 +285,9 @@ public AimingParameters getAimingParameters() { } private static final Translation2d superPoopTarget = - FieldConstants.Stage.podiumLeg.getTranslation().interpolate(FieldConstants.ampCenter, 0.5); + FieldConstants.Subwoofer.centerFace + .getTranslation() + .interpolate(FieldConstants.ampCenter, 0.5); public AimingParameters getSuperPoopAimingParameters() { if (latestSuperPoopParameters != null) { @@ -293,13 +298,25 @@ public AimingParameters getSuperPoopAimingParameters() { Translation2d predictedRobotToTarget = AllianceFlipUtil.apply(superPoopTarget).minus(predictedFieldToRobot.getTranslation()); double effectiveDistance = predictedRobotToTarget.getNorm(); + var flywheelSpeeds = superPoopFlywheelSpeedsMap.get(effectiveDistance); + var armAngle = Rotation2d.fromDegrees(superPoopArmAngleMap.get(effectiveDistance)); + + Translation2d vehicleVelocity = + new Translation2d(robotVelocity.dx, robotVelocity.dy) + .rotateBy(predictedRobotToTarget.getAngle().unaryMinus()); + Logger.recordOutput("RobotState/SuperPoopParameters/RadialVelocity", vehicleVelocity.getX()); + double radialVelocity = + Units.radiansPerSecondToRotationsPerMinute( + vehicleVelocity.getX() / Units.inchesToMeters(1.5)) + * armAngle.getCos(); + flywheelSpeeds = + new FlywheelSpeeds( + flywheelSpeeds.leftSpeed() - radialVelocity, + flywheelSpeeds.rightSpeed() - radialVelocity); latestSuperPoopParameters = new AimingParameters( - predictedRobotToTarget.getAngle(), - Rotation2d.fromDegrees(superPoopArmAngleMap.get(effectiveDistance)), - effectiveDistance, - superPoopFlywheelSpeedsMap.get(effectiveDistance)); + predictedRobotToTarget.getAngle(), armAngle, effectiveDistance, flywheelSpeeds); return latestSuperPoopParameters; } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 5446fa70..fd0be936 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -108,6 +108,8 @@ private double getRads() { private TrapezoidProfile.Constraints currentConstraints = maxProfileConstraints.get(); private TrapezoidProfile profile; private TrapezoidProfile.State setpointState = new TrapezoidProfile.State(); + + private double goalAngle; private ArmFeedforward ff; private final ArmVisualizer measuredVisualizer; @@ -202,7 +204,7 @@ public void periodic() { // Don't run profile when characterizing, coast mode, or disabled if (!characterizing && brakeModeEnabled && !disableSupplier.getAsBoolean()) { // Run closed loop - double goalAngle = + goalAngle = goal.getRads() + (goal == Goal.AIM ? Units.degreesToRadians(currentCompensation) : 0.0); if (goal == Goal.STOW) { goalAngle = getStowAngle(); @@ -217,15 +219,22 @@ public void periodic() { Units.degreesToRadians(lowerLimitDegrees.get()), Units.degreesToRadians(upperLimitDegrees.get())), 0.0)); - io.runSetpoint( - setpointState.position, ff.calculate(setpointState.position, setpointState.velocity)); + if (goal == Goal.STOW + && EqualsUtil.epsilonEquals(goalAngle, minAngle.getRadians()) + && atGoal()) { + io.stop(); + } else { + io.runSetpoint( + setpointState.position, ff.calculate(setpointState.position, setpointState.velocity)); + } + + goalVisualizer.update(goalAngle); + Logger.recordOutput("Arm/GoalAngle", goalAngle); } // Logs measuredVisualizer.update(inputs.positionRads); setpointVisualizer.update(setpointState.position); - goalVisualizer.update(goal.getRads()); - Logger.recordOutput("Arm/GoalAngle", goal.getRads()); Logger.recordOutput("Arm/SetpointAngle", setpointState.position); Logger.recordOutput("Arm/SetpointVelocity", setpointState.velocity); Logger.recordOutput("Superstructure/Arm/Goal", goal); @@ -237,7 +246,7 @@ public void stop() { @AutoLogOutput(key = "Superstructure/Arm/AtGoal") public boolean atGoal() { - return EqualsUtil.epsilonEquals(setpointState.position, goal.getRads(), 1e-3); + return EqualsUtil.epsilonEquals(setpointState.position, goalAngle, 1e-3); } public void setBrakeMode(boolean enabled) {