From d7f786151a22d8a5b41cec4fd1385ff1af42d9e7 Mon Sep 17 00:00:00 2001 From: suryatho Date: Wed, 14 Feb 2024 01:50:51 -0500 Subject: [PATCH] Resolves #11 --- .../frc2024/RobotContainer.java | 14 ++-- .../frc2024/subsystems/drive/ModuleIOSim.java | 1 - .../subsystems/flywheels/Flywheels.java | 44 +++++-------- .../rollers/GenericRollerSystemIOSim.java | 8 +-- .../frc2024/subsystems/rollers/Rollers.java | 44 +++++++------ .../subsystems/rollers/feeder/Feeder.java | 2 +- .../superstructure/Superstructure.java | 66 +++++++++++-------- .../subsystems/superstructure/arm/Arm.java | 4 +- .../superstructure/arm/ArmConstants.java | 2 +- 9 files changed, 92 insertions(+), 93 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index a38755aa..3b3172fa 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -130,7 +130,6 @@ public RobotContainer() { rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIO() {}); arm = new Arm(new ArmIOSim()); - superstructure = new Superstructure(arm); } } } @@ -236,14 +235,19 @@ 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 @@ -251,8 +255,8 @@ private void configureButtonBindings() { .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() diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java index a21d893d..9ed51192 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/ModuleIOSim.java @@ -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 = diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 883bc4e0..666ab786 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -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; @@ -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; @@ -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 @@ -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); @@ -105,7 +98,7 @@ public void periodic() { } public void runCharacterizationVolts(double volts) { - setGoal(Goal.CHARACTERIZING); + goal = Goal.CHARACTERIZING; io.runCharacterizationLeftVolts(volts); io.runCharacterizationRightVolts(volts); } @@ -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"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java index a483da9f..0709b1c6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystemIOSim.java @@ -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 @@ -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(); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java index af04a75f..d226f82d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -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; @@ -26,14 +26,14 @@ 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; @@ -41,7 +41,7 @@ public Rollers(Feeder feeder, Indexer indexer, Intake intake, RollersSensorsIO s this.intake = intake; this.sensorsIO = sensorsIO; - setDefaultCommand(idle()); + setDefaultCommand(runOnce(this::goIdle).withName("Rollers Idling")); } @Override @@ -49,13 +49,17 @@ 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); @@ -63,7 +67,7 @@ public void periodic() { 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); @@ -71,12 +75,12 @@ public void periodic() { 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); @@ -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"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java index 6ac8a094..45f7b8b8 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/feeder/Feeder.java @@ -19,7 +19,7 @@ public class Feeder extends GenericRollerSystem { @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)), diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index c1c75d9b..dc7487d2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -7,78 +7,86 @@ package org.littletonrobotics.frc2024.subsystems.superstructure; +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.RequiredArgsConstructor; -import lombok.Setter; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -@RequiredArgsConstructor public class Superstructure extends SubsystemBase { - public enum SystemState { - STOWING, - AIMING, - INTAKING, - STATION_INTAKING, + public enum Goal { + STOW, + AIM, + INTAKE, + STATION_INTAKE, AMP, PREPARE_CLIMB, CLIMB, TRAP } - @Getter private SystemState currentState = SystemState.STOWING; - @Getter @Setter private SystemState goal = SystemState.STOWING; + @Getter private Goal currentGoal = Goal.STOW; + @Getter private Goal desiredGoal = Goal.STOW; private final Arm arm; + public Superstructure(Arm arm) { + this.arm = arm; + + setDefaultCommand(runOnce(this::stow).withName("Superstructure Stowing")); + } + @Override public void periodic() { - currentState = goal; // Will change soon + if (DriverStation.isDisabled()) { + desiredGoal = Goal.STOW; + arm.stop(); + } + + currentGoal = desiredGoal; // Will change soon - switch (currentState) { - case STOWING -> arm.setGoal(Arm.Goal.STOW); - case AIMING -> arm.setGoal(Arm.Goal.AIM); - case INTAKING -> arm.setGoal(Arm.Goal.FLOOR_INTAKE); - case STATION_INTAKING -> arm.setGoal(Arm.Goal.STATION_INTAKE); + switch (currentGoal) { + case STOW -> arm.setGoal(Arm.Goal.STOW); + case AIM -> arm.setGoal(Arm.Goal.AIM); + case INTAKE -> arm.setGoal(Arm.Goal.FLOOR_INTAKE); + case STATION_INTAKE -> arm.setGoal(Arm.Goal.STATION_INTAKE); default -> {} // DO NOTHING ELSE } arm.periodic(); - Logger.recordOutput("Superstructure/GoalState", goal); - Logger.recordOutput("Superstructure/CurrentState", currentState); + Logger.recordOutput("Superstructure/GoalState", desiredGoal); + Logger.recordOutput("Superstructure/CurrentState", currentGoal); } @AutoLogOutput(key = "Superstructure/ReadyToShoot") public boolean atShootingSetpoint() { - return currentState == SystemState.AIMING && arm.atSetpoint(); + return currentGoal == Goal.AIM && arm.atSetpoint(); } - @AutoLogOutput(key = "Superstructure/AtGoalState") + @AutoLogOutput(key = "Superstructure/CompletedGoal") public boolean atGoal() { - return arm.atSetpoint(); + return currentGoal == desiredGoal && arm.atSetpoint(); } - public Command stow() { - return runOnce(() -> setGoal(SystemState.STOWING)).withName("Superstructure Stow"); + public void stow() { + desiredGoal = Goal.STOW; } public Command aim() { - return startEnd(() -> setGoal(SystemState.AIMING), () -> setGoal(SystemState.STOWING)) - .withName("Superstructure Aim"); + return startEnd(() -> desiredGoal = Goal.AIM, this::stow).withName("Superstructure Aiming"); } public Command intake() { - return startEnd(() -> setGoal(SystemState.INTAKING), () -> setGoal(SystemState.STOWING)) - .withName("Superstructure Intake"); + return startEnd(() -> desiredGoal = Goal.INTAKE, this::stow) + .withName("Superstructure Intaking"); } public Command stationIntake() { - return startEnd(() -> setGoal(SystemState.STATION_INTAKING), () -> setGoal(SystemState.STOWING)) - .withName("Superstructure Station Intake"); + return startEnd(() -> desiredGoal = Goal.STATION_INTAKE, this::stow) + .withName("Superstructure Station Intaking"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index 9067c960..2f890fe5 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; @@ -25,7 +24,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class Arm extends SubsystemBase { +public class Arm { private static final LoggedTunableNumber kP = new LoggedTunableNumber("Arm/kP", gains.kP()); private static final LoggedTunableNumber kI = new LoggedTunableNumber("Arm/kI", gains.kI()); private static final LoggedTunableNumber kD = new LoggedTunableNumber("Arm/kD", gains.kD()); @@ -128,6 +127,7 @@ public void periodic() { // Logs Logger.recordOutput("Arm/SetpointAngle", output.position); + Logger.recordOutput("Arm/SetpointVelocity", output.velocity); setpointVisualizer.update(Rotation2d.fromRadians(output.position)); goalVisualizer.update(Rotation2d.fromRadians(goal.getRads())); } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java index 46842221..f063c837 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java @@ -41,7 +41,7 @@ public class ArmConstants { public static Gains gains = switch (Constants.getRobot()) { - case SIMBOT -> new Gains(1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + case SIMBOT -> new Gains(90.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); case DEVBOT -> new Gains(1200, 0.0, 120, 6.22, 0.0, 0.0, 8.12); case COMPBOT -> new Gains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); };