Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tune super poop #259

Merged
merged 5 commits into from
Apr 13, 2024
Merged

Tune super poop #259

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
317fd07
Add inspirational auto
suryatho Apr 13, 2024
1befc76
check style
suryatho Apr 13, 2024
b3f7c8b
Always un power arm at min angle and stow
suryatho Apr 13, 2024
71e03d7
Merge branch 'main' into enhance-super-poop-2
camearle20 Apr 13, 2024
2d1658f
Use epsilonEquals for goal check
camearle20 Apr 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -451,15 +451,15 @@ private void configureButtonBindings() {
Trigger nearSpeaker = new Trigger(robotState::inShootingZone);
driver
.a()
.and(nearSpeaker)
.and(nearSpeaker.or(shootPresets))
.whileTrue(
driveAimCommand
.get()
.alongWith(superstructureAimCommand.get(), flywheels.shootCommand())
.withName("Prepare Shot"));
driver
.a()
.and(nearSpeaker.negate())
.and(nearSpeaker.negate().and(shootPresets.negate()))
.whileTrue(
Commands.startEnd(
() ->
Expand All @@ -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(
Expand All @@ -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(
Expand Down
41 changes: 29 additions & 12 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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<Double, FlywheelSpeeds> 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
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
}

Expand Down
21 changes: 15 additions & 6 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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();
Expand All @@ -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);
Expand All @@ -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) {
Expand Down
Loading