Skip to content

Commit

Permalink
Sim works in teleop
Browse files Browse the repository at this point in the history
  • Loading branch information
@suryatho
suryatho committed Feb 13, 2024
1 parent 2d94203 commit 96c5149
Show file tree
Hide file tree
Showing 3 changed files with 88 additions and 64 deletions.
58 changes: 30 additions & 28 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,15 @@
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIO;
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.FeederIOSim;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIO;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSim;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.IndexerIOSparkFlex;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIO;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOKrakenFOC;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.IntakeIOSim;
import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm;
import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO;
Expand Down Expand Up @@ -126,6 +129,7 @@ public RobotContainer() {
}
}

// No implementation for replay
if (drive == null) {
drive =
new Drive(
Expand Down Expand Up @@ -163,6 +167,9 @@ public RobotContainer() {
}
superstructure = new Superstructure(arm);

if (superstructure == null) {
superstructure = new Superstructure(arm);
}
// Configure autos and buttons
configureAutos();
configureButtonBindings();
Expand Down Expand Up @@ -195,53 +202,48 @@ private void configureAutos() {
private void configureButtonBindings() {
// Drive Commands
drive.setDefaultCommand(
drive
.run(
() ->
drive.setTeleopDriveGoal(
-controller.getLeftY(), -controller.getLeftX(), -controller.getRightX()))
.withName("DriveTeleop"));
drive.run(
() ->
drive.setTeleopDriveGoal(
-controller.getLeftY(), -controller.getLeftX(), -controller.getRightX())));

// Aim
controller
.a()
.whileTrue(
Commands.startEnd(drive::setAutoAimGoal, drive::clearAutoAimGoal)
.alongWith(superstructure.aimCommand(), flywheels.shootCommand()));
.onTrue(Commands.runOnce(drive::setAutoAimGoal))
.onFalse(Commands.runOnce(drive::clearAutoAimGoal));
controller.a().whileTrue(Commands.parallel(flywheels.shoot(), superstructure.aim()));

Trigger readyToShootTrigger =
// Shoot
Trigger readyToShoot =
new Trigger(
() ->
drive.isAutoAimGoalCompleted()
&& flywheels.atSetpoint()
&& superstructure.atArmSetpoint());
readyToShootTrigger
&& superstructure.atShootingSetpoint()
&& flywheels.atGoal());
readyToShoot
.whileTrue(
Commands.run(
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0)))
.whileFalse(
Commands.run(
() -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0)));
controller
.rightTrigger()
.and(readyToShootTrigger)
.onTrue(
rollers
.feedShooterCommand()
.withTimeout(1.0)
// Take over superstructure and flywheels, cancelling the main aiming command
.deadlineWith(superstructure.aimCommand(), flywheels.shootCommand()));
controller.rightTrigger().and(readyToShoot).whileTrue(rollers.feedShooter());

// Intake Floor
controller
.leftTrigger()
.whileTrue(
superstructure
.floorIntakeCommand()
.alongWith(Commands.waitSeconds(0.25).andThen(rollers.floorIntakeCommand())));
Commands.parallel(
superstructure.intake(),
Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake())));
// Eject Floor
controller
.leftBumper()
.whileTrue(
superstructure
.floorIntakeCommand()
.alongWith(Commands.waitSeconds(0.25).andThen(rollers.ejectFloorCommand())));
Commands.parallel(
superstructure.intake(),
Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor())));

controller
.y()
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ public class FlywheelConstants {
case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0);
};

public record Config(int leftID, int rightID, double reduction, double toleranceRPM) {}
public record Config(int leftID, int rightID, double reduction, double toleranceRpm) {}

public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {}
}
92 changes: 57 additions & 35 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 @@ -11,10 +11,12 @@

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import java.util.function.DoubleSupplier;
import lombok.Getter;
import lombok.RequiredArgsConstructor;
import lombok.Setter;
import org.littletonrobotics.frc2024.util.LoggedTunableNumber;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;
Expand All @@ -26,31 +28,31 @@ public class Flywheels extends SubsystemBase {
private static final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheels/kS", gains.kS());
private static final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheels/kV", gains.kV());
private static final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheels/kA", gains.kA());
private static LoggedTunableNumber shootingLeftRPM =
new LoggedTunableNumber("Superstructure/ShootingLeftRPM", 6000.0);
private static LoggedTunableNumber shootingRightRPM =
new LoggedTunableNumber("Superstructure/ShootingRightRPM", 4000.0);
private static LoggedTunableNumber idleLeftRPM =
new LoggedTunableNumber("Superstructure/IdleLeftRPM", 200.0);
private static LoggedTunableNumber idleRightRPM =
new LoggedTunableNumber("Superstructure/IdleRightRPM", 200.0);

private static LoggedTunableNumber intakingLeftRPM =
new LoggedTunableNumber("Superstructure/IntakingLeftRPM", -2000.0);
private static LoggedTunableNumber intakingRightRPM =
new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0);
private static final LoggedTunableNumber shootingLeftRpm =
new LoggedTunableNumber("Superstructure/ShootingLeftRpm", 6000.0);
private static final LoggedTunableNumber shootingRightRpm =
new LoggedTunableNumber("Superstructure/ShootingRightRpm", 4000.0);
private static final LoggedTunableNumber idleLeftRpm =
new LoggedTunableNumber("Superstructure/IdleLeftRpm", 1500.0);
private static final LoggedTunableNumber idleRightRpm =
new LoggedTunableNumber("Superstructure/IdleRightRpm", 1000.0);
private static final LoggedTunableNumber intakingRpm =
new LoggedTunableNumber("Superstructure/IntakingRpm", -2000.0);
private static final LoggedTunableNumber ejectingRpm =
new LoggedTunableNumber("Superstructure/EjectingRpm", 2000.0);
private static final LoggedTunableNumber shooterTolerance =
new LoggedTunableNumber("Flywheels/ToleranceRPM", config.toleranceRPM());
new LoggedTunableNumber("Flywheels/ToleranceRpm", config.toleranceRpm());

private final FlywheelsIO io;
private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged();

@RequiredArgsConstructor
public enum Goal {
STOP(() -> 0.0, () -> 0.0),
IDLE(idleLeftRPM, idleRightRPM),
SHOOTING(shootingLeftRPM, shootingRightRPM),
INTAKING(intakingLeftRPM, intakingRightRPM),
IDLING(idleLeftRpm, idleRightRpm),
SHOOTING(shootingLeftRpm, shootingRightRpm),
INTAKING(intakingRpm, intakingRpm),
EJECTing(ejectingRpm, ejectingRpm),
CHARACTERIZING(() -> 0.0, () -> 0.0);

private final DoubleSupplier leftSetpoint;
Expand All @@ -65,11 +67,15 @@ private double getRightSetpoint() {
}
}

@Getter private Goal goal = Goal.IDLE;
@Getter @Setter private Goal goal = Goal.IDLING;
private double characterizationVolts = 0.0;
private boolean characterizing = false;

public Flywheels(FlywheelsIO io) {
System.out.println("[Init] Creating Shooter");
this.io = io;
setDefaultCommand(runOnce(() -> goal = Goal.IDLE).withName("FlywheelsIdle"));

setDefaultCommand(idle());
}

@Override
Expand All @@ -83,7 +89,7 @@ public void periodic() {
hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA);

if (DriverStation.isDisabled()) {
goal = Goal.STOP;
setGoal(Goal.STOP);
}

switch (goal) {
Expand All @@ -93,14 +99,14 @@ public void periodic() {
}

Logger.recordOutput("Flywheels/Goal", goal);
Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.getLeftSetpoint());
Logger.recordOutput("Flywheels/RightSetpointRPM", goal.getRightSetpoint());
Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm);
Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm);
Logger.recordOutput("Flywheels/LeftSetpointRpm", goal.getLeftSetpoint());
Logger.recordOutput("Flywheels/RightSetpointRpm", goal.getRightSetpoint());
Logger.recordOutput("Flywheels/LeftRpm", inputs.leftVelocityRpm);
Logger.recordOutput("Flywheels/RightRpm", inputs.rightVelocityRpm);
}

public void runCharacterizationVolts(double volts) {
goal = Goal.CHARACTERIZING;
setGoal(Goal.CHARACTERIZING);
io.runCharacterizationLeftVolts(volts);
io.runCharacterizationRightVolts(volts);
}
Expand All @@ -109,19 +115,35 @@ public double getCharacterizationVelocity() {
return (inputs.leftVelocityRpm + inputs.rightVelocityRpm) / 2.0;
}

@AutoLogOutput(key = "Shooter/AtSetpoint")
public boolean atSetpoint() {
return Math.abs(inputs.leftVelocityRpm - goal.leftSetpoint.getAsDouble())
<= shooterTolerance.get()
&& Math.abs(inputs.rightVelocityRpm - goal.rightSetpoint.getAsDouble())
<= shooterTolerance.get();
@AutoLogOutput(key = "Flywheels/AtGoal")
public boolean atGoal() {
return Math.abs(inputs.leftVelocityRpm - goal.getLeftSetpoint()) <= shooterTolerance.get()
&& Math.abs(inputs.rightVelocityRpm - goal.getRightSetpoint()) <= shooterTolerance.get();
}

public Command stop() {
return runOnce(() -> setGoal(Goal.STOP)).andThen(Commands.idle()).withName("Flywheels Stop");
}

public Command idle() {
return runOnce(() -> setGoal(Goal.IDLING)).andThen(Commands.idle()).withName("Flywheels Idle");
}

public Command shoot() {
return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLING))
.andThen(Commands.idle())
.withName("Flywheels Shooting");
}

public Command shootCommand() {
return startEnd(() -> goal = Goal.SHOOTING, () -> goal = Goal.IDLE).withName("FlywheelsShoot");
public Command intake() {
return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLING))
.andThen(Commands.idle())
.withName("Flywheels Intaking");
}

public Command intakeCommand() {
return startEnd(() -> goal = Goal.INTAKING, () -> goal = Goal.IDLE).withName("FlywheelsIntake");
public Command eject() {
return startEnd(() -> setGoal(Goal.EJECTing), () -> setGoal(Goal.IDLING))
.andThen(Commands.idle())
.withName("Flywheels Ejecting");
}
}

0 comments on commit 96c5149

Please sign in to comment.