Skip to content

Commit

Permalink
Resolves #11
Browse files Browse the repository at this point in the history
  • Loading branch information
@suryatho
suryatho committed Feb 14, 2024
1 parent 0e9a34a commit d7f7861
Show file tree
Hide file tree
Showing 9 changed files with 92 additions and 93 deletions.
14 changes: 9 additions & 5 deletions src/main/java/org/littletonrobotics/frc2024/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,6 @@ public RobotContainer() {
rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {});

arm = new Arm(new ArmIOSim());
superstructure = new Superstructure(arm);
}
}
}
Expand Down Expand Up @@ -236,23 +235,28 @@ private void configureButtonBindings() {
controller
.rightTrigger()
.and(readyToShoot)
.onTrue(rollers.feedShooter().withTimeout(1.0).withName("Shoot"));
.onTrue(
Commands.parallel(
Commands.waitSeconds(0.5),
Commands.waitUntil(controller.rightTrigger().negate()))
.deadlineWith(rollers.feedShooter(), superstructure.aim(), flywheels.shoot()));
// Intake Floor
controller
.leftTrigger()
.whileTrue(
superstructure
.intake()
.alongWith(rollers.floorIntake().onlyIf(superstructure::atGoal))
.alongWith(
Commands.waitUntil(superstructure::atGoal).andThen(rollers.floorIntake()))
.withName("Floor Intake"));
// Eject Floor
controller
.leftBumper()
.whileTrue(
superstructure
.intake()
.alongWith(rollers.ejectFloor().onlyIf(superstructure::atGoal))
.withName("Eject Floor"));
.alongWith(Commands.waitUntil(superstructure::atGoal).andThen(rollers.ejectFloor()))
.withName("Eject To Floor"));

controller
.y()
Expand Down
1 change: 0 additions & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
import edu.wpi.first.wpilibj.simulation.DCMotorSim;

public class ModuleIOSim implements ModuleIO {

private final DCMotorSim driveSim =
new DCMotorSim(DCMotor.getKrakenX60Foc(1), moduleConstants.driveReduction(), 0.025);
private final DCMotorSim turnSim =
Expand Down
44 changes: 15 additions & 29 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 @@ -15,7 +15,6 @@
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 Down Expand Up @@ -47,11 +46,10 @@ public class Flywheels extends SubsystemBase {

@RequiredArgsConstructor
public enum Goal {
STOP(() -> 0.0, () -> 0.0),
IDLING(idleLeftRpm, idleRightRpm),
SHOOTING(shootingLeftRpm, shootingRightRpm),
INTAKING(intakingRpm, intakingRpm),
EJECTING(ejectingRpm, ejectingRpm),
IDLE(idleLeftRpm, idleRightRpm),
SHOOT(shootingLeftRpm, shootingRightRpm),
INTAKE(intakingRpm, intakingRpm),
EJECT(ejectingRpm, ejectingRpm),
CHARACTERIZING(() -> 0.0, () -> 0.0);

private final DoubleSupplier leftSetpoint;
Expand All @@ -66,15 +64,12 @@ private double getRightSetpoint() {
}
}

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

public Flywheels(FlywheelsIO io) {
System.out.println("[Init] Creating Shooter");
this.io = io;

setDefaultCommand(idle());
setDefaultCommand(runOnce(this::goIdle).withName("Flywheels Idle"));
}

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

if (DriverStation.isDisabled()) {
setGoal(Goal.STOP);
io.stop();
}

switch (goal) {
case STOP -> io.stop();
case CHARACTERIZING -> {} // Handled by runCharacterizationVolts
default -> io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
if (goal != Goal.CHARACTERIZING) {
io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint());
}

Logger.recordOutput("Flywheels/Goal", goal);
Expand All @@ -105,7 +98,7 @@ public void periodic() {
}

public void runCharacterizationVolts(double volts) {
setGoal(Goal.CHARACTERIZING);
goal = Goal.CHARACTERIZING;
io.runCharacterizationLeftVolts(volts);
io.runCharacterizationRightVolts(volts);
}
Expand All @@ -120,26 +113,19 @@ public boolean atGoal() {
&& Math.abs(inputs.rightVelocityRpm - goal.getRightSetpoint()) <= shooterTolerance.get();
}

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

public Command idle() {
return runOnce(() -> setGoal(Goal.IDLING)).withName("Flywheels Idle");
private void goIdle() {
goal = Goal.IDLE;
}

public Command shoot() {
return startEnd(() -> setGoal(Goal.SHOOTING), () -> setGoal(Goal.IDLING))
.withName("Flywheels Shooting");
return startEnd(() -> goal = Goal.SHOOT, this::goIdle).withName("Flywheels Shooting");
}

public Command intake() {
return startEnd(() -> setGoal(Goal.INTAKING), () -> setGoal(Goal.IDLING))
.withName("Flywheels Intaking");
return startEnd(() -> goal = Goal.INTAKE, this::goIdle).withName("Flywheels Intaking");
}

public Command eject() {
return startEnd(() -> setGoal(Goal.EJECTING), () -> setGoal(Goal.IDLING))
.withName("Flywheels Ejecting");
return startEnd(() -> goal = Goal.EJECT, this::goIdle).withName("Flywheels Ejecting");
}
}
8 changes: 4 additions & 4 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,14 +10,14 @@
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import edu.wpi.first.wpilibj.simulation.DCMotorSim;

public class GenericRollerSystemIOSim implements GenericRollerSystemIO {
private final FlywheelSim sim;
private final DCMotorSim sim;
private double appliedVoltage = 0.0;

public GenericRollerSystemIOSim(DCMotor motorModel, double reduction, double moi) {
sim = new FlywheelSim(motorModel, reduction, moi);
sim = new DCMotorSim(motorModel, reduction, moi);
}

@Override
Expand All @@ -27,7 +27,7 @@ public void updateInputs(GenericRollerSystemIOInputs inputs) {
}

sim.update(0.02);
inputs.positionRads += sim.getAngularVelocityRadPerSec() * 0.02;
inputs.positionRads = sim.getAngularPositionRad();
inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec();
inputs.appliedVoltage = appliedVoltage;
inputs.outputCurrent = sim.getCurrentDrawAmps();
Expand Down
44 changes: 23 additions & 21 deletions src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,10 @@

package org.littletonrobotics.frc2024.subsystems.rollers;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import lombok.Getter;
import lombok.Setter;
import org.littletonrobotics.frc2024.subsystems.rollers.feeder.Feeder;
import org.littletonrobotics.frc2024.subsystems.rollers.indexer.Indexer;
import org.littletonrobotics.frc2024.subsystems.rollers.intake.Intake;
Expand All @@ -26,57 +26,61 @@ public class Rollers extends SubsystemBase {
new RollersSensorsIOInputsAutoLogged();

public enum Goal {
IDLING,
FLOOR_INTAKING,
STATION_INTAKING,
EJECTING_TO_FLOOR,
FEEDING_TO_SHOOTER
IDLE,
FLOOR_INTAKE,
STATION_INTAKE,
EJECT_TO_FLOOR,
FEED_TO_SHOOTER
}

@Getter @Setter private Goal goal = Goal.IDLING;
@Getter private Goal goal = Goal.IDLE;

public Rollers(Feeder feeder, Indexer indexer, Intake intake, RollersSensorsIO sensorsIO) {
this.feeder = feeder;
this.indexer = indexer;
this.intake = intake;
this.sensorsIO = sensorsIO;

setDefaultCommand(idle());
setDefaultCommand(runOnce(this::goIdle).withName("Rollers Idling"));
}

@Override
public void periodic() {
sensorsIO.updateInputs(sensorInputs);
Logger.processInputs("RollersSensors", sensorInputs);

if (DriverStation.isDisabled()) {
goIdle();
}

switch (goal) {
case IDLING -> {
case IDLE -> {
feeder.setGoal(Feeder.Goal.IDLING);
indexer.setGoal(Indexer.Goal.IDLING);
intake.setGoal(Intake.Goal.IDLING);
}
case FLOOR_INTAKING -> {
case FLOOR_INTAKE -> {
feeder.setGoal(Feeder.Goal.FLOOR_INTAKING);
indexer.setGoal(Indexer.Goal.FLOOR_INTAKING);
intake.setGoal(Intake.Goal.FLOOR_INTAKING);
if (sensorInputs.shooterStaged) {
indexer.setGoal(Indexer.Goal.IDLING);
}
}
case STATION_INTAKING -> {
case STATION_INTAKE -> {
feeder.setGoal(Feeder.Goal.IDLING);
indexer.setGoal(Indexer.Goal.STATION_INTAKING);
intake.setGoal(Intake.Goal.IDLING);
if (sensorInputs.shooterStaged) { // TODO: add this banner sensor
indexer.setGoal(Indexer.Goal.IDLING);
}
}
case EJECTING_TO_FLOOR -> {
case EJECT_TO_FLOOR -> {
feeder.setGoal(Feeder.Goal.EJECTING);
indexer.setGoal(Indexer.Goal.EJECTING);
intake.setGoal(Intake.Goal.EJECTING);
}
case FEEDING_TO_SHOOTER -> {
case FEED_TO_SHOOTER -> {
feeder.setGoal(Feeder.Goal.SHOOTING);
indexer.setGoal(Indexer.Goal.SHOOTING);
intake.setGoal(Intake.Goal.IDLING);
Expand All @@ -88,27 +92,25 @@ public void periodic() {
intake.periodic();
}

public Command idle() {
return runOnce(() -> setGoal(Goal.IDLING)).withName("Rollers Idle");
private void goIdle() {
goal = Goal.IDLE;
}

public Command floorIntake() {
return startEnd(() -> setGoal(Goal.FLOOR_INTAKING), () -> setGoal(Goal.IDLING))
.withName("Rollers Floor Intake");
return startEnd(() -> goal = Goal.FLOOR_INTAKE, this::goIdle).withName("Rollers Floor Intake");
}

public Command stationIntake() {
return startEnd(() -> setGoal(Goal.STATION_INTAKING), () -> setGoal(Goal.IDLING))
return startEnd(() -> goal = Goal.STATION_INTAKE, this::goIdle)
.withName("Rollers Station Intake");
}

public Command ejectFloor() {
return startEnd(() -> setGoal(Goal.EJECTING_TO_FLOOR), () -> setGoal(Goal.IDLING))
.withName("Rollers Eject Floor");
return startEnd(() -> goal = Goal.EJECT_TO_FLOOR, this::goIdle).withName("Rollers Eject Floor");
}

public Command feedShooter() {
return startEnd(() -> setGoal(Goal.FEEDING_TO_SHOOTER), () -> setGoal(Goal.IDLING))
return startEnd(() -> goal = Goal.FEED_TO_SHOOTER, this::goIdle)
.withName("Rollers Feed Shooter");
}
}
2 changes: 1 addition & 1 deletion src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
public class Feeder extends GenericRollerSystem<Feeder.Goal> {
@RequiredArgsConstructor
@Getter
public enum Goal implements GenericRollerSystem.VoltageGoal {
public enum Goal implements VoltageGoal {
IDLING(() -> 0.0),
FLOOR_INTAKING(new LoggedTunableNumber("Feeder/FloorIntakingVoltage", 8.0)),
SHOOTING(new LoggedTunableNumber("Feeder/Shooting", 8.0)),
Expand Down
Loading

0 comments on commit d7f7861

Please sign in to comment.