diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4e79550..9f90d3d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.Mode; import frc.robot.commands.DriveCommands; +import frc.robot.commands.FireOutput; import frc.robot.commands.controllers.DrivePoseController; import frc.robot.commands.pipeline.DriveInput; import frc.robot.commands.pipeline.DriveInputPipeline; @@ -38,6 +39,11 @@ import frc.robot.subsystems.led.LEDStripIOBlinken; import frc.robot.subsystems.led.LEDStripIOSim; import frc.robot.subsystems.led.LEDSubsystem; +import frc.robot.subsystems.output.Output; +import frc.robot.subsystems.output.OutputConstants; +import frc.robot.subsystems.output.OutputIO; +import frc.robot.subsystems.output.OutputIOSim; +import frc.robot.subsystems.output.OutputIOSparkMax; import frc.robot.subsystems.vision.AprilTagVision; import frc.robot.subsystems.vision.CameraIOSim; import frc.robot.subsystems.vision.VisionConstants; @@ -57,6 +63,7 @@ public class RobotContainer { private final Drive drive; private final AprilTagVision vision; private final LEDSubsystem leds; + private final Output output; // Controller private final CommandXboxController driverController = new CommandXboxController(0); @@ -99,6 +106,7 @@ public RobotContainer() { new ModuleIOTalonFX(TunerConstants.BackRight)); vision = new AprilTagVision(); leds = new LEDSubsystem(); + output = new Output(new OutputIOSparkMax(OutputConstants.SPARKMAX_DEVICE_ID)); break; case CHASSIS_2025: @@ -117,6 +125,7 @@ public RobotContainer() { LEDConstants.LEDS_STRIP_2025_LEFT, LEDConstants.DEFAULT_PATTERN), new LEDStripIOBlinken( LEDConstants.LEDS_STRIP_2025_RIGHT, LEDConstants.DEFAULT_PATTERN)); + output = new Output(new OutputIOSparkMax(OutputConstants.SPARKMAX_DEVICE_ID)); break; case SIM_BOT: @@ -132,6 +141,7 @@ public RobotContainer() { new AprilTagVision( new CameraIOSim(VisionConstants.SIM_FRONT_CAMERA, drive::getRobotPose)); leds = new LEDSubsystem(new LEDStripIOSim(LEDConstants.DEFAULT_PATTERN)); + output = new Output(new OutputIOSim()); break; default: @@ -145,6 +155,7 @@ public RobotContainer() { new ModuleIO() {}); vision = new AprilTagVision(); leds = new LEDSubsystem(); + output = new Output(new OutputIO() {}); break; } @@ -363,7 +374,9 @@ private void configureDriverControllerBindings(CommandXboxController xbox) { .withName("Drive to Pose Goal")); } - private void configureOperatorControllerBindings(CommandXboxController xbox) {} + private void configureOperatorControllerBindings(CommandXboxController xbox) { + xbox.a().onTrue(new FireOutput(output)); + } private Command rumbleController( CommandXboxController controller, double rumbleIntensity, RumbleType type) { diff --git a/src/main/java/frc/robot/commands/FireOutput.java b/src/main/java/frc/robot/commands/FireOutput.java new file mode 100644 index 0000000..1aa6201 --- /dev/null +++ b/src/main/java/frc/robot/commands/FireOutput.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.output.Output; +import frc.robot.subsystems.output.OutputConstants; + +public class FireOutput extends Command { + private final Output output; + private Timer timer; + + public FireOutput(Output output) { + this.output = output; + timer = new Timer(); + addRequirements(output); + } + + @Override + public void initialize() { + timer.restart(); + output.setVelocity(OutputConstants.FIRE_VELOCITY_RAD_PER_SEC); + } + + @Override + public void execute() {} + + @Override + public boolean isFinished() { + return timer.hasElapsed(OutputConstants.FIRE_TIME_SECONDS); + } + + @Override + public void end(boolean interrupted) { + output.stop(); + timer.stop(); + } +} diff --git a/src/main/java/frc/robot/subsystems/output/Output.java b/src/main/java/frc/robot/subsystems/output/Output.java new file mode 100644 index 0000000..9b451ed --- /dev/null +++ b/src/main/java/frc/robot/subsystems/output/Output.java @@ -0,0 +1,54 @@ +package frc.robot.subsystems.output; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +/** Subsystem for the output mechanism */ +public class Output extends SubsystemBase { + private final OutputIO io; + private final OutputIOInputsAutoLogged inputs = new OutputIOInputsAutoLogged(); + + private final SimpleMotorFeedforward ffModel; + + public Output(OutputIO io) { + this.io = io; + this.ffModel = + new SimpleMotorFeedforward( + OutputConstants.FF_CONFIG.kS(), + OutputConstants.FF_CONFIG.kV(), + OutputConstants.FF_CONFIG.kA()); + io.configurePID( + OutputConstants.PID_CONFIG.kP(), + OutputConstants.PID_CONFIG.kI(), + OutputConstants.PID_CONFIG.kD()); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Output", inputs); + } + + /** + * Run the output in a closed-loop configuration at the specified velocity (radians per second) + */ + public void setVelocity(double velocityRadPerSec) { + // io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); + io.setVelocity(velocityRadPerSec, 0); + Logger.recordOutput("Output/SetpointVelocity", velocityRadPerSec); + } + + /** Stop the output and brake */ + public void stop() { + setVelocity(0); + io.stop(); + } + + /** Get the current velocity of the output in radians per second */ + @AutoLogOutput(key = "Output/CurrentVelocity") + public double getVelocityRadPerSec() { + return inputs.velocityRadPerSec; + } +} diff --git a/src/main/java/frc/robot/subsystems/output/OutputConstants.java b/src/main/java/frc/robot/subsystems/output/OutputConstants.java new file mode 100644 index 0000000..6db0e95 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/output/OutputConstants.java @@ -0,0 +1,44 @@ +package frc.robot.subsystems.output; + +import frc.robot.Constants; + +public final class OutputConstants { + // Device ID for the hardware output motor + // TODO: Get real device IDs to put here + public static final int SPARKMAX_DEVICE_ID = + switch (Constants.getRobot()) { + case PHOENIX_TUNER_X -> 0; + case CHASSIS_2025 -> 0; + case CHASSIS_CANNON -> 0; + case SIM_BOT -> 0; + }; + + public record PID(double kP, double kI, double kD) {} + + public record Feedforward(double kS, double kV, double kA) {} + + // PID constants for the closed-loop controller + // TODO: Do tuning and get real constants to put here + public static final PID PID_CONFIG = + switch (Constants.getRobot()) { + case PHOENIX_TUNER_X -> new PID(0, 0, 0); + case CHASSIS_2025 -> new PID(0, 0, 0); + case CHASSIS_CANNON -> new PID(0, 0, 0); + case SIM_BOT -> new PID(1, 0, 0.5); + }; + + // Feedforward constants + // TODO: Do tuning and get real constants to put here + public static final Feedforward FF_CONFIG = + switch (Constants.getRobot()) { + case PHOENIX_TUNER_X -> new Feedforward(0, 0, 0); + case CHASSIS_2025 -> new Feedforward(0, 0, 0); + case CHASSIS_CANNON -> new Feedforward(0, 0, 0); + case SIM_BOT -> new Feedforward(0, 0, 0); + }; + + // Constants for the launcher fire command + // TODO: Get real values for these constants + public static final double FIRE_VELOCITY_RAD_PER_SEC = 2 * Math.PI; + public static final double FIRE_TIME_SECONDS = 5; +} diff --git a/src/main/java/frc/robot/subsystems/output/OutputIO.java b/src/main/java/frc/robot/subsystems/output/OutputIO.java new file mode 100644 index 0000000..c372a2d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/output/OutputIO.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.output; + +import org.littletonrobotics.junction.AutoLog; + +/** General IO interface between the Output subsystem and the hardware */ +public interface OutputIO { + @AutoLog + public static class OutputIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double supplyCurrentAmps = 0.0; + } + + /** Updates the set of loggable inputs. */ + default void updateInputs(OutputIOInputs inputs) {} + + /** Sets the velocity of the motor */ + default void setVelocity(double velocityRadPerSec, double ffVolts) {} + + /** Stop the motor and enable the brake */ + default void stop() {} + + /** Configure the PID constants */ + default void configurePID(double kP, double kI, double kD) {} +} diff --git a/src/main/java/frc/robot/subsystems/output/OutputIOSim.java b/src/main/java/frc/robot/subsystems/output/OutputIOSim.java new file mode 100644 index 0000000..3812ad0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/output/OutputIOSim.java @@ -0,0 +1,59 @@ +package frc.robot.subsystems.output; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import frc.robot.Constants; + +public class OutputIOSim implements OutputIO { + private DCMotor motor; + private DCMotorSim sim; + private PIDController pid; + + private double ffVolts = 0.0; + + public OutputIOSim() { + // Setup devices + motor = DCMotor.getNEO(1); + sim = new DCMotorSim(LinearSystemId.createDCMotorSystem(motor, 0.004, 1.0), motor); + pid = new PIDController(0, 0, 0); + } + + /** Updates the set of loggable inputs. */ + @Override + public void updateInputs(OutputIOInputs inputs) { + // Calculate input from PID controller + double appliedVolts = + MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); + sim.setInputVoltage(appliedVolts); + + // Update simulation + sim.update(Constants.LOOP_PERIOD_SECONDS); + inputs.positionRad = sim.getAngularPositionRad(); + inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); + inputs.appliedVolts = appliedVolts; + inputs.supplyCurrentAmps = sim.getCurrentDrawAmps(); + } + + /** Sets the velocity of the motor */ + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + pid.setSetpoint(velocityRadPerSec); + this.ffVolts = ffVolts; + } + + /** Stop the motor and enable the brake */ + @Override + public void stop() { + pid.setSetpoint(0); + sim.setInputVoltage(0); + } + + /** Configure the PID constants */ + @Override + public void configurePID(double kP, double kI, double kD) { + pid.setPID(kP, kI, kD); + } +} diff --git a/src/main/java/frc/robot/subsystems/output/OutputIOSparkMax.java b/src/main/java/frc/robot/subsystems/output/OutputIOSparkMax.java new file mode 100644 index 0000000..29e0d26 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/output/OutputIOSparkMax.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems.output; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.util.Units; + +public class OutputIOSparkMax implements OutputIO { + private SparkMax motor; + + public OutputIOSparkMax(int deviceID) { + motor = new SparkMax(deviceID, MotorType.kBrushless); + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(IdleMode.kBrake); + motor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + /** Updates the set of loggable inputs. */ + @Override + public void updateInputs(OutputIOInputs inputs) { + inputs.positionRad = Units.rotationsToRadians(motor.getEncoder().getPosition()); + inputs.velocityRadPerSec = + Units.rotationsPerMinuteToRadiansPerSecond(motor.getEncoder().getVelocity()); + inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.supplyCurrentAmps = motor.getOutputCurrent(); + } + + /** Sets the velocity of the motor */ + @Override + public void setVelocity(double velocityRadPerSec, double ffVolts) { + motor + .getClosedLoopController() + .setReference( + Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + ffVolts, + ArbFFUnits.kVoltage); + } + + /** Stop the motor and enable the brake */ + @Override + public void stop() { + motor.stopMotor(); + } + + /** Configure the PID constants */ + @Override + public void configurePID(double kP, double kI, double kD) { + SparkMaxConfig config = new SparkMaxConfig(); + config.closedLoop.pid(kP, kI, kD); + motor.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } +}