diff --git a/src/main/java/com/team973/frc2025/Robot.java b/src/main/java/com/team973/frc2025/Robot.java index 04ee4c4..20c9567 100644 --- a/src/main/java/com/team973/frc2025/Robot.java +++ b/src/main/java/com/team973/frc2025/Robot.java @@ -5,6 +5,7 @@ package com.team973.frc2025; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.shooter.ShooterIO; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.util.AllianceCache; import com.team973.lib.util.Joystick; @@ -21,29 +22,37 @@ public class Robot extends TimedRobot { private final GreyPigeonIO m_pigeon = m_subsystemManager.getPigeon(); private final DriveController m_driveController = m_subsystemManager.getDriveController(); + private final ShooterIO m_shooter = m_subsystemManager.getShooter(); private final Joystick m_driverStick = new Joystick(0, Joystick.Type.XboxController, m_logger.subLogger("driverStick")); + private final Joystick m_coDriverStick = + new Joystick(1, Joystick.Type.XboxController, m_logger.subLogger("coDriverStick")); private void syncSensors() { m_driveController.syncSensors(); + m_shooter.syncSensors(); } private void updateSubsystems() { m_driveController.update(); + m_shooter.update(); } private void resetSubsystems() { m_driveController.reset(); + m_shooter.reset(); } private void log() { m_subsystemManager.log(); m_driveController.log(); + m_shooter.log(); } private void updateJoysticks() { m_driverStick.update(); + m_coDriverStick.update(); } public Robot() { @@ -95,6 +104,18 @@ public void teleopPeriodic() { allianceScalar * m_driverStick.getLeftXAxis() * 0.7, m_driverStick.getRightXAxis() * 0.8); + if (m_coDriverStick.getAButtonPressed()) { + m_shooter.setTargetPreset(ShooterIO.Preset.One); + } else if (m_coDriverStick.getBButtonPressed()) { + m_shooter.setTargetPreset(ShooterIO.Preset.Two); + } else if (m_coDriverStick.getXButtonPressed()) { + m_shooter.setTargetPreset(ShooterIO.Preset.Three); + } else if (m_coDriverStick.getYButtonPressed()) { + m_shooter.setState(ShooterIO.State.Manual); + } + + m_shooter.setManualInput(m_coDriverStick.getLeftYAxis()); + updateSubsystems(); } diff --git a/src/main/java/com/team973/frc2025/shared/RobotInfo.java b/src/main/java/com/team973/frc2025/shared/RobotInfo.java index ed18f58..cbfe799 100644 --- a/src/main/java/com/team973/frc2025/shared/RobotInfo.java +++ b/src/main/java/com/team973/frc2025/shared/RobotInfo.java @@ -18,6 +18,35 @@ public class RobotInfo { public static final DriveInfo DRIVE_INFO = new DriveInfo(); + public static final ShooterInfo SHOOTER_INFO = new ShooterInfo(); + + public static class ShooterInfo { + public final int MOTOR_ID = 20; + + public final double MOTOR_GEAR_RATIO = 40.0 / 18.0; + + public final double SHOOTER_KS = 0.0; + public final double SHOOTER_KV = 0.0; + public final double SHOOTER_KA = 0.0; + public final double SHOOTER_KP = 0.1; + public final double SHOOTER_KI = 0.0; + public final double SHOOTER_KD = 0.0; + + public final double SHOOTER_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD = 0.0; + + public final double STATOR_CURRENT_LIMIT = 60.0; + public final double SUPPLY_CURRENT_LIMIT = 40.0; + + public final double PEAK_FORWARD_VOLTAGE = 12.0; + public final double PEAK_REVERSE_VOLTAGE = -12.0; + + public final double MANUAL_INPUT_TO_VOLTS = 12.0; + + public final double PRESET_ONE = 100; + public final double PRESET_TWO = -100; + public final double PRESET_THREE = 200; + } + public static class DriveInfo { public final int STATUS_SIGNAL_FREQUENCY = 200; diff --git a/src/main/java/com/team973/frc2025/subsystems/shooter/Shooter.java b/src/main/java/com/team973/frc2025/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..d543666 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/shooter/Shooter.java @@ -0,0 +1,105 @@ +package com.team973.frc2025.subsystems.shooter; + +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.devices.GreyTalonFX; +import com.team973.lib.util.Logger; +import edu.wpi.first.math.geometry.Pose3d; + +public class Shooter extends ShooterIO { + protected static final RobotInfo.ShooterInfo m_shooterInfo = RobotInfo.SHOOTER_INFO; + + private final Logger m_logger; + + protected final GreyTalonFX m_motor; + + private double m_targetVelocityRPS; + private double m_manualInput; + + public Shooter(Logger logger) { + m_logger = logger; + m_motor = + new GreyTalonFX( + m_shooterInfo.MOTOR_ID, RobotInfo.CANIVORE_CANBUS, m_logger.subLogger("motorRight")); + + TalonFXConfiguration motorConfig = new TalonFXConfiguration(); + + motorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + + motorConfig.Slot0.kS = m_shooterInfo.SHOOTER_KS; + motorConfig.Slot0.kV = m_shooterInfo.SHOOTER_KV; + motorConfig.Slot0.kA = m_shooterInfo.SHOOTER_KA; + motorConfig.Slot0.kP = m_shooterInfo.SHOOTER_KP; + motorConfig.Slot0.kI = m_shooterInfo.SHOOTER_KI; + motorConfig.Slot0.kD = m_shooterInfo.SHOOTER_KD; + + motorConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = + m_shooterInfo.SHOOTER_VOLTAGE_CLOSED_LOOP_RAMP_PERIOD; + + motorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + motorConfig.CurrentLimits.StatorCurrentLimit = m_shooterInfo.STATOR_CURRENT_LIMIT; + motorConfig.CurrentLimits.StatorCurrentLimitEnable = true; + motorConfig.CurrentLimits.SupplyCurrentLimit = m_shooterInfo.SUPPLY_CURRENT_LIMIT; + motorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + motorConfig.Voltage.PeakForwardVoltage = m_shooterInfo.PEAK_FORWARD_VOLTAGE; + motorConfig.Voltage.PeakReverseVoltage = m_shooterInfo.PEAK_REVERSE_VOLTAGE; + + m_motor.setConfig(motorConfig); + m_motor.setPosition(0.0); + + m_targetVelocityRPS = 0.0; + m_manualInput = 0.0; + } + + @Override + public Pose3d getPose() { + return new Pose3d(); + } + + @Override + public GreyTalonFX getMotor() { + return m_motor; + } + + @Override + public void setTargetPreset(Preset preset) { + m_targetVelocityRPS = preset.getVelocityRPS(); + setState(State.ClosedLoop); + } + + @Override + public void setManualInput(double input) { + m_manualInput = input; + } + + @Override + public double getTargetVelocityMotorRPS() { + return m_targetVelocityRPS; + } + + @Override + public double getManualInput() { + return m_manualInput; + } + + @Override + public void syncSensors() {} + + @Override + public void log() { + m_motor.log(); + + m_logger.log("currentVelocityRPS", m_motor.getVelocity().getValueAsDouble()); + m_logger.log("targetVelocityRPS", m_targetVelocityRPS); + m_logger.log("manualInput", m_manualInput); + + m_logger.log("state", getState().toString()); + } + + @Override + public void reset() {} +} diff --git a/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterIO.java b/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterIO.java new file mode 100644 index 0000000..8fb406e --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterIO.java @@ -0,0 +1,63 @@ +package com.team973.frc2025.subsystems.shooter; + +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.devices.GreyTalonFX; +import com.team973.lib.util.StateMap; +import com.team973.lib.util.Subsystem; +import edu.wpi.first.math.geometry.Pose3d; + +public abstract class ShooterIO extends Subsystem { + private final StateMap m_stateMap; + + protected static final RobotInfo.ShooterInfo m_flywheelInfo = RobotInfo.SHOOTER_INFO; + + public enum State { + ClosedLoop, + Manual, + Off + } + + public enum Preset { + One(m_flywheelInfo.PRESET_ONE), + Two(m_flywheelInfo.PRESET_TWO), + Three(m_flywheelInfo.PRESET_THREE); + + private final double m_velocityRPS; + + private Preset(double velocityRPS) { + m_velocityRPS = velocityRPS; + } + + public double getVelocityRPS() { + return m_velocityRPS; + } + } + + @SuppressWarnings("unchecked") + public ShooterIO() { + super(State.Off); + + m_stateMap = + new StateMap<>( + State.class, + new StateMap.Entry<>(State.ClosedLoop, new ShooterStates.ClosedLoop(this)), + new StateMap.Entry<>(State.Manual, new ShooterStates.Manual(this)), + new StateMap.Entry<>(State.Off, new ShooterStates.Off(this))); + } + + public StateMap getStateMap() { + return m_stateMap; + } + + public abstract Pose3d getPose(); + + public abstract GreyTalonFX getMotor(); + + public abstract void setTargetPreset(Preset preset); + + public abstract void setManualInput(double volts); + + public abstract double getTargetVelocityMotorRPS(); + + public abstract double getManualInput(); +} diff --git a/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterSim.java b/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterSim.java new file mode 100644 index 0000000..ee82b39 --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterSim.java @@ -0,0 +1,66 @@ +package com.team973.frc2025.subsystems.shooter; + +import com.ctre.phoenix6.sim.TalonFXSimState; +import com.team973.lib.util.Logger; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; + +public class ShooterSim extends Shooter { + private final FlywheelSim m_sim; + private final TalonFXSimState m_motorSimState; + + private double m_lastVelocityRPM; + private double m_lastTimeSec; + + public ShooterSim(Logger logger) { + super(logger); + + m_sim = + new FlywheelSim( + LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), 0.002, m_shooterInfo.MOTOR_GEAR_RATIO), + DCMotor.getKrakenX60(1)); + + m_motorSimState = m_motor.getSimState(); + m_lastVelocityRPM = m_sim.getAngularVelocityRPM(); + m_lastTimeSec = Timer.getFPGATimestamp(); + } + + @Override + public Pose3d getPose() { + return new Pose3d(); + } + + private double flywheelRPSToMotorRPS(double flywheelRPS) { + return flywheelRPS * m_shooterInfo.MOTOR_GEAR_RATIO; + } + + @Override + public void syncSensors() { + m_motorSimState.addRotorPosition( + flywheelRPSToMotorRPS(((m_sim.getAngularVelocityRPM() + m_lastVelocityRPM) / 2.0) / 60.0) + * (Timer.getFPGATimestamp() - m_lastTimeSec)); + + m_lastVelocityRPM = m_sim.getAngularVelocityRPM(); + m_lastTimeSec = Timer.getFPGATimestamp(); + + m_motorSimState.setRotorVelocity(flywheelRPSToMotorRPS(m_sim.getAngularVelocityRPM() / 60.0)); + + m_motorSimState.setSupplyVoltage(RobotController.getBatteryVoltage()); + + super.syncSensors(); + } + + @Override + public void update() { + super.update(); + + m_sim.setInputVoltage(m_motorSimState.getMotorVoltage()); + + m_sim.update(0.02); + } +} diff --git a/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterStates.java b/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterStates.java new file mode 100644 index 0000000..8185a3d --- /dev/null +++ b/src/main/java/com/team973/frc2025/subsystems/shooter/ShooterStates.java @@ -0,0 +1,63 @@ +package com.team973.frc2025.subsystems.shooter; + +import com.team973.frc2025.shared.RobotInfo; +import com.team973.lib.devices.GreyTalonFX.ControlMode; +import com.team973.lib.util.SubsystemState; + +public class ShooterStates { + private abstract static class ShooterState implements SubsystemState { + protected final ShooterIO m_flywheel; + + private ShooterState(ShooterIO turret) { + m_flywheel = turret; + } + } + + public static class ClosedLoop extends ShooterState { + public ClosedLoop(ShooterIO turret) { + super(turret); + } + + public void init() {} + + public void run() { + m_flywheel + .getMotor() + .setControl(ControlMode.VelocityVoltage, m_flywheel.getTargetVelocityMotorRPS()); + } + + public void exit() {} + } + + public static class Manual extends ShooterState { + public Manual(ShooterIO turret) { + super(turret); + } + + public void init() {} + + public void run() { + m_flywheel + .getMotor() + .setControl( + ControlMode.VoltageOut, + m_flywheel.getManualInput() * RobotInfo.SHOOTER_INFO.MANUAL_INPUT_TO_VOLTS); + } + + public void exit() {} + } + + public static class Off extends ShooterState { + public Off(ShooterIO turret) { + super(turret); + } + + public void init() {} + + public void run() { + m_flywheel.getMotor().setControl(ControlMode.DutyCycleOut, 0); + } + + public void exit() {} + } +} diff --git a/src/main/java/com/team973/lib/util/SubsystemManager.java b/src/main/java/com/team973/lib/util/SubsystemManager.java index 16ef22e..66fbc35 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManager.java +++ b/src/main/java/com/team973/lib/util/SubsystemManager.java @@ -2,7 +2,9 @@ import com.team973.frc2025.Robot; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.shooter.ShooterIO; import com.team973.lib.devices.GreyPigeonIO; +import edu.wpi.first.math.geometry.Pose3d; public abstract class SubsystemManager { private final Logger m_logger; @@ -27,5 +29,9 @@ public static SubsystemManager init(Logger logger) { public abstract DriveController getDriveController(); - public void log() {} + public abstract ShooterIO getShooter(); + + public void log() { + m_logger.log("components", new Pose3d[] {getShooter().getPose()}); + } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerReal.java b/src/main/java/com/team973/lib/util/SubsystemManagerReal.java index 8dd2b0c..6147bf1 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerReal.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerReal.java @@ -2,6 +2,8 @@ import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.shooter.Shooter; +import com.team973.frc2025.subsystems.shooter.ShooterIO; import com.team973.frc2025.subsystems.swerve.SwerveModule; import com.team973.lib.devices.GreyPigeon; import com.team973.lib.devices.GreyPigeonIO; @@ -9,6 +11,7 @@ public class SubsystemManagerReal extends SubsystemManager { private final GreyPigeonIO m_pigeon; private final DriveController m_driveController; + private final ShooterIO m_shooter; public SubsystemManagerReal(Logger logger) { super(logger); @@ -33,6 +36,8 @@ public SubsystemManagerReal(Logger logger) { new SwerveModule( 3, RobotInfo.DRIVE_INFO.BACK_RIGHT_CONSTANTS, driveLogger.subLogger("swerve/mod3")), m_pigeon); + + m_shooter = new Shooter(logger.subLogger("shooter")); } public GreyPigeonIO getPigeon() { @@ -42,4 +47,8 @@ public GreyPigeonIO getPigeon() { public DriveController getDriveController() { return m_driveController; } + + public ShooterIO getShooter() { + return m_shooter; + } } diff --git a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java index 1d22df3..6ab7670 100644 --- a/src/main/java/com/team973/lib/util/SubsystemManagerSim.java +++ b/src/main/java/com/team973/lib/util/SubsystemManagerSim.java @@ -2,6 +2,8 @@ import com.team973.frc2025.shared.RobotInfo; import com.team973.frc2025.subsystems.DriveController; +import com.team973.frc2025.subsystems.shooter.ShooterIO; +import com.team973.frc2025.subsystems.shooter.ShooterSim; import com.team973.frc2025.subsystems.swerve.SwerveModuleSim; import com.team973.lib.devices.GreyPigeonIO; import com.team973.lib.devices.GreyPigeonSim; @@ -12,6 +14,7 @@ public class SubsystemManagerSim extends SubsystemManager { private final SwerveDriveSimulation m_swerveDriveSimulation; private final GreyPigeonIO m_pigeon; private final DriveController m_driveController; + private final ShooterIO m_shooter; public SubsystemManagerSim(Logger logger) { super(logger); @@ -56,6 +59,8 @@ public SubsystemManagerSim(Logger logger) { RobotInfo.DRIVE_INFO.BACK_RIGHT_CONSTANTS, driveLogger.subLogger("swerve/mod3")), m_pigeon); + + m_shooter = new ShooterSim(logger.subLogger("shooter")); } public GreyPigeonIO getPigeon() { @@ -66,6 +71,10 @@ public DriveController getDriveController() { return m_driveController; } + public ShooterIO getShooter() { + return m_shooter; + } + @Override public void log() { super.log();