Skip to content

Commit

Permalink
Enhance super poop (#255)
Browse files Browse the repository at this point in the history 
* Enhance super poop

* Tune enhanced super poop

* Fix alert

---------

Co-authored-by: suryatho <suryathoppae@gmail.com>
  • Loading branch information
@jwbonner @suryatho
jwbonner and suryatho authored Apr 12, 2024
1 parent 71fdea1 commit 00fe4e7
Show file tree
Hide file tree
Showing 5 changed files with 89 additions and 39 deletions.
16 changes: 13 additions & 3 deletions src/main/java/org/littletonrobotics/frc2024/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,6 @@

package org.littletonrobotics.frc2024;

import static org.littletonrobotics.frc2024.util.Alert.AlertType;

import com.ctre.phoenix6.CANBus;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringSubscriber;
Expand All @@ -32,6 +30,7 @@
import org.littletonrobotics.frc2024.Constants.Mode;
import org.littletonrobotics.frc2024.subsystems.leds.Leds;
import org.littletonrobotics.frc2024.util.Alert;
import org.littletonrobotics.frc2024.util.Alert.AlertType;
import org.littletonrobotics.frc2024.util.NoteVisualizer;
import org.littletonrobotics.frc2024.util.VirtualSubsystem;
import org.littletonrobotics.junction.LogFileUtil;
Expand Down Expand Up @@ -215,8 +214,19 @@ public void robotPeriodic() {
Logger.recordOutput("RobotState/AimingParameters/ArmAngle", aimingParameters.armAngle());
Logger.recordOutput(
"RobotState/AimingParameters/EffectiveDistance", aimingParameters.effectiveDistance());
var superPoopParameters = RobotState.getInstance().getSuperPoopAimingParameters();
Logger.recordOutput(
"RobotState/SuperPoopParameters/DriveHeading", superPoopParameters.driveHeading());
Logger.recordOutput("RobotState/SuperPoopParameters/ArmAngle", superPoopParameters.armAngle());
Logger.recordOutput(
"RobotState/SuperPoopParameters/EffectiveDistance",
superPoopParameters.effectiveDistance());
Logger.recordOutput(
"RobotState/SuperPoopParameters/FlywheelsLeftSpeed",
superPoopParameters.flywheelSpeeds().leftSpeed());
Logger.recordOutput(
"RobotState/AimingParameters/DriveFeedVelocity", aimingParameters.driveFeedVelocity());
"RobotState/SuperPoopParameters/FlywheelsRightSpeed",
superPoopParameters.flywheelSpeeds().rightSpeed());

// Robot container periodic methods
robotContainer.checkControllers();
Expand Down
17 changes: 5 additions & 12 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -467,34 +467,27 @@ private void configureButtonBindings() {
.get()
.alongWith(superstructureAimCommand.get(), flywheels.shootCommand())
.withName("Prepare Shot"));
Translation2d superPoopTarget =
FieldConstants.Speaker.centerSpeakerOpening
.toTranslation2d()
.interpolate(FieldConstants.ampCenter, 0.5);
driver
.a()
.and(nearSpeaker.negate())
.whileTrue(
Commands.startEnd(
() ->
drive.setHeadingGoal(
() ->
AllianceFlipUtil.apply(superPoopTarget)
.minus(robotState.getEstimatedPose().getTranslation())
.getAngle()),
() -> robotState.getSuperPoopAimingParameters().driveHeading()),
drive::clearHeadingGoal)
.alongWith(
superstructure.setGoalCommand(Superstructure.Goal.SUPER_POOP),
flywheels.superPoopCommand())
.withName("Super Poop"));
Trigger readyToShoot =
new Trigger(() -> drive.atHeadingGoal() && superstructure.atArmGoal() && flywheels.atGoal())
.debounce(0.2, DebounceType.kRising);
new Trigger(
() -> drive.atHeadingGoal() && superstructure.atArmGoal() && flywheels.atGoal());
driver
.rightTrigger()
.and(driver.a())
.and(nearSpeaker)
.and(readyToShoot)
.and(readyToShoot.debounce(0.2, DebounceType.kRising))
.onTrue(
Commands.parallel(
Commands.waitSeconds(0.5), Commands.waitUntil(driver.rightTrigger().negate()))
Expand All @@ -506,7 +499,7 @@ private void configureButtonBindings() {
.rightTrigger()
.and(driver.a())
.and(nearSpeaker.negate())
.and(readyToShoot)
.and(readyToShoot.debounce(0.3, DebounceType.kFalling))
.onTrue(
Commands.parallel(
Commands.waitSeconds(0.5), Commands.waitUntil(driver.rightTrigger().negate()))
Expand Down
85 changes: 65 additions & 20 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,9 @@

package org.littletonrobotics.frc2024;

import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.*;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.interpolation.*;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
Expand Down Expand Up @@ -40,18 +38,26 @@ public record OdometryObservation(

public record VisionObservation(Pose2d visionPose, double timestamp, Matrix<N3, N1> stdDevs) {}

public record FlywheelSpeeds(double leftSpeed, double rightSpeed) {
public static FlywheelSpeeds interpolate(FlywheelSpeeds t1, FlywheelSpeeds t2, double v) {
double leftSpeed = MathUtil.interpolate(t1.leftSpeed(), t2.leftSpeed(), v);
double rightSpeed = MathUtil.interpolate(t1.rightSpeed(), t2.rightSpeed(), v);
return new FlywheelSpeeds(leftSpeed, rightSpeed);
}
}

public record AimingParameters(
Rotation2d driveHeading,
Rotation2d armAngle,
double effectiveDistance,
double driveFeedVelocity) {}
FlywheelSpeeds flywheelSpeeds) {}

private static final LoggedTunableNumber autoLookahead =
new LoggedTunableNumber("RobotState/AutoLookahead", 0.5);
private static final LoggedTunableNumber lookahead =
new LoggedTunableNumber("RobotState/lookaheadS", 0.35);
private static final LoggedTunableNumber shootingZoneFeet =
new LoggedTunableNumber("RobotState/ShootingZoneFeet", 25.0);
private static final LoggedTunableNumber superPoopLookahead =
new LoggedTunableNumber("RobotState/SuperPoopLookahead", 1.0);
private static final LoggedTunableNumber closeShootingZoneFeet =
new LoggedTunableNumber("RobotState/CloseShootingZoneFeet", 11.0);
private static final double poseBufferSizeSeconds = 2.0;
Expand All @@ -62,6 +68,25 @@ public record AimingParameters(
@AutoLogOutput @Getter @Setter private boolean flywheelAccelerating = false;
@AutoLogOutput @Getter @Setter private double shotCompensationDegrees = 0.0;

// Super poop
private static final InterpolatingDoubleTreeMap superPoopArmAngleMap =
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);
}

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));
}

private static final double autoFarShotCompensationDegrees = 0.0; // 0.6 at NECMP

public void adjustShotCompensationDegrees(double deltaDegrees) {
Expand Down Expand Up @@ -99,6 +124,8 @@ public static RobotState getInstance() {
/** Cached latest aiming parameters. Calculated in {@code getAimingParameters()} */
private AimingParameters latestParameters = null;

private AimingParameters latestSuperPoopParameters = null;

@Setter private BooleanSupplier lookaheadDisable = () -> false;

private RobotState() {
Expand All @@ -114,6 +141,7 @@ private RobotState() {
/** Add odometry observation */
public void addOdometryObservation(OdometryObservation observation) {
latestParameters = null;
latestSuperPoopParameters = null;
Twist2d twist = kinematics.toTwist2d(lastWheelPositions, observation.wheelPositions());
lastWheelPositions = observation.wheelPositions();
// Check gyro connected
Expand All @@ -134,6 +162,7 @@ public void addOdometryObservation(OdometryObservation observation) {

public void addVisionObservation(VisionObservation observation) {
latestParameters = null;
latestSuperPoopParameters = null;
// If measurement is old enough to be outside the pose buffer's timespan, skip.
try {
if (poseBuffer.getInternalBuffer().lastKey() - poseBufferSizeSeconds
Expand Down Expand Up @@ -232,15 +261,9 @@ public AimingParameters getAimingParameters() {
Translation2d predictedVehicleFixedToTargetTranslation =
fieldToPredictedVehicleFixed.inverse().transformBy(fieldToTarget).getTranslation();

Rotation2d vehicleToGoalDirection = predictedVehicleToTargetTranslation.getAngle();

Rotation2d targetVehicleDirection = predictedVehicleFixedToTargetTranslation.getAngle();
double targetDistance = predictedVehicleToTargetTranslation.getNorm();

double feedVelocity =
robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance
- robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance;

double armAngleDegrees = armAngleCoefficient * Math.pow(targetDistance, armAngleExponent);
double autoFarArmCorrection =
DriverStation.isAutonomousEnabled() && targetDistance >= Units.inchesToMeters(125)
Expand All @@ -254,23 +277,45 @@ public AimingParameters getAimingParameters() {
Rotation2d.fromDegrees(
armAngleDegrees + shotCompensationDegrees + autoFarArmCorrection),
targetDistance,
feedVelocity);
new FlywheelSpeeds(0, 0));
return latestParameters;
}

private static final Translation2d superPoopTarget =
FieldConstants.Stage.podiumLeg.getTranslation().interpolate(FieldConstants.ampCenter, 0.5);

public AimingParameters getSuperPoopAimingParameters() {
if (latestSuperPoopParameters != null) {
return latestSuperPoopParameters;
}
Pose2d predictedFieldToRobot =
getPredictedPose(superPoopLookahead.get(), superPoopLookahead.get());
Translation2d predictedRobotToTarget =
AllianceFlipUtil.apply(superPoopTarget).minus(predictedFieldToRobot.getTranslation());
double effectiveDistance = predictedRobotToTarget.getNorm();

latestSuperPoopParameters =
new AimingParameters(
predictedRobotToTarget.getAngle(),
Rotation2d.fromDegrees(superPoopArmAngleMap.get(effectiveDistance)),
effectiveDistance,
superPoopFlywheelSpeedsMap.get(effectiveDistance));
return latestSuperPoopParameters;
}

public ModuleLimits getModuleLimits() {
return flywheelAccelerating && !DriverStation.isAutonomousEnabled()
? DriveConstants.moduleLimitsFlywheelSpinup
: DriveConstants.moduleLimitsFree;
}

public boolean inShootingZone() {
return getEstimatedPose()
.getTranslation()
.getDistance(
AllianceFlipUtil.apply(
FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d()))
< Units.feetToMeters(shootingZoneFeet.get());
Pose2d robot = AllianceFlipUtil.apply(getEstimatedPose());
if (robot.getY() <= FieldConstants.Stage.ampLeg.getY()) {
return robot.getX() <= FieldConstants.wingX;
} else {
return robot.getX() <= FieldConstants.fieldLength / 2.0 + 0.5;
}
}

public boolean inCloseShootingZone() {
Expand Down
7 changes: 4 additions & 3 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,6 @@ public class Flywheels extends SubsystemBase {
new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0);
private static final LoggedTunableNumber poopingRpm =
new LoggedTunableNumber("Flywheels/PoopingRpm", 3000.0);
private static final LoggedTunableNumber superPoopRpm =
new LoggedTunableNumber("Flywheels/SuperPoopingRpm", 3000.0);
private static final LoggedTunableNumber maxAcceleration =
new LoggedTunableNumber(
"Flywheels/MaxAccelerationRpmPerSec", flywheelConfig.maxAcclerationRpmPerSec());
Expand Down Expand Up @@ -74,7 +72,10 @@ public enum Goal {
INTAKE(intakingRpm, intakingRpm),
EJECT(ejectingRpm, ejectingRpm),
POOP(poopingRpm, poopingRpm),
SUPER_POOP(superPoopRpm, superPoopRpm),
SUPER_POOP(
() -> RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().leftSpeed(),
() ->
RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().rightSpeed()),
CHARACTERIZING(() -> 0.0, () -> 0.0);

private final DoubleSupplier leftGoal;
Expand Down
3 changes: 2 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,8 @@ public enum Goal {
UNJAM_INTAKE(new LoggedTunableNumber("Arm/UnjamDegrees", 40.0)),
STATION_INTAKE(new LoggedTunableNumber("Arm/StationIntakeDegrees", 45.0)),
AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()),
SUPER_POOP(new LoggedTunableNumber("Arm/SuperPoopDegrees", 48.0)),
SUPER_POOP(
() -> RobotState.getInstance().getSuperPoopAimingParameters().armAngle().getDegrees()),
AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)),
SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)),
PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 34.0)),
Expand Down

2 comments on commit 00fe4e7

@mpatankar6
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi Jonah,
For super poop, I see you're interpolating both the arm angle and the flywheel speed at the same time. How did you get your data points? Did you make the map for one mechanism and then use the other to compensate for the error, or did you do both simultaneously somehow?

@jwbonner
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We ended up making more changes, see #259. We tuned both mechanisms simultaneously while trying to achieve a consistent shot a little bit over the stage.

Please sign in to comment.