Shooter Subsystem PID Control

added basic PID functionality to shooter subsystem

src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java

@@ -10,6 +10,8 @@
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand;
+import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands.ShooterCommand;
 import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem;
 
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -35,6 +37,8 @@ public class RobotContainer {
 
 	private final VisionSubsystem vision = new VisionSubsystem(this.driveSubsystem);
 
+	private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+
 	private final SendableChooser<Command> autoChooser;
 
 	public RobotContainer() {
@@ -47,6 +51,8 @@ public RobotContainer() {
 		configurePathPlanner();
 		autoChooser = AutoBuilder.buildAutoChooser();
 		SmartDashboard.putData("Auto Chooser", autoChooser);
+		ShooterCommand shooterCommand = new ShooterCommand(2000, shooterSubsystem);
+		SmartDashboard.putData("Shooter Command", shooterCommand);
 	}
 
 	private void configurePathPlanner() {

src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java

@@ -1,12 +1,16 @@
 package org.jmhsrobotics.frc2024.subsystems.shooter;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class ShooterSubsystem extends SubsystemBase {
-	private CANSparkMax motor1;
-	private CANSparkMax motor2;
+	private CANSparkMax motor1 = new CANSparkMax(7, MotorType.kBrushless);
+	private CANSparkMax motor2 = new CANSparkMax(8, MotorType.kBrushless);;
+	private RelativeEncoder encoder;
 
 	// private double speed;
 
@@ -26,6 +30,11 @@ private void initializeMotors() {
 		motor2.follow(motor1);
 	}
 
+	public double getRPM() {
+		encoder = motor1.getEncoder();
+		return encoder.getVelocity();
+	}
+
 	public void setSpeed(double speed) {
 		motor1.set(speed);
 	}

src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/shooterCommands/ShooterCommand.java

@@ -1,23 +1,42 @@
 package org.jmhsrobotics.frc2024.subsystems.shooter.shooterCommands;
 
 import org.jmhsrobotics.frc2024.subsystems.shooter.ShooterSubsystem;
-import org.jmhsrobotics.frc2024.controlBoard.ControlBoard;
+
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class ShooterCommand extends Command {
-	private ShooterSubsystem shooterSubsystem;
-	private ControlBoard control;
+	private ShooterSubsystem shooter;
+	private double targetRPM;
+	private PIDController pid;
+
+	public ShooterCommand(double targetRPM, ShooterSubsystem shooterSubsystem) {
+		this.shooter = shooterSubsystem;
+		this.targetRPM = targetRPM;
+		pid = new PIDController(.1, 0, 0);
+
+		addRequirements(this.shooter);
+	}
+
+	@Override
+	public void initialize() {
+		pid.reset();
+		pid.setSetpoint(targetRPM);
+	}
 
-	public ShooterCommand(ShooterSubsystem shooterSubsystem, ControlBoard control) {
-		this.shooterSubsystem = shooterSubsystem;
-		this.control = control;
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
 
-		addRequirements(this.shooterSubsystem);
+	@Override
+	public void end(boolean interrupted) {
+		shooter.setSpeed(0);
 	}
 
 	@Override
 	public void execute() {
-		double motorSpeed = control.shooterInput();
-		shooterSubsystem.setSpeed(motorSpeed);
+		double motorSpeed = pid.calculate(shooter.getRPM(), targetRPM);
+		shooter.setSpeed(motorSpeed);
 	}
 }