beginnings of shooter subsystem

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -0,0 +1,26 @@
+package frc.robot.constants;
+
+public class ShooterConstants {
+    
+    public static final int kTopShooterID = 11;
+    public static final int kBottomShooterID = 12;
+    public static final int kShooterPivotID = 13;
+
+    public static final double kShooterP = 0.0;
+    public static final double kShooterI = 0.0;
+    public static final double kShooterD = 0.0;
+
+    public static final double kSShooterFF = 0.0;
+    public static final double kVShooterFF = 0.0;
+
+    public static final double kShooterPivotP = 0.0;
+    public static final double kShooterPivotI = 0.0;
+    public static final double kShooterPivotD = 0.0;
+
+    public static final double kSShooterPivotFF = 0.0;
+    public static final double kGShooterPivotFF = 0.0;
+    public static final double kVShooterPivotFF = 0.0;
+
+    public static final double kMaxPivotSpeed = 0.0;
+    public static final double kMaxPivotAcceleration = 0.0;
+}

src/main/java/frc/robot/subsystems/Shooter.java

@@ -0,0 +1,69 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ShooterConstants;
+
+public class Shooter extends SubsystemBase{
+    private CANSparkMax topShooter;
+    private CANSparkMax bottomShooter;
+
+    private RelativeEncoder bottomEncoder;
+    private RelativeEncoder topEncoder;
+
+    private CANSparkMax shooterPivot;
+
+    private Encoder pivotEncoder;
+
+    private PIDController topShooterPID;
+    private SimpleMotorFeedforward topShooterFF;
+
+    private PIDController bottomShooterPID;
+    private SimpleMotorFeedforward bottomShooterFF;
+
+    private ProfiledPIDController shooterPivotPID;
+    private ArmFeedforward shooterPivotFF;
+
+    public Shooter(){
+        topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
+        bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
+
+        topEncoder = topShooter.getEncoder();
+        bottomEncoder = bottomShooter.getEncoder();
+
+        shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
+
+        pivotEncoder = new Encoder(0, 1);
+
+        topShooterPID = new PIDController(ShooterConstants.kShooterP, ShooterConstants.kShooterI, ShooterConstants.kShooterD);
+        bottomShooterPID = new PIDController(ShooterConstants.kShooterP, ShooterConstants.kShooterI, ShooterConstants.kShooterD);
+
+        topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
+        topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
+
+        shooterPivotPID = new ProfiledPIDController(
+        ShooterConstants.kShooterPivotP,
+        ShooterConstants.kShooterPivotI,
+        ShooterConstants.kShooterPivotD,
+        new TrapezoidProfile.Constraints(ShooterConstants.kMaxPivotSpeed, ShooterConstants.kMaxPivotAcceleration));
+
+        shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF,
+        ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF);
+    }
+
+    public Command setShooterAngle(double setpointAngle){
+        return run(()-> {
+            shooterPivot.setVoltage(shooterPivotPID.calculate(pivotEncoder.getPosition(), setpointAngle) + );
+        });
+    }
+}