From f7a25c80a9c90e167dbb0db952e4e72e50350375 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Mon, 12 Feb 2024 19:14:23 -0500 Subject: [PATCH 1/5] Rename GenericRollerSubsystem to GenericRollerSystem --- ...icRollerSubsystem.java => GenericRollerSystem.java} | 2 +- .../frc2024/subsystems/rollers/feeder/Feeder.java | 10 ++++++---- .../frc2024/subsystems/rollers/indexer/Indexer.java | 6 +++--- .../frc2024/subsystems/rollers/intake/Intake.java | 8 +++++--- 4 files changed, 15 insertions(+), 11 deletions(-) rename src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/{GenericRollerSubsystem.java => GenericRollerSystem.java} (88%) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSubsystem.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java similarity index 88% rename from src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSubsystem.java rename to src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java index 8640d18e..664bace9 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSubsystem.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/GenericRollerSystem.java @@ -5,7 +5,7 @@ import org.littletonrobotics.junction.Logger; @RequiredArgsConstructor -public abstract class GenericRollerSubsystem { +public abstract class GenericRollerSystem { protected interface VoltageGoal { DoubleSupplier getVoltageSupplier(); } 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 c77326a5..e4f174ff 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 @@ -4,13 +4,15 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSubsystem; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; -public class Feeder extends GenericRollerSubsystem { +@Setter +@Getter +public class Feeder extends GenericRollerSystem { @RequiredArgsConstructor @Getter - public enum Goal implements GenericRollerSubsystem.VoltageGoal { + public enum Goal implements GenericRollerSystem.VoltageGoal { IDLE(() -> 0.0), FLOOR_INTAKING(new LoggedTunableNumber("Feeder/FloorIntakingVoltage", 8.0)), BACKSTOPPING(new LoggedTunableNumber("Feeder/BackstoppingVoltage", -4.0)), @@ -20,7 +22,7 @@ public enum Goal implements GenericRollerSubsystem.VoltageGoal { private final DoubleSupplier voltageSupplier; } - @Getter @Setter private Feeder.Goal goal = Feeder.Goal.IDLE; + private Feeder.Goal goal = Feeder.Goal.IDLE; public Feeder(FeederIO io) { super("Feeder", io); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java index 784d8049..bd49ea97 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/indexer/Indexer.java @@ -4,12 +4,12 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSubsystem; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @Setter @Getter -public class Indexer extends GenericRollerSubsystem { +public class Indexer extends GenericRollerSystem { @RequiredArgsConstructor @Getter public enum Goal implements VoltageGoal { @@ -22,7 +22,7 @@ public enum Goal implements VoltageGoal { private final DoubleSupplier voltageSupplier; } - @Getter @Setter private Indexer.Goal goal = Indexer.Goal.IDLE; + private Indexer.Goal goal = Indexer.Goal.IDLE; public Indexer(IndexerIO io) { super("Indexer", io); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java index e1451451..ae3e9ada 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/intake/Intake.java @@ -4,10 +4,12 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSubsystem; +import org.littletonrobotics.frc2024.subsystems.rollers.GenericRollerSystem; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; -public class Intake extends GenericRollerSubsystem { +@Setter +@Getter +public class Intake extends GenericRollerSystem { @RequiredArgsConstructor @Getter public enum Goal implements VoltageGoal { @@ -19,7 +21,7 @@ public enum Goal implements VoltageGoal { private final DoubleSupplier voltageSupplier; } - @Getter @Setter private Goal goal = Goal.IDLE; + private Goal goal = Goal.IDLE; public Intake(IntakeIO io) { super("Intake", io); From 56eba5a007ca68e9b2d7f10850f8a0b462d5cfb6 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 12 Feb 2024 20:00:53 -0500 Subject: [PATCH 2/5] Use Commands for Rollers.java --- .../frc2024/RobotContainer.java | 183 +++++++----------- .../frc2024/subsystems/rollers/Rollers.java | 114 +++++++---- 2 files changed, 150 insertions(+), 147 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 1697a8dd..14357922 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -48,10 +48,10 @@ import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim; -import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; -import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIO; -import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSim; -import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOSparkFlex; +import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels; +import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO; +import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim; +import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -206,28 +206,7 @@ public RobotContainer() { AutoCommands autoCommands = new AutoCommands(drive, superstructure); - autoChooser.addOption("Drive Straight", autoCommands.driveStraight()); - - // Testing autos paths - // Function> trajectoryCommandFactory = - // trajectoryFile -> { - // Optional trajectory = - // ChoreoTrajectoryDeserializer.deserialize(trajectoryFile); - // return trajectory.map( - // traj -> - // Commands.runOnce( - // () -> - // robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - // .andThen(Commands.runOnce(() -> - // drive.setTrajectoryGoal(traj))) - // .until(drive::isTrajectoryGoalCompleted)); - // }; - // final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); - // for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { - // trajectoryCommandFactory - // .apply(trajectoryFile) - // .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); - // } + // autoChooser.addOption("Drive Straight", autoCommands.driveStraight()); // Configure the button bindings configureButtonBindings(); @@ -239,6 +218,7 @@ public RobotContainer() { * XboxController}), and then passing it to a {@link JoystickButton}. */ private void configureButtonBindings() { + // Drive Commands drive.setDefaultCommand( drive.run( () -> @@ -250,90 +230,73 @@ private void configureButtonBindings() { .onTrue(Commands.runOnce(drive::setAutoAimGoal)) .onFalse(Commands.runOnce(drive::clearAutoAimGoal)); - if (superstructure != null) { - controller - .a() - .onTrue( - Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT))) - .onFalse( - Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); - - Trigger readyToShootTrigger = - new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint()); - readyToShootTrigger - .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( - Commands.runOnce( - () -> { - superstructure.setGoalState(Superstructure.SystemState.SHOOT); - rollers.setGoal(Rollers.Goal.FEED_SHOOTER); - }, - superstructure, - rollers) - .andThen(Commands.waitSeconds(1.0)) - .andThen( - Commands.runOnce( - () -> { - superstructure.setGoalState(Superstructure.SystemState.IDLE); - rollers.setGoal(Rollers.Goal.IDLE); - }))); - - controller - .leftTrigger() - .whileTrue( - Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE), - superstructure) - .andThen( - Commands.waitSeconds(0.25), - Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.FLOOR_INTAKE), rollers), - Commands.idle()) - .finallyDo( - () -> { - rollers.setGoal(Rollers.Goal.IDLE); - superstructure.setGoalState(Superstructure.SystemState.IDLE); - })); - - controller - .leftBumper() - .whileTrue( - Commands.runOnce( - () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE), - superstructure) - .andThen( - Commands.waitSeconds(0.25), - Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.EJECT_TO_FLOOR), rollers), - Commands.idle()) - .finallyDo( - () -> { - rollers.setGoal(Rollers.Goal.IDLE); - superstructure.setGoalState(Superstructure.SystemState.IDLE); - })); - - // controller - // .a() - // .onTrue( - // Commands.runOnce( - // () -> { - // superstructure.setGoalState(Superstructure.SystemState.REVERSE_INTAKE); - // rollers.setGoal(Rollers.Goal.STATION_INTAKE); - // })) - // .onFalse( - // Commands.runOnce( - // () -> { - // superstructure.setGoalState(Superstructure.SystemState.IDLE); - // rollers.setGoal(Rollers.Goal.IDLE); - // })); - } + controller + .a() + .onTrue( + Commands.runOnce( + () -> superstructure.setGoalState(Superstructure.SystemState.PREPARE_SHOOT))) + .onFalse( + Commands.runOnce(() -> superstructure.setGoalState(Superstructure.SystemState.IDLE))); + + Trigger readyToShootTrigger = + new Trigger(() -> drive.isAutoAimGoalCompleted() && superstructure.atShootingSetpoint()); + readyToShootTrigger + .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( + Commands.runOnce( + () -> { + superstructure.setGoalState(Superstructure.SystemState.SHOOT); + rollers.setGoal(Rollers.Goal.FEED_SHOOTER); + }, + superstructure, + rollers) + .andThen(Commands.waitSeconds(1.0)) + .andThen( + Commands.runOnce( + () -> { + superstructure.setGoalState(Superstructure.SystemState.IDLE); + rollers.setGoal(Rollers.Goal.IDLE); + }))); + + controller + .leftTrigger() + .whileTrue( + Commands.runOnce( + () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE), + superstructure) + .andThen( + Commands.waitSeconds(0.25), + Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.FLOOR_INTAKE), rollers), + Commands.idle()) + .finallyDo( + () -> { + rollers.setGoal(Rollers.Goal.IDLE); + superstructure.setGoalState(Superstructure.SystemState.IDLE); + })); + + controller + .leftBumper() + .whileTrue( + Commands.runOnce( + () -> superstructure.setGoalState(Superstructure.SystemState.INTAKE), + superstructure) + .andThen( + Commands.waitSeconds(0.25), + Commands.runOnce(() -> rollers.setGoal(Rollers.Goal.EJECT_TO_FLOOR), rollers), + Commands.idle()) + .finallyDo( + () -> { + rollers.setGoal(Rollers.Goal.IDLE); + superstructure.setGoalState(Superstructure.SystemState.IDLE); + })); controller .y() 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 d3e76f9c..abda0289 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/rollers/Rollers.java @@ -1,22 +1,23 @@ package org.littletonrobotics.frc2024.subsystems.rollers; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; -import lombok.RequiredArgsConstructor; 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; import org.littletonrobotics.junction.Logger; -@RequiredArgsConstructor() public class Rollers extends SubsystemBase { private final Feeder feeder; private final Indexer indexer; private final Intake intake; - private final RollersSensorsIO sensorsIO; - private final RollersSensorsIOInputsAutoLogged inputs = new RollersSensorsIOInputsAutoLogged(); + private final RollersSensorsIO sensorsIO; + private final RollersSensorsIOInputsAutoLogged sensorInputs = + new RollersSensorsIOInputsAutoLogged(); public enum Goal { IDLE, @@ -28,44 +29,83 @@ public enum Goal { @Getter @Setter private Goal goal = Goal.IDLE; - @Override - public void periodic() { - sensorsIO.updateInputs(inputs); - Logger.processInputs("RollersSensors", inputs); + public Rollers(Feeder feeder, Indexer indexer, Intake intake, RollersSensorsIO sensorsIO) { + this.feeder = feeder; + this.indexer = indexer; + this.intake = intake; + this.sensorsIO = sensorsIO; - switch (goal) { - case IDLE -> { - feeder.setGoal(Feeder.Goal.IDLE); - indexer.setGoal(Indexer.Goal.IDLE); - intake.setGoal(Intake.Goal.IDLE); - } - case FLOOR_INTAKE -> { - feeder.setGoal(Feeder.Goal.FLOOR_INTAKING); - indexer.setGoal(Indexer.Goal.FLOOR_INTAKING); - intake.setGoal(Intake.Goal.FLOOR_INTAKING); - if (inputs.shooterStaged) { - goal = Goal.IDLE; - } - } - case STATION_INTAKE -> { - indexer.setGoal(Indexer.Goal.STATION_INTAKING); - } - case FEED_SHOOTER -> { - indexer.setGoal(Indexer.Goal.SHOOTING); - intake.setGoal(Intake.Goal.SHOOTING); - feeder.setGoal(Feeder.Goal.SHOOTING); - } - case EJECT_TO_FLOOR -> { - feeder.setGoal(Feeder.Goal.EJECTING); - indexer.setGoal(Indexer.Goal.EJECTING); - intake.setGoal(Intake.Goal.EJECTING); - } - } + setDefaultCommand(idleCommand()); + } - Logger.recordOutput("Rollers/Goal", goal); + @Override + public void periodic() { + sensorsIO.updateInputs(sensorInputs); + Logger.processInputs("RollersSensors", sensorInputs); feeder.periodic(); indexer.periodic(); intake.periodic(); } + + public Command idleCommand() { + return runOnce( + () -> { + feeder.setGoal(Feeder.Goal.IDLE); + indexer.setGoal(Indexer.Goal.IDLE); + intake.setGoal(Intake.Goal.IDLE); + }) + .andThen(Commands.idle()) + .withName("Rollers Idle"); + } + + public Command floorIntakeCommand() { + return runOnce( + () -> { + 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.IDLE); + } + }) + .andThen(Commands.idle()) + .withName("Rollers Floor Intake"); + } + + public Command stationIntakeCommand() { + return runOnce( + () -> { + feeder.setGoal(Feeder.Goal.IDLE); + indexer.setGoal(Indexer.Goal.STATION_INTAKING); + intake.setGoal(Intake.Goal.IDLE); + if (sensorInputs.shooterStaged) { // TODO: ADD THIS BANNER + indexer.setGoal(Indexer.Goal.IDLE); + } + }) + .andThen(Commands.idle()) + .withName("Rollers Station Intake"); + } + + public Command ejectFloorCommand() { + return runOnce( + () -> { + feeder.setGoal(Feeder.Goal.EJECTING); + indexer.setGoal(Indexer.Goal.EJECTING); + intake.setGoal(Intake.Goal.EJECTING); + }) + .andThen(Commands.idle()) + .withName("Rollers Eject Floor"); + } + + public Command feedShooterCommand() { + return runOnce( + () -> { + feeder.setGoal(Feeder.Goal.SHOOTING); + indexer.setGoal(Indexer.Goal.SHOOTING); + intake.setGoal(Intake.Goal.IDLE); + }) + .andThen(Commands.idle()) + .withName("Rollers Feed Shooter"); + } } From 0d47d745d136fcd4e0375c292c16beb44c1dd1df Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 12 Feb 2024 20:06:11 -0500 Subject: [PATCH 3/5] Move Arm and Flywheels into own packages Change Flywheels run using commands --- .../littletonrobotics/frc2024/RobotState.java | 6 +- .../flywheels/FlywheelConstants.java | 28 +++++ .../flywheels/Flywheels.java | 105 +++++++++++------- .../flywheels/FlywheelsIO.java | 2 +- .../flywheels/FlywheelsIOSim.java | 8 +- .../flywheels/FlywheelsIOSparkFlex.java | 20 ++-- .../superstructure/Superstructure.java | 2 +- .../SuperstructureConstants.java | 80 ------------- .../subsystems/superstructure/arm/Arm.java | 2 +- .../superstructure/arm/ArmConstants.java | 52 +++++++++ .../superstructure/arm/ArmIOKrakenFOC.java | 2 +- .../superstructure/arm/ArmIOSim.java | 2 +- .../superstructure/arm/ArmVisualizer.java | 40 +++++++ 13 files changed, 204 insertions(+), 145 deletions(-) create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java rename src/main/java/org/littletonrobotics/frc2024/subsystems/{superstructure => }/flywheels/Flywheels.java (57%) rename src/main/java/org/littletonrobotics/frc2024/subsystems/{superstructure => }/flywheels/FlywheelsIO.java (94%) rename src/main/java/org/littletonrobotics/frc2024/subsystems/{superstructure => }/flywheels/FlywheelsIOSim.java (90%) rename src/main/java/org/littletonrobotics/frc2024/subsystems/{superstructure => }/flywheels/FlywheelsIOSparkFlex.java (85%) delete mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java create mode 100644 src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 5a08d693..358c3563 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -13,7 +13,7 @@ import java.util.NoSuchElementException; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants; -import org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants; +import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @@ -185,9 +185,9 @@ public AimingParameters getAimingParameters() { new AimingParameters( targetVehicleDirection, new Rotation2d( - targetDistance - SuperstructureConstants.ArmConstants.armOrigin.getX(), + targetDistance - ArmConstants.armOrigin.getX(), FieldConstants.Speaker.centerSpeakerOpening.getZ() - - SuperstructureConstants.ArmConstants.armOrigin.getY() + - ArmConstants.armOrigin.getY() + shotHeightCompensation.get()), feedVelocity); Logger.recordOutput("RobotState/AimingParameters/Direction", latestParameters.driveHeading); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java new file mode 100644 index 00000000..9c15b094 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java @@ -0,0 +1,28 @@ +package org.littletonrobotics.frc2024.subsystems.flywheels; + +import org.littletonrobotics.frc2024.Constants; + +public class FlywheelConstants { + public static double reduction = (1.0 / 2.0); + public static double shooterToleranceRPM = 100.0; + public static int leftID = 5; + public static int rightID = 4; + + public static Config config = + switch (Constants.getRobot()) { + case COMPBOT -> null; + case DEVBOT -> new Config(5, 4, (1.0 / 2.0), 100.0); + case SIMBOT -> new Config(0, 0, (1.0 / 2.0), 100.0); + }; + + public static Gains gains = + switch (Constants.getRobot()) { + case COMPBOT -> null; + case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0); + 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 Gains(double kP, double kI, double kD, double kS, double kV, double kA) { + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java similarity index 57% rename from src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java rename to src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java index 0c48fe0d..6616503d 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -1,15 +1,21 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; +package org.littletonrobotics.frc2024.subsystems.flywheels; -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FlywheelConstants.*; +import static org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelConstants.*; 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 lombok.Getter; +import lombok.RequiredArgsConstructor; import lombok.Setter; +import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOInputsAutoLogged; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import java.util.function.DoubleSupplier; + public class Flywheels extends SubsystemBase { private static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheels/kP", gains.kP()); private static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheels/kI", gains.kI()); @@ -17,7 +23,6 @@ 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 = @@ -32,35 +37,26 @@ public class Flywheels extends SubsystemBase { private static LoggedTunableNumber intakingRightRPM = new LoggedTunableNumber("Superstructure/IntakingRightRPM", -2000.0); private static final LoggedTunableNumber shooterTolerance = - new LoggedTunableNumber("Flywheels/ToleranceRPM", shooterToleranceRPM); + new LoggedTunableNumber("Flywheels/ToleranceRPM", config.toleranceRPM()); private final FlywheelsIO io; private final FlywheelsIOInputsAutoLogged inputs = new FlywheelsIOInputsAutoLogged(); - private Double leftSetpointRpm = null; - private Double rightSetpointRPM = null; - + @RequiredArgsConstructor public enum Goal { - IDLE, - SHOOTING, - INTAKING, - - CHARACTERIZATION + STOP(() -> 0.0, () -> 0.0), + IDLE(idleLeftRPM, idleRightRPM), + SHOOTING(shootingLeftRPM, shootingRightRPM), + INTAKING(intakingLeftRPM, intakingRightRPM), + CHARACTERIZING(() -> 0.0, () -> 0.0); + + private final DoubleSupplier leftSetpoint; + private final DoubleSupplier rightSetpoint; } @Getter @Setter private Goal goal = Goal.IDLE; - - private void setSetpoint(double left, double right) { - leftSetpointRpm = left; - rightSetpointRPM = right; - io.runVelocity(left, right); - } - - private void stop() { - leftSetpointRpm = null; - rightSetpointRPM = null; - io.stop(); - } + private double characterizationVolts = 0.0; + private boolean characterizing = false; public Flywheels(FlywheelsIO io) { System.out.println("[Init] Creating Shooter"); @@ -69,39 +65,40 @@ public Flywheels(FlywheelsIO io) { @Override public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Flywheels", inputs); + // check controllers LoggedTunableNumber.ifChanged(hashCode(), pid -> io.setPID(pid[0], pid[1], pid[2]), kP, kI, kD); LoggedTunableNumber.ifChanged( hashCode(), kSVA -> io.setFF(kSVA[0], kSVA[1], kSVA[2]), kS, kV, kA); - io.updateInputs(inputs); - Logger.processInputs("Flywheels", inputs); - if (DriverStation.isDisabled()) { - stop(); - setGoal(Goal.IDLE); - } else { - switch (goal) { - case IDLE -> io.stop(); - case INTAKING -> setSetpoint(intakingLeftRPM.get(), intakingRightRPM.get()); - case SHOOTING -> setSetpoint(shootingLeftRPM.get(), shootingRightRPM.get()); - } + setGoal(Goal.STOP); + } + + switch (goal) { + case STOP -> io.stop(); + case CHARACTERIZING -> {} // Handled by runCharacterizationVolts + default -> io.runVelocity( + goal.leftSetpoint.getAsDouble(), + goal.rightSetpoint.getAsDouble()); } Logger.recordOutput("Flywheels/Goal", goal); - Logger.recordOutput( - "Flywheels/LeftSetpointRPM", leftSetpointRpm != null ? leftSetpointRpm : 0.0); - Logger.recordOutput( - "Flywheels/RightSetpointRPM", rightSetpointRPM != null ? rightSetpointRPM : 0.0); + Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.leftSetpoint.getAsDouble()); + Logger.recordOutput("Flywheels/RightSetpointRPM", goal.rightSetpoint.getAsDouble()); Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm); Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm); } public void runLeftCharacterizationVolts(double volts) { + setGoal(Goal.CHARACTERIZING); io.runCharacterizationLeftVolts(volts); } public void runRightCharacterizationVolts(double volts) { + setGoal(Goal.CHARACTERIZING); io.runCharacterizationRightVolts(volts); } @@ -115,9 +112,31 @@ public double getRightCharacterizationVelocity() { @AutoLogOutput(key = "Shooter/AtSetpoint") public boolean atSetpoint() { - return leftSetpointRpm != null - && rightSetpointRPM != null - && Math.abs(inputs.leftVelocityRpm - leftSetpointRpm) <= shooterTolerance.get() - && Math.abs(inputs.rightVelocityRpm - rightSetpointRPM) <= shooterTolerance.get(); + return Math.abs(inputs.leftVelocityRpm - goal.leftSetpoint.getAsDouble()) <= shooterTolerance.get() + && Math.abs(inputs.rightVelocityRpm - goal.rightSetpoint.getAsDouble()) <= shooterTolerance.get(); + } + + public Command stopCommand() { + return runOnce(() -> setGoal(Goal.STOP)) + .andThen(Commands.idle()) + .withName("Flywheels Stop"); + } + + public Command idleCommand() { + return runOnce(() -> setGoal(Goal.IDLE)) + .andThen(Commands.idle()) + .withName("Flywheels Idle"); + } + + public Command shootingCommand() { + return runOnce(() -> setGoal(Goal.SHOOTING)) + .andThen(Commands.idle()) + .withName("Flywheels Shooting"); + } + + public Command intakingCommand() { + return runOnce(() -> setGoal(Goal.INTAKING)) + .andThen(Commands.idle()) + .withName("Flywheels Intaking"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIO.java similarity index 94% rename from src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java rename to src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIO.java index ce155b32..9d914beb 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIO.java @@ -1,4 +1,4 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; +package org.littletonrobotics.frc2024.subsystems.flywheels; import org.littletonrobotics.junction.AutoLog; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java similarity index 90% rename from src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java rename to src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java index 4e686c4d..eb8b1f55 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSim.java @@ -1,6 +1,6 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; +package org.littletonrobotics.frc2024.subsystems.flywheels; -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FlywheelConstants.*; +import static org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; @@ -11,9 +11,9 @@ public class FlywheelsIOSim implements FlywheelsIO { private final FlywheelSim leftSim = - new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); + new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292); private final FlywheelSim rightSim = - new FlywheelSim(DCMotor.getKrakenX60Foc(1), reduction, 0.00363458292); + new FlywheelSim(DCMotor.getKrakenX60Foc(1), config.reduction(), 0.00363458292); private final PIDController leftController = new PIDController(gains.kP(), gains.kI(), gains.kD()); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java similarity index 85% rename from src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java rename to src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java index 7a37950c..cd29ffd6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/flywheels/FlywheelsIOSparkFlex.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java @@ -1,6 +1,6 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure.flywheels; +package org.littletonrobotics.frc2024.subsystems.flywheels; -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.FlywheelConstants.*; +import static org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelConstants.*; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkFlex; @@ -24,8 +24,8 @@ public class FlywheelsIOSparkFlex implements FlywheelsIO { public FlywheelsIOSparkFlex() { // Init Hardware - leftMotor = new CANSparkFlex(leftID, CANSparkFlex.MotorType.kBrushless); - rightMotor = new CANSparkFlex(rightID, CANSparkFlex.MotorType.kBrushless); + leftMotor = new CANSparkFlex(config.leftID(), CANSparkFlex.MotorType.kBrushless); + rightMotor = new CANSparkFlex(config.rightID(), CANSparkFlex.MotorType.kBrushless); leftEncoder = leftMotor.getEncoder(); rightEncoder = rightMotor.getEncoder(); @@ -64,14 +64,14 @@ public FlywheelsIOSparkFlex() { @Override public void updateInputs(FlywheelsIOInputs inputs) { - inputs.leftPositionRads = Units.rotationsToRadians(leftEncoder.getPosition()) / reduction; - inputs.leftVelocityRpm = leftEncoder.getVelocity() / reduction; + inputs.leftPositionRads = Units.rotationsToRadians(leftEncoder.getPosition()) / config.reduction(); + inputs.leftVelocityRpm = leftEncoder.getVelocity() / config.reduction(); inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); inputs.leftOutputCurrent = leftMotor.getOutputCurrent(); inputs.leftTempCelsius = leftMotor.getMotorTemperature(); - inputs.rightPositionRads = Units.rotationsToRadians(rightEncoder.getPosition()) / reduction; - inputs.rightVelocityRpm = rightEncoder.getVelocity() / reduction; + inputs.rightPositionRads = Units.rotationsToRadians(rightEncoder.getPosition()) / config.reduction(); + inputs.rightVelocityRpm = rightEncoder.getVelocity() / config.reduction(); inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); inputs.rightOutputCurrent = rightMotor.getOutputCurrent(); inputs.rightTempCelsius = rightMotor.getMotorTemperature(); @@ -86,13 +86,13 @@ public void runVolts(double leftVolts, double rightVolts) { @Override public void runVelocity(double leftRpm, double rightRpm) { leftController.setReference( - leftRpm * reduction, + leftRpm * config.reduction(), CANSparkBase.ControlType.kVelocity, 0, ff.calculate(leftRpm), SparkPIDController.ArbFFUnits.kVoltage); rightController.setReference( - rightRpm * reduction, + rightRpm * config.reduction(), CANSparkBase.ControlType.kVelocity, 0, ff.calculate(rightRpm), 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 0cacc437..a33b00a2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -8,7 +8,7 @@ import lombok.Setter; import org.littletonrobotics.frc2024.RobotState; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; -import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.Flywheels; +import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java deleted file mode 100644 index e45dfa79..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/SuperstructureConstants.java +++ /dev/null @@ -1,80 +0,0 @@ -package org.littletonrobotics.frc2024.subsystems.superstructure; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -import org.littletonrobotics.frc2024.Constants; - -public class SuperstructureConstants { - - public static class FlywheelConstants { - // encoder / flywheelReduction = flywheel - public static double reduction = (1.0 / 2.0); - public static double shooterToleranceRPM = 100.0; - public static int leftID = 5; - public static int rightID = 4; - - public static Gains gains = - switch (Constants.getRobot()) { - case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0); - case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0); - case COMPBOT -> null; - }; - - public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} - } - - public static class IntakeConstants { - public static double reduction = (18.0 / 12.0); - public static int id = - switch (Constants.getRobot()) { - default -> 45; - }; - public static boolean inverted = - switch (Constants.getRobot()) { - default -> false; - }; - } - - public static class ArmConstants { - // reduction is 12:62 18:60 12:65 - public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); - public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); - public static Translation2d armOrigin = - new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); - public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); - public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); - - public static int leaderID = 25; - public static int followerID = 26; - public static int armEncoderID = 42; - - public static boolean leaderInverted = false; - public static boolean followerInverted = false; - - /** The offset of the arm encoder in rotations. */ - public static double armEncoderOffsetRotations = - Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0); - - public static double armLength = - switch (Constants.getRobot()) { - case DEVBOT -> Units.inchesToMeters(24.8); - default -> Units.inchesToMeters(25.866); - }; - - public static Gains gains = - switch (Constants.getRobot()) { - case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01); - 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); - }; - - public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10); - - public record ProfileConstraints( - double cruiseVelocityRadPerSec, double accelerationRadPerSec2) {} - - public record Gains( - double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) {} - } -} 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 14446fc6..a5480a15 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 @@ -1,6 +1,6 @@ package org.littletonrobotics.frc2024.subsystems.superstructure.arm; -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ArmConstants.*; +import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; 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 new file mode 100644 index 00000000..664657db --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmConstants.java @@ -0,0 +1,52 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.arm; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import org.littletonrobotics.frc2024.Constants; + +public class ArmConstants { + // reduction is 12:62 18:60 12:65 + public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); + public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); + public static Translation2d armOrigin = + new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); + public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); + public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); + + public static int leaderID = 25; + public static int followerID = 26; + public static int armEncoderID = 42; + + public static boolean leaderInverted = false; + public static boolean followerInverted = false; + + /** + * The offset of the arm encoder in rotations. + */ + public static double armEncoderOffsetRotations = + Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0); + + public static double armLength = + switch (Constants.getRobot()) { + case DEVBOT -> Units.inchesToMeters(24.8); + default -> Units.inchesToMeters(25.866); + }; + + public static Gains gains = + switch (Constants.getRobot()) { + case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01); + 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); + }; + + public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10); + + public record ProfileConstraints( + double cruiseVelocityRadPerSec, double accelerationRadPerSec2) { + } + + public record Gains( + double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) { + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java index 1ed6ed22..9a4a1ec3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java @@ -1,6 +1,6 @@ package org.littletonrobotics.frc2024.subsystems.superstructure.arm; -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ArmConstants.*; +import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java index f3499b2f..fc18c749 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java @@ -1,6 +1,6 @@ package org.littletonrobotics.frc2024.subsystems.superstructure.arm; -import static org.littletonrobotics.frc2024.subsystems.superstructure.SuperstructureConstants.ArmConstants.*; +import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java new file mode 100644 index 00000000..6fc06ef2 --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmVisualizer.java @@ -0,0 +1,40 @@ +package org.littletonrobotics.frc2024.subsystems.superstructure.arm; + +import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import org.littletonrobotics.junction.Logger; + +public class ArmVisualizer { + private final Mechanism2d mechanism; + private final MechanismRoot2d root; + private final MechanismLigament2d arm; + private final String key; + + public ArmVisualizer(String key, Color color) { + this.key = key; + mechanism = new Mechanism2d(3.0, 3.0, new Color8Bit(Color.kAntiqueWhite)); + root = mechanism.getRoot("pivot", 1.0, 0.4); + arm = new MechanismLigament2d("arm", armLength, 20.0, 6, new Color8Bit(color)); + root.append(arm); + } + + /** Update arm visualizer with current arm angle */ + public void update(Rotation2d angle) { + arm.setAngle(angle); + Logger.recordOutput("Arm/" + key + "Mechanism2d", mechanism); + + // Log 3d poses + Pose3d pivot = + new Pose3d( + armOrigin.getX(), 0.0, armOrigin.getY(), new Rotation3d(0.0, -angle.getRadians(), 0.0)); + Logger.recordOutput("Arm/" + key + "3d", pivot); + } +} From ba35a761d2bd12b9ee19e53a90776e1a4119bad7 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 12 Feb 2024 20:08:27 -0500 Subject: [PATCH 4/5] Move Arm and Flywheels into own packages Change Flywheels run using commands --- .../flywheels/FlywheelConstants.java | 33 ++++++-------- .../subsystems/flywheels/Flywheels.java | 43 +++++++++++-------- .../flywheels/FlywheelsIOSparkFlex.java | 18 ++++---- 3 files changed, 48 insertions(+), 46 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java index 9c15b094..99b00787 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelConstants.java @@ -3,26 +3,21 @@ import org.littletonrobotics.frc2024.Constants; public class FlywheelConstants { - public static double reduction = (1.0 / 2.0); - public static double shooterToleranceRPM = 100.0; - public static int leftID = 5; - public static int rightID = 4; + public static Config config = + switch (Constants.getRobot()) { + case COMPBOT -> null; + case DEVBOT -> new Config(5, 4, (1.0 / 2.0), 100.0); + case SIMBOT -> new Config(0, 0, (1.0 / 2.0), 100.0); + }; - public static Config config = - switch (Constants.getRobot()) { - case COMPBOT -> null; - case DEVBOT -> new Config(5, 4, (1.0 / 2.0), 100.0); - case SIMBOT -> new Config(0, 0, (1.0 / 2.0), 100.0); - }; + public static Gains gains = + switch (Constants.getRobot()) { + case COMPBOT -> null; + case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0); + case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.09078, 0.00103, 0.0); + }; - public static Gains gains = - switch (Constants.getRobot()) { - case COMPBOT -> null; - case DEVBOT -> new Gains(0.0006, 0.0, 0.05, 0.33329, 0.00083, 0.0); - 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) { - } + public record Gains(double kP, double kI, double kD, double kS, double kV, double kA) {} } 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 6616503d..cd2d283a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -6,6 +6,7 @@ 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; @@ -14,8 +15,6 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -import java.util.function.DoubleSupplier; - public class Flywheels extends SubsystemBase { private static final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheels/kP", gains.kP()); private static final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheels/kI", gains.kI()); @@ -52,6 +51,14 @@ public enum Goal { private final DoubleSupplier leftSetpoint; private final DoubleSupplier rightSetpoint; + + private double getLeftSetpoint() { + return leftSetpoint.getAsDouble(); + } + + private double getRightSetpoint() { + return rightSetpoint.getAsDouble(); + } } @Getter @Setter private Goal goal = Goal.IDLE; @@ -61,6 +68,8 @@ public enum Goal { public Flywheels(FlywheelsIO io) { System.out.println("[Init] Creating Shooter"); this.io = io; + + setDefaultCommand(idleCommand()); } @Override @@ -80,14 +89,12 @@ public void periodic() { switch (goal) { case STOP -> io.stop(); case CHARACTERIZING -> {} // Handled by runCharacterizationVolts - default -> io.runVelocity( - goal.leftSetpoint.getAsDouble(), - goal.rightSetpoint.getAsDouble()); + default -> io.runVelocity(goal.getLeftSetpoint(), goal.getRightSetpoint()); } Logger.recordOutput("Flywheels/Goal", goal); - Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.leftSetpoint.getAsDouble()); - Logger.recordOutput("Flywheels/RightSetpointRPM", goal.rightSetpoint.getAsDouble()); + Logger.recordOutput("Flywheels/LeftSetpointRPM", goal.getLeftSetpoint()); + Logger.recordOutput("Flywheels/RightSetpointRPM", goal.getRightSetpoint()); Logger.recordOutput("Flywheels/LeftRPM", inputs.leftVelocityRpm); Logger.recordOutput("Flywheels/RightRPM", inputs.rightVelocityRpm); } @@ -112,31 +119,29 @@ public double getRightCharacterizationVelocity() { @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(); + return Math.abs(inputs.leftVelocityRpm - goal.leftSetpoint.getAsDouble()) + <= shooterTolerance.get() + && Math.abs(inputs.rightVelocityRpm - goal.rightSetpoint.getAsDouble()) + <= shooterTolerance.get(); } public Command stopCommand() { - return runOnce(() -> setGoal(Goal.STOP)) - .andThen(Commands.idle()) - .withName("Flywheels Stop"); + return runOnce(() -> setGoal(Goal.STOP)).andThen(Commands.idle()).withName("Flywheels Stop"); } public Command idleCommand() { - return runOnce(() -> setGoal(Goal.IDLE)) - .andThen(Commands.idle()) - .withName("Flywheels Idle"); + return runOnce(() -> setGoal(Goal.IDLE)).andThen(Commands.idle()).withName("Flywheels Idle"); } public Command shootingCommand() { return runOnce(() -> setGoal(Goal.SHOOTING)) - .andThen(Commands.idle()) - .withName("Flywheels Shooting"); + .andThen(Commands.idle()) + .withName("Flywheels Shooting"); } public Command intakingCommand() { return runOnce(() -> setGoal(Goal.INTAKING)) - .andThen(Commands.idle()) - .withName("Flywheels Intaking"); + .andThen(Commands.idle()) + .withName("Flywheels Intaking"); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java index cd29ffd6..3c8515d3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/FlywheelsIOSparkFlex.java @@ -11,14 +11,14 @@ public class FlywheelsIOSparkFlex implements FlywheelsIO { // Hardware - private CANSparkFlex leftMotor; - private CANSparkFlex rightMotor; - private RelativeEncoder leftEncoder; - private RelativeEncoder rightEncoder; + private final CANSparkFlex leftMotor; + private final CANSparkFlex rightMotor; + private final RelativeEncoder leftEncoder; + private final RelativeEncoder rightEncoder; // Controllers - private SparkPIDController leftController; - private SparkPIDController rightController; + private final SparkPIDController leftController; + private final SparkPIDController rightController; // Open loop private SimpleMotorFeedforward ff = new SimpleMotorFeedforward(0.0, 0.0, 0.0); @@ -64,13 +64,15 @@ public FlywheelsIOSparkFlex() { @Override public void updateInputs(FlywheelsIOInputs inputs) { - inputs.leftPositionRads = Units.rotationsToRadians(leftEncoder.getPosition()) / config.reduction(); + inputs.leftPositionRads = + Units.rotationsToRadians(leftEncoder.getPosition()) / config.reduction(); inputs.leftVelocityRpm = leftEncoder.getVelocity() / config.reduction(); inputs.leftAppliedVolts = leftMotor.getAppliedOutput(); inputs.leftOutputCurrent = leftMotor.getOutputCurrent(); inputs.leftTempCelsius = leftMotor.getMotorTemperature(); - inputs.rightPositionRads = Units.rotationsToRadians(rightEncoder.getPosition()) / config.reduction(); + inputs.rightPositionRads = + Units.rotationsToRadians(rightEncoder.getPosition()) / config.reduction(); inputs.rightVelocityRpm = rightEncoder.getVelocity() / config.reduction(); inputs.rightAppliedVolts = rightMotor.getAppliedOutput(); inputs.rightOutputCurrent = rightMotor.getOutputCurrent(); From bc9936d2bab2cb5244c2ea0c12d78490deeafffa Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 12 Feb 2024 21:12:53 -0500 Subject: [PATCH 5/5] Arm uses commands Disable superstructure --- .../frc2024/RobotContainer.java | 13 +- .../subsystems/flywheels/Flywheels.java | 1 - .../superstructure/Superstructure.java | 41 +----- .../subsystems/superstructure/arm/Arm.java | 103 +++++++------ .../superstructure/arm/ArmConstants.java | 81 +++++----- .../subsystems/superstructure/arm/ArmIO.java | 9 +- .../superstructure/arm/ArmIOKrakenFOC.java | 138 ++++++++++-------- .../superstructure/arm/ArmIOSim.java | 42 ++---- 8 files changed, 200 insertions(+), 228 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 14357922..3064c21c 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -31,6 +31,10 @@ import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIONorthstar; import org.littletonrobotics.frc2024.subsystems.drive.*; +import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels; +import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO; +import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim; +import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex; import org.littletonrobotics.frc2024.subsystems.rollers.Rollers; import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIO; import org.littletonrobotics.frc2024.subsystems.rollers.RollersSensorsIOReal; @@ -48,10 +52,6 @@ import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIO; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmIOSim; -import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels; -import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIO; -import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSim; -import org.littletonrobotics.frc2024.subsystems.flywheels.FlywheelsIOSparkFlex; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -102,9 +102,8 @@ public RobotContainer() { intake = new Intake(new IntakeIOKrakenFOC()); rollers = new Rollers(feeder, indexer, intake, new RollersSensorsIOReal()); - arm = new Arm(new ArmIOKrakenFOC()); flywheels = new Flywheels(new FlywheelsIOSparkFlex()); - superstructure = new Superstructure(arm, flywheels); + arm = new Arm(new ArmIOKrakenFOC()); aprilTagVision = new AprilTagVision( @@ -125,8 +124,6 @@ public RobotContainer() { new ModuleIOSim(DriveConstants.moduleConfigs[3])); arm = new Arm(new ArmIOSim()); flywheels = new Flywheels(new FlywheelsIOSim()); - // intake = new Intake(new IntakeIOSim()); - // feeder = new Feeder(new FeederIOSim()); superstructure = new Superstructure(arm, flywheels); } case COMPBOT -> { 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 cd2d283a..fc07612f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/flywheels/Flywheels.java @@ -10,7 +10,6 @@ import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.subsystems.superstructure.flywheels.FlywheelsIOInputsAutoLogged; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; 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 a33b00a2..2c6d6f91 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -1,28 +1,17 @@ package org.littletonrobotics.frc2024.subsystems.superstructure; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import lombok.Getter; import lombok.RequiredArgsConstructor; import lombok.Setter; -import org.littletonrobotics.frc2024.RobotState; -import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.frc2024.subsystems.flywheels.Flywheels; -import org.littletonrobotics.frc2024.util.LoggedTunableNumber; +import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @RequiredArgsConstructor public class Superstructure extends SubsystemBase { - private static LoggedTunableNumber armIdleSetpointDegrees = - new LoggedTunableNumber("Superstructure/ArmIdleSetpointDegrees", 20.0); - private static LoggedTunableNumber armStationIntakeSetpointDegrees = - new LoggedTunableNumber("Superstructure/ArmStationIntakeSetpointDegrees", 45.0); - private static LoggedTunableNumber armIntakeSetpointDegrees = - new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); - private static LoggedTunableNumber followThroughTime = - new LoggedTunableNumber("Superstructure/FollowthroughTimeSecs", 0.5); public enum SystemState { PREPARE_SHOOT, @@ -62,34 +51,6 @@ public void periodic() { case SHOOT -> currentState = SystemState.SHOOT; } - switch (currentState) { - case IDLE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armIdleSetpointDegrees.get())); - flywheels.setGoal(Flywheels.Goal.IDLE); - } - case INTAKE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); - flywheels.setGoal(Flywheels.Goal.IDLE); - } - case STATION_INTAKE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armStationIntakeSetpointDegrees.get())); - flywheels.setGoal(Flywheels.Goal.INTAKING); - } - case REVERSE_INTAKE -> { - arm.setSetpoint(Rotation2d.fromDegrees(armIntakeSetpointDegrees.get())); - } - case PREPARE_SHOOT -> { - var aimingParams = RobotState.getInstance().getAimingParameters(); - arm.setSetpoint(aimingParams.armAngle()); - flywheels.setGoal(Flywheels.Goal.SHOOTING); - } - case SHOOT -> { - var aimingParams = RobotState.getInstance().getAimingParameters(); - flywheels.setGoal(Flywheels.Goal.SHOOTING); - arm.setSetpoint(aimingParams.armAngle()); - } - } - Logger.recordOutput("Superstructure/GoalState", goalState); Logger.recordOutput("Superstructure/CurrentState", currentState); } 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 a5480a15..0ffd4e19 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 @@ -7,14 +7,14 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; 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.RobotState; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -28,44 +28,49 @@ public class Arm extends SubsystemBase { private static final LoggedTunableNumber kA = new LoggedTunableNumber("Arm/kA", gains.ffkA()); private static final LoggedTunableNumber kG = new LoggedTunableNumber("Arm/kG", gains.ffkG()); private static final LoggedTunableNumber armVelocity = - new LoggedTunableNumber("Arm/Velocity", profileConstraints.cruiseVelocityRadPerSec()); + new LoggedTunableNumber("Arm/Velocity", profileConstraints.maxVelocity); private static final LoggedTunableNumber armAcceleration = - new LoggedTunableNumber("Arm/Acceleration", profileConstraints.accelerationRadPerSec2()); + new LoggedTunableNumber("Arm/Acceleration", profileConstraints.maxAcceleration); private static final LoggedTunableNumber armToleranceDegreees = new LoggedTunableNumber("Arm/ToleranceDegrees", positionTolerance.getDegrees()); private static final LoggedTunableNumber armLowerLimit = new LoggedTunableNumber("Arm/LowerLimitDegrees", 15.0); private static final LoggedTunableNumber armUpperLimit = new LoggedTunableNumber("Arm/UpperLimitDegrees", 90.0); + private static LoggedTunableNumber armStowDegrees = + new LoggedTunableNumber("Superstructure/ArmStowDegrees", 20.0); + private static LoggedTunableNumber armStationIntakeDegrees = + new LoggedTunableNumber("Superstructure/ArmStationIntakeDegrees", 45.0); + private static LoggedTunableNumber armIntakeDegrees = + new LoggedTunableNumber("Superstructure/ArmIntakeDegrees", 40.0); + + @RequiredArgsConstructor + public enum Goal { + STOW(() -> Units.degreesToRadians(armStowDegrees.get())), + FLOOR_INTAKE(() -> Units.degreesToRadians(armIntakeDegrees.get())), + STATION_INTAKE(() -> Units.degreesToRadians(armStowDegrees.get())), + AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getRadians()); + + private final DoubleSupplier armSetpointSupplier; + + private double getArmSetpointRads() { + return armSetpointSupplier.getAsDouble(); + } + } + + @Getter @Setter Goal goal; private final ArmIO io; private final ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged(); - private final Mechanism2d armMechanism; - private final MechanismRoot2d armRoot; - private final MechanismLigament2d armMeasured; - private boolean homed = false; - @Setter private Rotation2d setpoint = null; - - // private final MechanismLigament2d armSetpoint; public Arm(ArmIO io) { System.out.println("[Init] Creating Arm"); this.io = io; io.setBrakeMode(true); - // Create a mechanism - armMechanism = new Mechanism2d(2, 3, new Color8Bit(Color.kAntiqueWhite)); - armRoot = armMechanism.getRoot("Arm Joint", armOrigin.getX(), armOrigin.getY()); - armMeasured = - new MechanismLigament2d( - "Arm Measured", - armLength, - Units.radiansToDegrees(inputs.armPositionRads), - 2.0, - new Color8Bit(Color.kBlack)); - armRoot.append(armMeasured); + setDefaultCommand(stowCommand()); } @Override @@ -85,26 +90,17 @@ public void periodic() { armVelocity, armAcceleration); - if (setpoint != null) { - io.setSetpoint( - MathUtil.clamp( - setpoint.getRadians(), - Units.degreesToRadians(armLowerLimit.get()), - Units.degreesToRadians(armUpperLimit.get()))); - } + io.runSetpoint( + MathUtil.clamp( + goal.getArmSetpointRads(), + Units.degreesToRadians(armLowerLimit.get()), + Units.degreesToRadians(armUpperLimit.get()))); if (DriverStation.isDisabled()) { io.stop(); } - // Logs - armMeasured.setAngle(Units.radiansToDegrees(inputs.armPositionRads)); - Logger.recordOutput("Arm/Mechanism", armMechanism); - } - - public void runVolts(double volts) { - setpoint = null; - io.runVolts(volts); + Logger.recordOutput("Arm/Goal", goal); } public void stop() { @@ -117,7 +113,7 @@ public Rotation2d getAngle() { @AutoLogOutput(key = "Arm/SetpointAngle") public Rotation2d getSetpoint() { - return setpoint != null ? setpoint : new Rotation2d(); + return Rotation2d.fromRadians(goal.getArmSetpointRads()); } @AutoLogOutput(key = "Arm/Homed") @@ -127,9 +123,28 @@ public boolean homed() { @AutoLogOutput(key = "Arm/AtSetpoint") public boolean atSetpoint() { - return setpoint != null - && Math.abs(Rotation2d.fromRadians(inputs.armPositionRads).minus(setpoint).getDegrees()) - <= armToleranceDegreees.get(); + return Math.abs(inputs.armPositionRads - goal.getArmSetpointRads()) + <= Units.degreesToRadians(armToleranceDegreees.get()); + } + + public Command stowCommand() { + return runOnce(() -> setGoal(Goal.STOW)).andThen(Commands.idle()).withName("Arm Stow"); + } + + public Command intakeCommand() { + return runOnce(() -> setGoal(Goal.FLOOR_INTAKE)) + .andThen(Commands.idle()) + .withName("Arm Intake"); + } + + public Command stationIntakeCommand() { + return runOnce(() -> setGoal(Goal.STATION_INTAKE)) + .andThen(Commands.idle()) + .withName("Arm Station Intake"); + } + + public Command aimCommand() { + return runOnce(() -> setGoal(Goal.AIM)).andThen(Commands.idle()).withName("Arm Aim"); } public Command getStaticCurrent() { 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 664657db..f76dbf65 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 @@ -2,51 +2,46 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import org.littletonrobotics.frc2024.Constants; public class ArmConstants { - // reduction is 12:62 18:60 12:65 - public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); - public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); - public static Translation2d armOrigin = - new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); - public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); - public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); - - public static int leaderID = 25; - public static int followerID = 26; - public static int armEncoderID = 42; - - public static boolean leaderInverted = false; - public static boolean followerInverted = false; - - /** - * The offset of the arm encoder in rotations. - */ - public static double armEncoderOffsetRotations = - Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0); - - public static double armLength = - switch (Constants.getRobot()) { - case DEVBOT -> Units.inchesToMeters(24.8); - default -> Units.inchesToMeters(25.866); - }; - - public static Gains gains = - switch (Constants.getRobot()) { - case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01); - 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); - }; - - public static ProfileConstraints profileConstraints = new ProfileConstraints(2 * Math.PI, 10); - - public record ProfileConstraints( - double cruiseVelocityRadPerSec, double accelerationRadPerSec2) { - } - - public record Gains( - double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) { - } + // reduction is 12:62 18:60 12:65 + public static double reduction = (62.0 / 12.0) * (60.0 / 18.0) * (65.0 / 12.0); + public static Rotation2d positionTolerance = Rotation2d.fromDegrees(3.0); + public static Translation2d armOrigin = + new Translation2d(-Units.inchesToMeters(9.11), Units.inchesToMeters(11.75)); + public static Rotation2d minAngle = Rotation2d.fromDegrees(10.0); + public static Rotation2d maxAngle = Rotation2d.fromDegrees(110.0); + + public static int leaderID = 25; + public static int followerID = 26; + public static int armEncoderID = 42; + + public static boolean leaderInverted = false; + public static boolean followerInverted = false; + + /** The offset of the arm encoder in rotations. */ + public static double armEncoderOffsetRotations = + Units.radiansToRotations(1.2747380347329678 + Math.PI / 2.0); + + public static double armLength = + switch (Constants.getRobot()) { + case DEVBOT -> Units.inchesToMeters(24.8); + default -> Units.inchesToMeters(25.866); + }; + + public static Gains gains = + switch (Constants.getRobot()) { + case SIMBOT -> new Gains(0.0, 0.0, 0.0, 0.02, 1.0, 0.0, 0.01); + 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); + }; + + public static TrapezoidProfile.Constraints profileConstraints = + new TrapezoidProfile.Constraints(2 * Math.PI, 10); + + public record Gains( + double kP, double kI, double kD, double ffkS, double ffkV, double ffkA, double ffkG) {} } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java index 3a88b23d..65b52147 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIO.java @@ -5,25 +5,22 @@ public interface ArmIO { @AutoLog class ArmIOInputs { - public boolean hasFoc = false; - public boolean hasAbsoluteSensor = false; public double armPositionRads = 0.0; - public double armEncoderPositionRads = 0.0; - - public double armEncoderAbsolutePositionRads = 0.0; + public double armAbsoluteEncoderPositionRads = 0.0; public double armTrajectorySetpointRads = 0.0; public double armVelocityRadsPerSec = 0.0; public double[] armAppliedVolts = new double[] {}; public double[] armCurrentAmps = new double[] {}; public double[] armTorqueCurrentAmps = new double[] {}; public double[] armTempCelcius = new double[] {}; + public boolean absoluteEncoderConnected = true; } default void updateInputs(ArmIOInputs inputs) {} /** Run to setpoint angle in radians */ - default void setSetpoint(double setpointRads) {} + default void runSetpoint(double setpointRads) {} /** Run motors at volts */ default void runVolts(double volts) {} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java index 9a4a1ec3..218e4d79 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOKrakenFOC.java @@ -3,41 +3,60 @@ import static org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants.*; import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.*; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.*; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import java.util.List; +import org.littletonrobotics.frc2024.util.Alert; public class ArmIOKrakenFOC implements ArmIO { + // Hardware private final TalonFX leaderMotor; private final TalonFX followerMotor; - private final CANcoder armEncoder; - - private final StatusSignal armPositionRotations; + private final CANcoder absoluteEncoder; + // Status Signals + private final StatusSignal armInternalPositionRotations; private final StatusSignal armEncoderPositionRotations; - private final StatusSignal armEncoderAbsolutePositionRotations; - private final StatusSignal armTrajectorySetpointPositionRotations; + private final StatusSignal armAbsolutePositionRotations; private final StatusSignal armVelocityRps; private final List> armAppliedVoltage; private final List> armOutputCurrent; private final List> armTorqueCurrent; private final List> armTempCelsius; - Slot0Configs controllerConfig; + // Control + private final Slot0Configs controllerConfig; + private TrapezoidProfile motionProfile; + private TrapezoidProfile.State setpointState = new TrapezoidProfile.State(); + + private final VoltageOut voltageControl = + new VoltageOut(0.0).withEnableFOC(true).withUpdateFreqHz(0.0); + private final TorqueCurrentFOC currentControl = new TorqueCurrentFOC(0.0).withUpdateFreqHz(0.0); + private final PositionTorqueCurrentFOC positionControl = + new PositionTorqueCurrentFOC(0.0).withUpdateFreqHz(0.0); + + // Alerts + private final Alert leaderMotorDisconnected = + new Alert("Arm", "Leader Motor Disconnected!", Alert.AlertType.WARNING); + private final Alert followerMotorDisconnected = + new Alert("Arm", "Follower Motor Disconnected!", Alert.AlertType.WARNING); + private final Alert absoluteEncoderDisconnected = + new Alert("Arm", "Absolute Encoder Disconnected!", Alert.AlertType.WARNING); public ArmIOKrakenFOC() { leaderMotor = new TalonFX(leaderID, "canivore"); followerMotor = new TalonFX(followerID, "canivore"); followerMotor.setControl(new Follower(leaderID, true)); - armEncoder = new CANcoder(armEncoderID, "canivore"); + absoluteEncoder = new CANcoder(armEncoderID, "canivore"); // Arm Encoder Configs CANcoderConfiguration armEncoderConfig = new CANcoderConfiguration(); @@ -45,7 +64,7 @@ public ArmIOKrakenFOC() { AbsoluteSensorRangeValue.Signed_PlusMinusHalf; armEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.Clockwise_Positive; armEncoderConfig.MagnetSensor.MagnetOffset = armEncoderOffsetRotations; - armEncoder.getConfigurator().apply(armEncoderConfig, 1); + absoluteEncoder.getConfigurator().apply(armEncoderConfig, 1); // Leader motor configs TalonFXConfiguration leaderConfig = new TalonFXConfiguration(); @@ -59,6 +78,7 @@ public ArmIOKrakenFOC() { leaderConfig.Feedback.SensorToMechanismRatio = 1.0; leaderConfig.Feedback.RotorToSensorRatio = reduction; + // Set up controller controllerConfig = new Slot0Configs() .withKP(gains.kP()) @@ -71,23 +91,14 @@ public ArmIOKrakenFOC() { .withGravityType(GravityTypeValue.Arm_Cosine); leaderConfig.Slot0 = controllerConfig; - leaderConfig.MotionMagic = - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity( - Units.radiansToRotations(profileConstraints.cruiseVelocityRadPerSec())) - .withMotionMagicAcceleration( - Units.radiansToRotations(profileConstraints.accelerationRadPerSec2())); - leaderMotor.getConfigurator().apply(leaderConfig, 1); - // Follower configs TalonFXConfiguration followerConfig = new TalonFXConfiguration(); followerConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; // Status signals - armPositionRotations = leaderMotor.getPosition(); - armEncoderPositionRotations = armEncoder.getPosition(); - armEncoderAbsolutePositionRotations = armEncoder.getAbsolutePosition(); - armTrajectorySetpointPositionRotations = leaderMotor.getClosedLoopReference(); + armInternalPositionRotations = leaderMotor.getPosition(); + armEncoderPositionRotations = absoluteEncoder.getPosition(); + armAbsolutePositionRotations = absoluteEncoder.getAbsolutePosition(); armVelocityRps = leaderMotor.getVelocity(); armAppliedVoltage = List.of(leaderMotor.getMotorVoltage(), followerMotor.getMotorVoltage()); armOutputCurrent = List.of(leaderMotor.getSupplyCurrent(), followerMotor.getSupplyCurrent()); @@ -96,10 +107,9 @@ public ArmIOKrakenFOC() { BaseStatusSignal.setUpdateFrequencyForAll( 100, - armPositionRotations, + armInternalPositionRotations, armEncoderPositionRotations, - armEncoderAbsolutePositionRotations, - armTrajectorySetpointPositionRotations, + armAbsolutePositionRotations, armVelocityRps, armAppliedVoltage.get(0), armAppliedVoltage.get(1), @@ -109,34 +119,38 @@ public ArmIOKrakenFOC() { armTorqueCurrent.get(1), armTempCelsius.get(0), armTempCelsius.get(1)); + + // Init profile + motionProfile = new TrapezoidProfile(profileConstraints); } public void updateInputs(ArmIOInputs inputs) { - inputs.hasFoc = true; - inputs.hasAbsoluteSensor = true; - - BaseStatusSignal.refreshAll( - armPositionRotations, - armEncoderPositionRotations, - armEncoderAbsolutePositionRotations, - armTrajectorySetpointPositionRotations, - armVelocityRps, - armAppliedVoltage.get(0), - armAppliedVoltage.get(1), - armOutputCurrent.get(0), - armOutputCurrent.get(1), - armTorqueCurrent.get(0), - armTorqueCurrent.get(1), - armTempCelsius.get(0), - armTempCelsius.get(1)); - - inputs.armPositionRads = Units.rotationsToRadians(armPositionRotations.getValue()); + inputs.absoluteEncoderConnected = true; + + // Refresh signals & set alerts + leaderMotorDisconnected.set( + BaseStatusSignal.refreshAll( + armInternalPositionRotations, + armVelocityRps, + armAppliedVoltage.get(0), + armOutputCurrent.get(0), + armTorqueCurrent.get(0), + armTempCelsius.get(0)) + == StatusCode.OK); + followerMotorDisconnected.set( + BaseStatusSignal.refreshAll( + armAppliedVoltage.get(1), + armOutputCurrent.get(1), + armTorqueCurrent.get(1), + armTempCelsius.get(1)) + == StatusCode.OK); + + inputs.armPositionRads = Units.rotationsToRadians(armInternalPositionRotations.getValue()); inputs.armEncoderPositionRads = Units.rotationsToRadians(armEncoderPositionRotations.getValue()); - inputs.armEncoderAbsolutePositionRads = - Units.rotationsToRadians(armEncoderAbsolutePositionRotations.getValue()); - inputs.armTrajectorySetpointRads = - Units.rotationsToRadians(armTrajectorySetpointPositionRotations.getValue()); + inputs.armAbsoluteEncoderPositionRads = + Units.rotationsToRadians(armAbsolutePositionRotations.getValue()); + inputs.armTrajectorySetpointRads = setpointState.position; inputs.armVelocityRadsPerSec = Units.rotationsToRadians(armVelocityRps.getValue()); inputs.armAppliedVolts = armAppliedVoltage.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); @@ -146,21 +160,34 @@ public void updateInputs(ArmIOInputs inputs) { armTorqueCurrent.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); inputs.armTempCelcius = armTempCelsius.stream().mapToDouble(StatusSignal::getValueAsDouble).toArray(); + + // Check encoder connected + inputs.absoluteEncoderConnected = + BaseStatusSignal.refreshAll(armEncoderPositionRotations, armAbsolutePositionRotations) + == StatusCode.OK; + absoluteEncoderDisconnected.set(inputs.absoluteEncoderConnected); } @Override - public void setSetpoint(double setpointRads) { - leaderMotor.setControl(new MotionMagicTorqueCurrentFOC(Units.radiansToRotations(setpointRads))); + public void runSetpoint(double setpointRads) { + TrapezoidProfile.State currentState = + new TrapezoidProfile.State( + Units.rotationsToRadians(armInternalPositionRotations.getValue()), + Units.rotationsToRadians(armVelocityRps.getValue())); + setpointState = + motionProfile.calculate(0.0, currentState, new TrapezoidProfile.State(setpointRads, 0.0)); + // Run control + leaderMotor.setControl(positionControl.withPosition(setpointState.position)); } @Override public void runVolts(double volts) { - leaderMotor.setControl(new VoltageOut(volts).withEnableFOC(true)); + leaderMotor.setControl(voltageControl.withOutput(volts)); } @Override public void runCurrent(double amps) { - leaderMotor.setControl(new TorqueCurrentFOC(amps)); + leaderMotor.setControl(currentControl.withOutput(amps)); } @Override @@ -189,12 +216,9 @@ public void setFF(double s, double v, double a, double g) { @Override public void setProfileConstraints( double cruiseVelocityRadsPerSec, double accelerationRadsPerSec2) { - leaderMotor - .getConfigurator() - .apply( - new MotionMagicConfigs() - .withMotionMagicCruiseVelocity(Units.radiansToRotations(cruiseVelocityRadsPerSec)) - .withMotionMagicAcceleration(Units.radiansToRotations(accelerationRadsPerSec2))); + motionProfile = + new TrapezoidProfile( + new TrapezoidProfile.Constraints(cruiseVelocityRadsPerSec, accelerationRadsPerSec2)); } @Override diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java index fc18c749..3354bdf6 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/ArmIOSim.java @@ -34,41 +34,17 @@ public class ArmIOSim implements ArmIO { public ArmIOSim() { ff = new ArmFeedforward(gains.ffkS(), gains.ffkG(), gains.ffkV(), gains.ffkA()); profiledController = - new ProfiledPIDController( - gains.kP(), - gains.kI(), - gains.kD(), - new TrapezoidProfile.Constraints( - profileConstraints.cruiseVelocityRadPerSec(), - profileConstraints.accelerationRadPerSec2()), - 0.001); + new ProfiledPIDController(gains.kP(), gains.kI(), gains.kD(), profileConstraints, 0.001); sim.setState(Units.degreesToRadians(45.0), 0.0); } @Override public void updateInputs(ArmIOInputs inputs) { + sim.update(0.02); if (DriverStation.isDisabled()) { controllerNeedsReset = true; } - if (controllerNeedsReset) { - profiledController.reset(sim.getAngleRads() + positionOffset, sim.getVelocityRadPerSec()); - controllerNeedsReset = false; - } - - for (int i = 0; i < 20; i++) { - // control - if (closedLoop) { - appliedVoltage = - profiledController.calculate(sim.getAngleRads() + positionOffset) - + ff.calculate( - profiledController.getSetpoint().position, - profiledController.getSetpoint().velocity); - sim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); - } - sim.update(0.001); - } - inputs.armPositionRads = sim.getAngleRads() + positionOffset; inputs.armTrajectorySetpointRads = profiledController.getSetpoint().position; inputs.armVelocityRadsPerSec = sim.getVelocityRadPerSec(); @@ -76,15 +52,23 @@ public void updateInputs(ArmIOInputs inputs) { inputs.armCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; inputs.armTorqueCurrentAmps = new double[] {sim.getCurrentDrawAmps()}; inputs.armTempCelcius = new double[] {0.0}; + + // Reset input + sim.setInputVoltage(0.0); } @Override - public void setSetpoint(double setpointRads) { + public void runSetpoint(double setpointRads) { if (!closedLoop) { controllerNeedsReset = true; } - closedLoop = true; - profiledController.setGoal(setpointRads); + if (controllerNeedsReset) { + profiledController.reset(sim.getAngleRads(), sim.getVelocityRadPerSec()); + } + // Control + double feedback = profiledController.calculate(sim.getAngleRads(), setpointRads); + sim.setInputVoltage( + feedback + ff.calculate(sim.getAngleRads(), profiledController.getSetpoint().velocity)); } @Override