Skip to content

Commit

Permalink
Add demo mode controls (#267)
Browse files Browse the repository at this point in the history 
* Add all demo mode controls

* Fix half stow override and force TeleopDriveController max velocity to 1 when not in demo mode

* Fixes

---------

Co-authored-by: nharnwal <[email protected]>
Co-authored-by: Jonah <[email protected]>
  • Loading branch information
@suryatho @harnwalN @jwbonner
3 people committed Jun 13, 2024
1 parent 907ba57 commit 7bbcb66
Show file tree
Hide file tree
Showing 9 changed files with 278 additions and 118 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 @@ -229,6 +229,7 @@ public void robotPeriodic() {
superPoopParameters.flywheelSpeeds().rightSpeed());

// 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.

6 changes: 6 additions & 0 deletions src/main/java/org/littletonrobotics/frc2024/RobotState.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ public record AimingParameters(
double effectiveDistance,
FlywheelSpeeds flywheelSpeeds) {}

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 @@ -129,6 +131,10 @@ public static RobotState getInstance() {

private AimingParameters latestSuperPoopParameters = null;

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

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

private RobotState() {
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 @@ -31,6 +31,7 @@ public enum Goal {
BACKPACK_OUT_UNJAM,
AIM,
SUPER_POOP,
DEMO_SHOT,
UNJAM_FEEDER,
STATION_INTAKE,
AMP,
Expand Down Expand Up @@ -116,6 +117,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 @@ -81,6 +81,7 @@ public enum Goal {
AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()),
SUPER_POOP(
() -> RobotState.getInstance().getSuperPoopAimingParameters().armAngle().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 @@ -125,6 +126,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 @@ -146,13 +148,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 7bbcb66

Please sign in to comment.