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

Jump to bottom

Add all demo mode controls #267

Merged
merged 3 commits into from
Jun 13, 2024
Merged

Add all demo mode controls #267

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b3610a7
Add all demo mode controls
suryatho Jun 12, 2024
529c9c6
Fix half stow override and force TeleopDriveController max velocity t…
harnwalN Jun 13, 2024
840db8e
Fixes
jwbonner Jun 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
1 change: 1 addition & 0 deletions src/main/java/org/littletonrobotics/frc2024/Robot.java
View file
Open in desktop
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
Open in desktop

Large diffs are not rendered by default.

6 changes: 6 additions & 0 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 @@ -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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading