From 317fd0747dbc0350519620e8c7157254ba952e93 Mon Sep 17 00:00:00 2001 From: suryatho Date: Fri, 12 Apr 2024 22:53:17 -0400 Subject: [PATCH 1/4] Add inspirational auto --- .../frc2024/RobotContainer.java | 8 ++-- .../littletonrobotics/frc2024/RobotState.java | 42 +++++++++++++------ .../subsystems/superstructure/arm/Arm.java | 25 +++++++---- 3 files changed, 52 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 5edb6e04..8b98478b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -461,7 +461,7 @@ private void configureButtonBindings() { Trigger nearSpeaker = new Trigger(robotState::inShootingZone); driver .a() - .and(nearSpeaker) + .and(nearSpeaker.or(shootPresets)) .whileTrue( driveAimCommand .get() @@ -469,7 +469,7 @@ private void configureButtonBindings() { .withName("Prepare Shot")); driver .a() - .and(nearSpeaker.negate()) + .and(nearSpeaker.negate().and(shootPresets.negate())) .whileTrue( Commands.startEnd( () -> @@ -486,7 +486,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( @@ -498,7 +498,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..916f2161 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,26 @@ 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..7c7bf580 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; @@ -149,8 +151,12 @@ public void setOverrides(BooleanSupplier disableOverride, BooleanSupplier coastO coastSupplier = coastOverride; } + private boolean shouldPartialStow() { + return (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()); + } + private double getStowAngle() { - if (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()) { + if (shouldPartialStow()) { return MathUtil.clamp( setpointState.position, minAngle.getRadians(), @@ -202,7 +208,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 +223,20 @@ 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 && !shouldPartialStow() && 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 +248,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) { From 1befc761e3f981a7cae47f90a428c5cb27b8b730 Mon Sep 17 00:00:00 2001 From: suryatho Date: Sat, 13 Apr 2024 01:20:32 -0400 Subject: [PATCH 2/4] check style --- src/main/java/org/littletonrobotics/frc2024/RobotState.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 916f2161..2c28467f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -304,8 +304,7 @@ public AimingParameters getSuperPoopAimingParameters() { Translation2d vehicleVelocity = new Translation2d(robotVelocity.dx, robotVelocity.dy) .rotateBy(predictedRobotToTarget.getAngle().unaryMinus()); - Logger.recordOutput( - "RobotState/SuperPoopParameters/RadialVelocity", vehicleVelocity.getX()); + Logger.recordOutput("RobotState/SuperPoopParameters/RadialVelocity", vehicleVelocity.getX()); double radialVelocity = Units.radiansPerSecondToRotationsPerMinute( vehicleVelocity.getX() / Units.inchesToMeters(1.5)) From b3f7c8b7c9e86cfa81737f940b09da4d2669d920 Mon Sep 17 00:00:00 2001 From: suryatho Date: Sat, 13 Apr 2024 13:46:42 -0400 Subject: [PATCH 3/4] Always un power arm at min angle and stow --- .../frc2024/subsystems/superstructure/arm/Arm.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) 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 7c7bf580..444af6f8 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 @@ -151,12 +151,8 @@ public void setOverrides(BooleanSupplier disableOverride, BooleanSupplier coastO coastSupplier = coastOverride; } - private boolean shouldPartialStow() { - return (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()); - } - private double getStowAngle() { - if (shouldPartialStow()) { + if (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()) { return MathUtil.clamp( setpointState.position, minAngle.getRadians(), @@ -223,7 +219,7 @@ public void periodic() { Units.degreesToRadians(lowerLimitDegrees.get()), Units.degreesToRadians(upperLimitDegrees.get())), 0.0)); - if (goal == Goal.STOW && !shouldPartialStow() && atGoal()) { + if (goal == Goal.STOW && goalAngle == minAngle.getRadians() && atGoal()) { io.stop(); } else { io.runSetpoint( From 2d1658f1db525c30862e8812007afcf484d90be5 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 13 Apr 2024 16:36:59 -0400 Subject: [PATCH 4/4] Use epsilonEquals for goal check --- .../frc2024/subsystems/superstructure/arm/Arm.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 444af6f8..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 @@ -219,7 +219,9 @@ public void periodic() { Units.degreesToRadians(lowerLimitDegrees.get()), Units.degreesToRadians(upperLimitDegrees.get())), 0.0)); - if (goal == Goal.STOW && goalAngle == minAngle.getRadians() && atGoal()) { + if (goal == Goal.STOW + && EqualsUtil.epsilonEquals(goalAngle, minAngle.getRadians()) + && atGoal()) { io.stop(); } else { io.runSetpoint(