add shooter subsystem

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -7,6 +7,7 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.shooter.Shooter;
 import com.stuypulse.robot.subsystems.conveyor.Conveyor;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
@@ -22,7 +23,9 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+    public final Shooter shooter = Shooter.getInstance();
     public final Conveyor conveyor = Conveyor.getInstance();
+
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 

src/main/java/com/stuypulse/robot/commands/shooter/ShooterPodiumShot.java

@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ShooterPodiumShot extends ShooterSetRPM{
+
+    public ShooterPodiumShot() {
+        super(Settings.Shooter.PODIUM_SHOT_LEFT_RPM, Settings.Shooter.PODIUM_SHOT_RIGHT_RPM);
+    }
+}

src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java

@@ -0,0 +1,27 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ShooterSetRPM extends InstantCommand {
+    private final Shooter shooter;
+    private final Number leftTargetRPM;
+    private final Number rightTargetRPM;
+
+    public ShooterSetRPM(Number leftTargetRPM, Number rightTargetRPM) {
+        shooter = Shooter.getInstance();
+        this.leftTargetRPM = leftTargetRPM;
+        this.rightTargetRPM = rightTargetRPM;
+
+        addRequirements(shooter);
+    }
+
+    @Override
+    public void initialize() {
+        shooter.setLeftTargetRPM(leftTargetRPM);
+        shooter.setRightTargetRPM(rightTargetRPM);
+    }
+
+}
+

src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java

@@ -0,0 +1,8 @@
+package com.stuypulse.robot.commands.shooter;
+
+public class ShooterStop extends ShooterSetRPM {    
+
+    public ShooterStop(){
+        super(0, 0);
+    }
+}

src/main/java/com/stuypulse/robot/constants/Motors.java

@@ -24,6 +24,11 @@ public interface Motors {
 
     /** Classes to store all of the values a motor needs */
 
+    public interface Shooter {
+        CANSparkMaxConfig LEFT_SHOOTER = new CANSparkMaxConfig(false,IdleMode.kCoast);
+        CANSparkMaxConfig RIGHT_SHOOTER = new CANSparkMaxConfig(false,IdleMode.kCoast);      
+    }
+
     public interface Conveyor{ 
 
         CANSparkMaxConfig GANDALF_MOTOR = new CANSparkMaxConfig(false, IdleMode.kBrake);

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -12,12 +12,16 @@ public interface Gamepad {
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
+
+    public interface Shooter {
+        int LEFT_MOTOR = 20; 
+        int RIGHT_MOTOR = 21;
+    }
 
     public interface Conveyor {
         int GANDALF_MOTOR_PORT = 50;
         int SHOOTER_FEEDER_MOTOR_PORT = 51;
 
         int IR_SENSOR_PORT = 0;
-
     }
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -14,9 +14,28 @@
  * values that we can edit on Shuffleboard.
  */
 public interface Settings {
+    double DT = 0.02;
 
-    public interface Conveyor {
+    public interface Shooter {
+        double MOMENT_OF_INERTIA = 1;
+
+        SmartNumber PODIUM_SHOT_LEFT_RPM = new SmartNumber("Shooter/Podium Shot Right RPM", 0);
+        SmartNumber PODIUM_SHOT_RIGHT_RPM = new SmartNumber("Shooter/Podium Shot Left RPM", 0);
+
+        public interface Feedforward {
+            SmartNumber kV = new SmartNumber("Shooter/Feedforward kV",0);
+            SmartNumber kA = new SmartNumber("Shooter/Feedforward kA",0);
+            SmartNumber kS = new SmartNumber("Shooter/Feedforward kS",0); //CHANGE LATER            
+        }
 
+        public interface PID {
+            SmartNumber kP = new SmartNumber("Shooter/PID kP", 0);
+            SmartNumber kI = new SmartNumber("Shooter/PID kI", 0);
+            SmartNumber kD = new SmartNumber("Shooter/PID kD", 0);
+        }
+    }
+
+    public interface Conveyor {
         SmartNumber GANDALF_SHOOTER_SPEED = new SmartNumber("Conveyor/Gandalf Shooter Speed", 1);
         SmartNumber GANDALF_AMP_SPEED = new SmartNumber("Conveyor/Gandalf Amp Speed", -1);
         SmartNumber SHOOTER_FEEDER_SPEED = new SmartNumber("Conveyor/Shooter Feeder Speed", 1);
@@ -25,4 +44,3 @@ public interface Conveyor {
     }
 
 }
-

src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java

@@ -0,0 +1,80 @@
+package com.stuypulse.robot.subsystems.shooter;
+
+import com.stuypulse.robot.constants.Settings.Shooter.Feedforward;
+import com.stuypulse.robot.constants.Settings.Shooter.PID;
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.control.feedforward.MotorFeedforward;
+import com.stuypulse.stuylib.network.SmartNumber;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class Shooter extends SubsystemBase {
+
+    private static final Shooter instance;
+
+    static {
+        if (RobotBase.isReal()) {
+            instance = new ShooterImpl();
+        } else {
+            instance = new SimShooter();
+        }
+    }
+
+    public static Shooter getInstance() {
+        return instance;
+    }
+
+    private final SmartNumber leftTargetRPM;
+    private final SmartNumber rightTargetRPM;
+
+    private final Controller leftController;
+    private final Controller rightController;
+
+    public Shooter() {
+        leftTargetRPM = new SmartNumber("Shooter/Left Target RPM", 0);
+        rightTargetRPM = new SmartNumber("Shooter/Right Target RPM", 0);
+        leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity()
+                        .add(new PIDController(PID.kP, PID.kI, PID.kD));
+        rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity()
+                        .add(new PIDController(PID.kP, PID.kI, PID.kD));  
+    }    
+
+    public double getLeftTargetRPM() {
+        return leftTargetRPM.get();
+    }   
+
+    public double getRightTargetRPM() {
+        return rightTargetRPM.get();
+    } 
+
+    public void setLeftTargetRPM(Number leftTargetRPM) {
+        this.leftTargetRPM.set(leftTargetRPM);
+    }
+
+    public void setRightTargetRPM(Number rightTargetRPM) {
+        this.rightTargetRPM.set(rightTargetRPM);
+    }
+
+    public abstract void stop();
+    public abstract double getLeftShooterRPM();
+    public abstract double getRightShooterRPM();
+
+    protected abstract void setLeftMotorVoltageImpl(double voltage);
+    protected abstract void setRightMotorVoltageImpl(double voltage);
+
+    @Override
+    public void periodic() {
+
+        leftController.update(getLeftTargetRPM(), getLeftShooterRPM());
+        rightController.update(getRightTargetRPM(), getRightShooterRPM());
+        setLeftMotorVoltageImpl(leftController.getOutput());
+        setRightMotorVoltageImpl(rightController.getOutput());
+
+        periodicallyCalled();
+    }
+
+    public void periodicallyCalled() {}
+}

src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java

@@ -0,0 +1,64 @@
+package com.stuypulse.robot.subsystems.shooter;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.stuypulse.robot.constants.Motors;
+import com.stuypulse.robot.constants.Ports;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ShooterImpl extends Shooter {
+    private final CANSparkMax leftMotor;
+    private final CANSparkMax rightMotor;
+
+    private final RelativeEncoder leftEncoder;
+    private final RelativeEncoder rightEncoder;
+
+
+    public ShooterImpl() {
+        leftMotor = new CANSparkMax(Ports.Shooter.LEFT_MOTOR, MotorType.kBrushless);
+        rightMotor = new CANSparkMax(Ports.Shooter.RIGHT_MOTOR, MotorType.kBrushless);
+
+        leftEncoder = leftMotor.getEncoder();
+        rightEncoder = rightMotor.getEncoder();
+
+        Motors.Shooter.LEFT_SHOOTER.configure(leftMotor);
+        Motors.Shooter.RIGHT_SHOOTER.configure(rightMotor);
+    }
+
+    @Override
+    public void stop() {
+        leftMotor.setVoltage(0);
+        rightMotor.setVoltage(0);
+    }
+
+    @Override
+    public double getLeftShooterRPM() {
+        return leftEncoder.getVelocity();
+    }
+
+    @Override
+    public double getRightShooterRPM() {
+        return rightEncoder.getVelocity();
+    }
+
+    @Override
+    public void setLeftMotorVoltageImpl(double voltage) {
+        leftMotor.setVoltage(voltage);
+    }
+
+    @Override
+    public void setRightMotorVoltageImpl(double voltage) {
+        rightMotor.setVoltage(voltage);
+    }
+
+    @Override
+    public void periodicallyCalled() {        
+        SmartDashboard.putNumber("Shooter/Right RPM", getRightShooterRPM());
+        SmartDashboard.putNumber("Shooter/Left RPM", getLeftShooterRPM());
+
+        SmartDashboard.putNumber("Shooter/Left Voltage", leftMotor.getBusVoltage()*leftMotor.getAppliedOutput());
+        SmartDashboard.putNumber("Shooter/Right Voltage", rightMotor.getBusVoltage()*rightMotor.getAppliedOutput());
+    }
+}                                
+

src/main/java/com/stuypulse/robot/subsystems/shooter/SimShooter.java

@@ -0,0 +1,49 @@
+package com.stuypulse.robot.subsystems.shooter;
+
+import com.stuypulse.stuylib.network.SmartNumber;
+import com.stuypulse.robot.constants.Settings;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class SimShooter extends Shooter {
+    private FlywheelSim leftWheel;
+    private FlywheelSim rightWheel;
+
+    public SimShooter() {
+        leftWheel = new FlywheelSim(DCMotor.getNEO(1), 1, Settings.Shooter.MOMENT_OF_INERTIA);
+        rightWheel = new FlywheelSim(DCMotor.getNEO(1), 1, Settings.Shooter.MOMENT_OF_INERTIA);
+    }
+
+    public void stop() {
+        leftWheel.setInputVoltage(0);
+        rightWheel.setInputVoltage(0);
+    }
+
+    public double getLeftShooterRPM() {
+        return leftWheel.getAngularVelocityRPM();
+    }
+
+    public double getRightShooterRPM() {
+        return rightWheel.getAngularVelocityRPM();
+    }
+
+    public void setRightMotorVoltageImpl(double voltage) {
+        rightWheel.setInputVoltage(voltage);
+    }
+
+    public void setLeftMotorVoltageImpl(double voltage) {
+        leftWheel.setInputVoltage(voltage);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        leftWheel.update(Settings.DT);
+        rightWheel.update(Settings.DT);
+
+
+        SmartDashboard.putNumber("Shooter/Right RPM", getRightShooterRPM());
+        SmartDashboard.putNumber("Shooter/Left RPM", getLeftShooterRPM());
+    }
+
+}