diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f0a3242..c70883f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.commands.intake.SpinIntake; import frc.robot.commands.angler.AimAngler; import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.shooter.SpinShooter; import frc.robot.constants.Constants; import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -31,6 +32,7 @@ import frc.robot.constants.ShootingState.ShootState; import frc.robot.subsystems.GyroSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; //import frc.robot.subsystems.RollerSubsystem; //import frc.robot.subsystems.TiltSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; @@ -62,6 +64,7 @@ public class RobotContainer { private final IntakeSubsystem intakeSubsystem; private final FeederSubsystem feederSubsystem; private final ApriltagSubsystem apriltagSubsystem; + private final ShooterSubsystem shooterSubsystem; private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; private SwerveSubsystem drivebase = null; @@ -86,8 +89,8 @@ public RobotContainer() { intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo(), IntakeSubsystem.createRealDeploymentSwitch()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo()); - feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); + shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo()); apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createRealIo()); RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo(); ThreadedGyro threadedGyro = gyroIo.getThreadedGyro(); @@ -104,6 +107,7 @@ public RobotContainer() { hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo()); apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createMockIo()); + shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo()); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; @@ -117,6 +121,7 @@ public RobotContainer() { hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer)); apriltagSubsystem = new ApriltagSubsystem(ApriltagSubsystem.createSimIo()); + shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer)); // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; @@ -231,6 +236,10 @@ public void putShuffleboardCommands() { "Spin Feeder", new SpinFeeder(feederSubsystem)); + SmartDashboard.putData( + "Spin Shooter", + new SpinShooter(shooterSubsystem, Constants.SHOOTER_SPEED)); + SmartDashboard.putData( "Shooting State: Stopped", new SetShootingState(shootState, ShootState.STOPPED)); @@ -238,6 +247,10 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "Shooting State: Fixed", new SetShootingState(shootState, ShootState.FIXED)); + + SmartDashboard.putData( + "Shooting State: Fixed 2", + new SetShootingState(shootState, ShootState.FIXED_2)); SmartDashboard.putData( "Shooting State: Into Hub", diff --git a/src/main/java/frc/robot/commands/shooter/SpinShooter.java b/src/main/java/frc/robot/commands/shooter/SpinShooter.java new file mode 100644 index 0000000..e21102c --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/SpinShooter.java @@ -0,0 +1,41 @@ +package frc.robot.commands.shooter; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class SpinShooter extends LoggableCommand{ + + private final ShooterSubsystem subsystem; + private final double speed; + private final Timer timer; + + public SpinShooter(ShooterSubsystem subsystem, double speed) { + this.subsystem = subsystem; + this.speed = speed; + timer = new Timer(); + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + subsystem.setPidVelocity(speed); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(Constants.SHOOTER_TIMEOUT); + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + +} diff --git a/src/main/java/frc/robot/constants/Constants2026.java b/src/main/java/frc/robot/constants/Constants2026.java index 847c955..da8428d 100644 --- a/src/main/java/frc/robot/constants/Constants2026.java +++ b/src/main/java/frc/robot/constants/Constants2026.java @@ -10,6 +10,8 @@ public class Constants2026 extends GameConstants { public static final int ANGLER_MOTOR_ID = 6; public static final int HOPPER_MOTOR_ID = 4; public static final int FEEDER_MOTOR_ID = 5; + public static final int SHOOTER_MOTOR_ID = 8; + public static final int SHOOTER_FOLLOWER_MOTOR_ID = 7; public static final double DRIVE_BASE_WIDTH = 0.635; public static final double DRIVE_BASE_LENGTH = 0.635; diff --git a/src/main/java/frc/robot/constants/ConstantsTestbed2026.java b/src/main/java/frc/robot/constants/ConstantsTestbed2026.java index bd29ac1..111b8ba 100644 --- a/src/main/java/frc/robot/constants/ConstantsTestbed2026.java +++ b/src/main/java/frc/robot/constants/ConstantsTestbed2026.java @@ -24,6 +24,8 @@ public class ConstantsTestbed2026 extends Constants2026 { public static final int INTAKE_MOTOR_ID = 50; public static final int ANGLER_MOTOR_ID = 40; + // public static final int SHOOTER_MOTOR_ID = 50; + // public static final int SHOOTER_FOLLOWER_MOTOR_ID = 40; public static final boolean TESTBED = true; } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index fdddc93..c709e61 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -50,6 +50,7 @@ public enum Mode { public static final double HOPPER_SPEED = 0.1; public static final double FEEDER_SPEED = 0.5; public static final double MAX_SPEED = Units.feetToMeters(14.5); + public static final double SHOOTER_SPEED = 100; //Timeouts public static final double SPIN_TIMEOUT = 5; @@ -57,6 +58,7 @@ public enum Mode { public static final double HOPPER_TIMEOUT = 10; public static final double FEEDER_TIMEOUT = 3; public static final int SERVER_SOCKET_CONNECTION_TIMEOUT = 2000; + public static final double SHOOTER_TIMEOUT = 5; //Angles public static final Rotation2d TILT_MIN_ANGLE = Rotation2d.fromDegrees(45); diff --git a/src/main/java/frc/robot/constants/ShootingState.java b/src/main/java/frc/robot/constants/ShootingState.java index 483ac5f..257ccc2 100644 --- a/src/main/java/frc/robot/constants/ShootingState.java +++ b/src/main/java/frc/robot/constants/ShootingState.java @@ -5,9 +5,10 @@ public class ShootingState { public enum ShootState { STOPPED, // Shooting motor does not run - FIXED, // Shooting motor runs at a constant speed - SHOOTING_HUB, // Used for shooting into the hub - SHUTTLING // Used for shuttling into our alliance zone + FIXED, // Shoot from a known location + FIXED_2, // Shoot from another known location + SHOOTING_HUB, // Auto-aim into the hub + SHUTTLING // Auto-aim into alliance zone } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..09d8d67 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,80 @@ +package frc.robot.subsystems; + + + +import com.ctre.phoenix6.controls.Follower; +import com.revrobotics.spark.SparkLowLevel; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.utils.logging.input.MotorLoggableInputs; +import frc.robot.utils.logging.io.motor.SparkMaxIo; +import frc.robot.utils.logging.io.pidmotor.MockSparkMaxPidMotorIo; +import frc.robot.utils.logging.io.pidmotor.RealSparkMaxPidMotorIo; +import frc.robot.utils.logging.io.pidmotor.SimSparkMaxPidMotorIo; +import frc.robot.utils.logging.io.pidmotor.SparkMaxPidConfig; +import frc.robot.utils.logging.io.pidmotor.SparkMaxPidMotor; +import frc.robot.utils.logging.io.pidmotor.SparkMaxPidMotorIo; +import frc.robot.utils.simulation.MotorSimulator; +import frc.robot.utils.simulation.RobotVisualizer; + +public class ShooterSubsystem extends SubsystemBase { + + public static final String LOGGING_NAME = "ShooterSubsystem"; + + private final SparkMaxPidMotorIo io; + private final SparkMax followerMotor; + private final SparkMaxConfig followerConfig; + + public ShooterSubsystem(SparkMaxPidMotorIo io) { + this.io = io; + io.setPid(0.0000002, 0.000015, 0.000015); // Pid needs tuning + followerMotor = new SparkMax(Constants.SHOOTER_FOLLOWER_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); + followerConfig = new SparkMaxConfig(); + followerConfig.follow(Constants.SHOOTER_MOTOR_ID, true); + followerMotor.configure(followerConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + } + + // setSpeed expects a power value from -1 to 1 + public void setSpeed(double speed) { + io.set(speed); + } + + public void stopMotors() { + io.stopMotor(); + } + + // setPidVelocity expects a speed in RPM + public void setPidVelocity(double velocity) { + io.setPidVelocity(velocity); + } + + @Override + public void periodic() { + io.periodic(); + } + + public static SparkMaxPidMotorIo createMockIo() { + return new MockSparkMaxPidMotorIo(LOGGING_NAME, MotorLoggableInputs.allMetrics()); + } + + public static SparkMaxPidMotorIo createRealIo() { + return new RealSparkMaxPidMotorIo(LOGGING_NAME, createMotor(), MotorLoggableInputs.allMetrics()); + } + + public static SparkMaxPidMotorIo createSimIo(RobotVisualizer visualizer) { + SparkMaxPidMotor motor = createMotor(); + return new SimSparkMaxPidMotorIo(LOGGING_NAME, motor, MotorLoggableInputs.allMetrics(), new MotorSimulator(motor.getNeoMotor(), visualizer.getShooterLigament())); + } + + public static SparkMaxPidMotor createMotor() { + return new SparkMaxPidMotor(Constants.SHOOTER_MOTOR_ID, true); + } + +} diff --git a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java index f7b1723..22866fc 100644 --- a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java +++ b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java @@ -18,6 +18,8 @@ public class RobotVisualizer { private final LoggedMechanismLigament2d hopperLigament; private final LoggedMechanismLigament2d feederLigament; private final LoggedMechanismLigament2d anglerLigament; + private final LoggedMechanismLigament2d shooterTiltLigament; + private final LoggedMechanismLigament2d shooterLigament; public RobotVisualizer() { LoggedMechanismRoot2d root = @@ -104,7 +106,34 @@ public RobotVisualizer() { 4, new Color8Bit(Color.kYellow))); - } + LoggedMechanismRoot2d shooterRoot = + mech2d.getRoot("Shooter Root", Constants.DRIVE_BASE_WIDTH * 2.5, Constants.INITIAL_ROBOT_HEIGHT); + + LoggedMechanismLigament2d shooterRiserLigament = + shooterRoot.append( + new LoggedMechanismLigament2d( + "Shooter Riser", 0.25, 90, 5, new Color8Bit(Color.kDarkGray))); + + this.shooterTiltLigament = + shooterRiserLigament.append( + new LoggedMechanismLigament2d( + "Shooter Tilt", + 0.5, + -45.0, + 4, + new Color8Bit(Color.kPurple))); + + this.shooterLigament = + shooterTiltLigament.append( + new LoggedMechanismLigament2d( + "Shooter Wheel", + 0.15, + -45.0, + 4, + new Color8Bit(Color.kOrange))); + + } + public LoggedMechanismLigament2d getRollerLigament() { return rollerLigament; @@ -130,6 +159,14 @@ public LoggedMechanismLigament2d getAnglerLigament() { return anglerLigament; } + public LoggedMechanismLigament2d getShooterTiltLigament() { + return shooterTiltLigament; + } + + public LoggedMechanismLigament2d getShooterLigament() { + return shooterLigament; + } + public void logMechanism() { Logger.recordOutput("Mechanism2d/", mech2d); }