Skip to content

Commit

Permalink
Merge branch 'main' into thinking-on-your-feet
Browse files Browse the repository at this point in the history
  • Loading branch information
@jwbonner
jwbonner committed Apr 12, 2024
2 parents 84efde6 + 00fe4e7 commit 9887179
Show file tree
Hide file tree
Showing 13 changed files with 182 additions and 100 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
54 changes: 18 additions & 36 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,7 @@ public class RobotContainer {
private final Trigger shootPresets = overrides.operatorSwitch(0);
private final Trigger shootAlignDisable = overrides.operatorSwitch(1);
private final Trigger lookaheadDisable = overrides.operatorSwitch(2);
private final Trigger autoDriveDisable = overrides.operatorSwitch(3);
private final Trigger autoDriveEnable = overrides.operatorSwitch(3);
private final Trigger autoFlywheelSpinupDisable = overrides.operatorSwitch(4);
private final Alert aprilTagLayoutAlert = new Alert("", AlertType.INFO);
private final Alert driverDisconnected =
Expand Down Expand Up @@ -304,9 +304,10 @@ public RobotContainer() {
() ->
autoFlywheelSpinupDisable.negate().getAsBoolean()
&& DriverStation.isTeleopEnabled()
&& robotState.isNearSpeaker()
&& rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED
&& !superstructure.getCurrentGoal().isClimbingGoal());
&& !superstructure.getCurrentGoal().isClimbingGoal()
&& (robotState.inCloseShootingZone()
|| (robotState.inShootingZone()
&& rollers.getGamepieceState() == GamepieceState.SHOOTER_STAGED)));

// Configure autos and buttons
configureAutos();
Expand Down Expand Up @@ -443,7 +444,7 @@ private void configureButtonBindings() {
drive.setHeadingGoal(() -> robotState.getAimingParameters().driveHeading()),
drive::clearHeadingGoal),
shootAlignDisable);
Trigger nearSpeaker = new Trigger(robotState::isNearSpeaker);
Trigger nearSpeaker = new Trigger(robotState::inShootingZone);
driver
.a()
.and(nearSpeaker)
Expand All @@ -452,34 +453,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 @@ -491,7 +485,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 Expand Up @@ -520,13 +514,7 @@ private void configureButtonBindings() {
.and(
DriverStation
::isEnabled) // Must be enabled, allowing driver to hold button as soon as auto ends
.whileTrue(
superstructure
.setGoalCommand(Superstructure.Goal.INTAKE)
.alongWith(
Commands.waitUntil(superstructure::atArmGoal)
.andThen(rollers.setGoalCommand(Rollers.Goal.FLOOR_INTAKE)))
.withName("Floor Intake"));
.whileTrue(rollers.setGoalCommand(Rollers.Goal.FLOOR_INTAKE).withName("Floor Intake"));
driver
.leftTrigger()
.and(() -> rollers.getGamepieceState() != GamepieceState.NONE)
Expand All @@ -535,13 +523,7 @@ private void configureButtonBindings() {
// Eject Floor
driver
.leftBumper()
.whileTrue(
superstructure
.setGoalCommand(Superstructure.Goal.INTAKE)
.alongWith(
Commands.waitUntil(superstructure::atArmGoal)
.andThen(rollers.setGoalCommand(Rollers.Goal.EJECT_TO_FLOOR)))
.withName("Eject To Floor"));
.whileTrue(rollers.setGoalCommand(Rollers.Goal.EJECT_TO_FLOOR).withName("Eject To Floor"));

// Intake source
driver
Expand Down Expand Up @@ -622,6 +604,9 @@ private void configureButtonBindings() {
.b()
.whileTrue(
Commands.either(
// Auto drive to amp
ampAutoDrive,

// Drive while heading is being controlled
drive
.run(
Expand All @@ -635,14 +620,11 @@ private void configureButtonBindings() {
Commands.startEnd(
() -> drive.setHeadingGoal(() -> Rotation2d.fromDegrees(-90.0)),
drive::clearHeadingGoal)),

// Auto drive to amp
ampAutoDrive,
autoDriveDisable)
autoDriveEnable)
.alongWith(
Commands.waitUntil(
() -> {
if (autoDriveDisable.getAsBoolean()) {
if (!autoDriveEnable.getAsBoolean()) {
return true;
}
Pose2d poseError =
Expand Down
99 changes: 82 additions & 17 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,28 @@ 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 nearSpeakerFeet =
new LoggedTunableNumber("RobotState/NearSpeakerFeet", 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;

private static final double armAngleCoefficient = 57.254371165197;
Expand All @@ -60,6 +68,27 @@ 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) {
shotCompensationDegrees += deltaDegrees;
}
Expand Down Expand Up @@ -95,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 @@ -110,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 @@ -130,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 @@ -228,38 +261,70 @@ 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)
? autoFarShotCompensationDegrees
: 0.0;
Logger.recordOutput(
"RobotState/AimingParameters/AutoFarArmCorrectionDegrees", autoFarArmCorrection);
latestParameters =
new AimingParameters(
targetVehicleDirection,
Rotation2d.fromDegrees(armAngleDegrees + shotCompensationDegrees),
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 isNearSpeaker() {
public boolean inShootingZone() {
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() {
return getEstimatedPose()
.getTranslation()
.getDistance(
AllianceFlipUtil.apply(
FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d()))
< Units.feetToMeters(nearSpeakerFeet.get());
< Units.feetToMeters(closeShootingZoneFeet.get());
}

/**
Expand Down
6 changes: 5 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/commands/ClimbingCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,10 @@ public class ClimbingCommands {
private static final LoggedTunableNumber autoUntrapTime =
new LoggedTunableNumber("ClimbingCommands/AutoUntrapTime", 134.5);

// Which mode to use for trap scoring. Can be changed between jackhammering and regular trap
// scoring.
private static final Rollers.Goal TRAP_SCORE_ROLLER_GOAL = Rollers.Goal.JACKHAMMERING;

public static Command trapSequence(
Drive drive,
Superstructure superstructure,
Expand Down Expand Up @@ -85,7 +89,7 @@ public static Command trapSequence(
// Trap.
superstructure
.setGoalCommand(Superstructure.Goal.TRAP)
.alongWith(rollers.setGoalCommand(Rollers.Goal.TRAP_SCORE)),
.alongWith(rollers.setGoalCommand(TRAP_SCORE_ROLLER_GOAL)),

// Untrap
superstructure.setGoalCommand(Superstructure.Goal.UNTRAP));
Expand Down
Loading

0 comments on commit 9887179

Please sign in to comment.