Skip to content

Commit

Permalink
Merge branch 'main' into demo-autos
Browse files Browse the repository at this point in the history
  • Loading branch information
@jwbonner
jwbonner committed Jun 13, 2024
2 parents b28c0d3 + 7bbcb66 commit 126a9a4
Show file tree
Hide file tree
Showing 10 changed files with 284 additions and 124 deletions.
1 change: 1 addition & 0 deletions src/main/java/org/littletonrobotics/frc2024/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,6 +237,7 @@ public void robotPeriodic() {
});

// Robot container periodic methods
robotContainer.updateDemoControls();
robotContainer.checkControllers();
robotContainer.updateDashboardOutputs();
robotContainer.updateAlerts();
Expand Down
323 changes: 213 additions & 110 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file

Large diffs are not rendered by default.

14 changes: 10 additions & 4 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,9 +54,11 @@ public record AimingParameters(
double effectiveDistance,
FlywheelSpeeds flywheelSpeeds) {}

public record DemoAimingParameters(
public record DemoFollowParameters(
Pose2d targetPose, Rotation2d targetHeading, Rotation2d armAngle) {}

public record DemoShotParameters(Rotation2d armAngle, FlywheelSpeeds flywheelSpeeds) {}

private static final LoggedTunableNumber autoLookahead =
new LoggedTunableNumber("RobotState/AutoLookahead", 0.5);
private static final LoggedTunableNumber lookahead =
Expand Down Expand Up @@ -136,7 +138,11 @@ public static RobotState getInstance() {
private AimingParameters latestSuperPoopParameters = null;
// Demo parameters
private Pose3d demoTagPose = null;
private DemoAimingParameters latestDemoParamters = null;
private DemoFollowParameters latestDemoParamters = null;

@Setter @Getter
private DemoShotParameters demoShotParameters =
new DemoShotParameters(Rotation2d.fromDegrees(0.0), new FlywheelSpeeds(0.0, 0.0));

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

Expand Down Expand Up @@ -337,7 +343,7 @@ public void setDemoTagPose(Pose3d demoTagPose) {
private static final LoggedTunableNumber demoTargetDistance =
new LoggedTunableNumber("RobotState/DemoTargetDistance", 2.0);

public Optional<DemoAimingParameters> getDemoTagParameters() {
public Optional<DemoFollowParameters> getDemoTagParameters() {
if (latestDemoParamters != null) {
// Use cached demo parameters.
return Optional.of(latestDemoParamters);
Expand Down Expand Up @@ -365,7 +371,7 @@ public Optional<DemoAimingParameters> getDemoTagParameters() {
robotToDemoTagFixed.getNorm() - ArmConstants.armOrigin.getX(),
z - ArmConstants.armOrigin.getY());

latestDemoParamters = new DemoAimingParameters(targetPose, targetHeading, armAngle);
latestDemoParamters = new DemoFollowParameters(targetPose, targetHeading, armAngle);
return Optional.of(latestDemoParamters);
}

Expand Down
4 changes: 2 additions & 2 deletions src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ private Command followTag() {
() ->
RobotState.getInstance()
.getDemoTagParameters()
.map(RobotState.DemoAimingParameters::targetPose)
.map(RobotState.DemoFollowParameters::targetPose)
.orElseGet(() -> RobotState.getInstance().getEstimatedPose()),
Translation2d::new,
false),
Expand All @@ -58,7 +58,7 @@ private Command lookAtTag() {
() ->
RobotState.getInstance()
.getDemoTagParameters()
.map(RobotState.DemoAimingParameters::targetHeading)
.map(RobotState.DemoFollowParameters::targetHeading)
.orElseGet(
() -> RobotState.getInstance().getEstimatedPose().getRotation())),
drive::clearHeadingGoal)
Expand Down
14 changes: 9 additions & 5 deletions ...ava/org/littletonrobotics/frc2024/subsystems/drive/controllers/TeleopDriveController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import lombok.Setter;
import org.littletonrobotics.frc2024.RobotState;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;

Expand All @@ -26,6 +27,8 @@ public class TeleopDriveController {
private static final LoggedTunableNumber maxAngularVelocityScalar =
new LoggedTunableNumber("TeleopDrive/MaxAngularVelocityScalar", 0.65);

@Setter private static double velocityScalar = 1.0;

private double controllerX = 0;
private double controllerY = 0;
private double controllerOmega = 0;
Expand Down Expand Up @@ -56,21 +59,22 @@ public ChassisSpeeds update() {
double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get());
omega = Math.copySign(omega * omega, omega);

final double maxLinearVelocity = driveConfig.maxLinearVelocity() * velocityScalar;
final double maxAngularVelocity =
driveConfig.maxAngularVelocity() * maxAngularVelocityScalar.get();
driveConfig.maxAngularVelocity() * (maxAngularVelocityScalar.get() * velocityScalar);
if (robotRelative) {
return new ChassisSpeeds(
linearVelocity.getX() * driveConfig.maxLinearVelocity(),
linearVelocity.getY() * driveConfig.maxLinearVelocity(),
linearVelocity.getX() * maxLinearVelocity,
linearVelocity.getY() * maxLinearVelocity,
omega * maxAngularVelocity);
} else {
if (DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
linearVelocity = linearVelocity.rotateBy(Rotation2d.fromRadians(Math.PI));
}
return ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * driveConfig.maxLinearVelocity(),
linearVelocity.getY() * driveConfig.maxLinearVelocity(),
linearVelocity.getX() * maxLinearVelocity,
linearVelocity.getY() * maxLinearVelocity,
omega * maxAngularVelocity,
RobotState.getInstance().getEstimatedPose().getRotation());
}
Expand Down
16 changes: 16 additions & 0 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 @@ -41,6 +41,8 @@ public class Flywheels extends SubsystemBase {
new LoggedTunableNumber("Flywheels/PrepareShootMultiplier", 1.0);
private static final LoggedTunableNumber intakingRpm =
new LoggedTunableNumber("Flywheels/IntakingRpm", -3000.0);
private static final LoggedTunableNumber demoIntakingRpm =
new LoggedTunableNumber("Flywheels/DemoIntakingRpm", -250.0);
private static final LoggedTunableNumber ejectingRpm =
new LoggedTunableNumber("Flywheels/EjectingRpm", 1000.0);
private static final LoggedTunableNumber poopingRpm =
Expand Down Expand Up @@ -70,12 +72,16 @@ public enum Goal {
IDLE(() -> 0.0, () -> 0.0),
SHOOT(shootingLeftRpm, shootingRightRpm),
INTAKE(intakingRpm, intakingRpm),
DEMO_INTAKE(demoIntakingRpm, demoIntakingRpm),
EJECT(ejectingRpm, ejectingRpm),
POOP(poopingRpm, poopingRpm),
SUPER_POOP(
() -> RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().leftSpeed(),
() ->
RobotState.getInstance().getSuperPoopAimingParameters().flywheelSpeeds().rightSpeed()),
DEMO_SHOT(
() -> RobotState.getInstance().getDemoShotParameters().flywheelSpeeds().leftSpeed(),
() -> RobotState.getInstance().getDemoShotParameters().flywheelSpeeds().rightSpeed()),
CHARACTERIZING(() -> 0.0, () -> 0.0);

private final DoubleSupplier leftGoal;
Expand Down Expand Up @@ -226,6 +232,11 @@ public Command intakeCommand() {
.withName("Flywheels Intake");
}

public Command demoIntakeCommand() {
return startEnd(() -> setGoal(Goal.DEMO_INTAKE), () -> setGoal(Goal.IDLE))
.withName("Flywheels Demo Intake");
}

public Command ejectCommand() {
return startEnd(() -> setGoal(Goal.EJECT), () -> setGoal(Goal.IDLE))
.withName("Flywheels Eject");
Expand All @@ -239,4 +250,9 @@ public Command superPoopCommand() {
return startEnd(() -> setGoal(Goal.SUPER_POOP), () -> setGoal(Goal.IDLE))
.withName("Flywheels Super Poop");
}

public Command demoShootCommand() {
return startEnd(() -> setGoal(Goal.DEMO_SHOT), () -> setGoal(Goal.IDLE))
.withName("Flywheels Demo Shoot");
}
}
2 changes: 2 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/leds/Leds.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -180,6 +180,8 @@ public synchronized void periodic() {
rainbow(rainbowCycleLength, rainbowDuration);
} else if (hasNote) {
solid(Color.kGreen);
} else if (demoMode) {
wave(allianceColor, secondaryDisabledColor, waveAllianceCycleLength, waveAllianceDuration);
}

if (endgameAlert) {
Expand Down
16 changes: 15 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,10 @@
public class Rollers extends SubsystemBase {
private static final LoggedTunableNumber jackhammerTime =
new LoggedTunableNumber("Rollers/JackhammerTime", 0.075);
private static final LoggedTunableNumber stationIntakeTime =
new LoggedTunableNumber("Rollers/StationIntakeTime", 0.06);
private static final LoggedTunableNumber demoStationIntakeTime =
new LoggedTunableNumber("Rollers/DemoStationIntakeTime", 0.1);

private final Feeder feeder;
private final Indexer indexer;
Expand All @@ -44,6 +48,7 @@ public enum Goal {
IDLE,
FLOOR_INTAKE,
STATION_INTAKE,
DEMO_STATION_INTAKE,
EJECT_TO_FLOOR,
UNJAM_UNTACO,
UNJAM_FEEDER,
Expand Down Expand Up @@ -131,7 +136,16 @@ public void periodic() {
}
}
case STATION_INTAKE -> {
if (gamepieceState != GamepieceState.NONE && gamepieceStateTimer.hasElapsed(0.06)) {
if (gamepieceState != GamepieceState.NONE
&& gamepieceStateTimer.hasElapsed(stationIntakeTime.get())) {
indexer.setGoal(Indexer.Goal.IDLING);
} else {
indexer.setGoal(Indexer.Goal.STATION_INTAKING);
}
}
case DEMO_STATION_INTAKE -> {
if (gamepieceState != GamepieceState.NONE
&& gamepieceStateTimer.hasElapsed(demoStationIntakeTime.get())) {
indexer.setGoal(Indexer.Goal.IDLING);
} else {
indexer.setGoal(Indexer.Goal.STATION_INTAKING);
Expand Down
6 changes: 6 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ public enum Goal {
AIM,
SUPER_POOP,
AIM_AT_DEMO_TAG,
DEMO_SHOT,
UNJAM_FEEDER,
STATION_INTAKE,
AMP,
Expand Down Expand Up @@ -122,6 +123,11 @@ public void periodic() {
climber.setGoal(Climber.Goal.IDLE);
backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
}
case DEMO_SHOT -> {
arm.setGoal(Arm.Goal.DEMO_SHOT);
climber.setGoal(Climber.Goal.IDLE);
backpackActuator.setGoal(BackpackActuator.Goal.RETRACT);
}
case STATION_INTAKE -> {
arm.setGoal(Arm.Goal.STATION_INTAKE);
climber.setGoal(Climber.Goal.IDLE);
Expand Down
12 changes: 10 additions & 2 deletions 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 @@ -87,6 +87,7 @@ public enum Goal {
.getDemoTagParameters()
.map(parameters -> parameters.armAngle().getDegrees())
.orElse(minAngle.getDegrees())),
DEMO_SHOT(() -> RobotState.getInstance().getDemoShotParameters().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 Expand Up @@ -131,6 +132,7 @@ private double getRads() {

private BooleanSupplier disableSupplier = DriverStation::isDisabled;
private BooleanSupplier coastSupplier = () -> false;
private BooleanSupplier halfStowSupplier = () -> true;
private boolean brakeModeEnabled = true;

private boolean wasNotAuto = false;
Expand All @@ -152,13 +154,19 @@ public Arm(ArmIO io) {
goalVisualizer = new ArmVisualizer("Goal", Color.kBlue);
}

public void setOverrides(BooleanSupplier disableOverride, BooleanSupplier coastOverride) {
public void setOverrides(
BooleanSupplier disableOverride,
BooleanSupplier coastOverride,
BooleanSupplier halfStowOverride) {
disableSupplier = () -> disableOverride.getAsBoolean() || DriverStation.isDisabled();
coastSupplier = coastOverride;
halfStowSupplier = halfStowOverride;
}

private double getStowAngle() {
if (DriverStation.isTeleopEnabled() && RobotState.getInstance().inCloseShootingZone()) {
if (DriverStation.isTeleopEnabled()
&& RobotState.getInstance().inCloseShootingZone()
&& !halfStowSupplier.getAsBoolean()) {
return MathUtil.clamp(
setpointState.position,
minAngle.getRadians(),
Expand Down

0 comments on commit 126a9a4

Please sign in to comment.