Shooter

.vscode/launch.json

@@ -18,26 +18,4 @@
       "desktop": false,
     }
   ]
-}
-
-{
-  // Use IntelliSense to learn about possible attributes.
-  // Hover to view descriptions of existing attributes.
-  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
-  "version": "0.2.0",
-  "configurations": [
-
-    {
-      "type": "wpilib",
-      "name": "WPILib Desktop Debug",
-      "request": "launch",
-      "desktop": true,
-    },
-    {
-      "type": "wpilib",
-      "name": "WPILib roboRIO Debug",
-      "request": "launch",
-      "desktop": false,
-    }
-  ]
-}
+}

build.gradle

@@ -27,6 +27,13 @@ deploy {
                 // getTargetTypeClass is a shortcut to get the class type using a string
 
                 frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
+                    // Enable VisualVM connection
+                    jvmArgs.add("-Dcom.sun.management.jmxremote=true")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
+                    jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+                    jvmArgs.add("-Djava.rmi.server.hostname=10.14.58.2") // Replace TE.AM with team number
                 }
 
                 // Static files artifact

simgui-ds.json

@@ -16,10 +16,15 @@
           "incKey": 68
         },
         {
+          "decKey": 50,
           "decayRate": 0.0,
+          "incKey": 49,
           "keyRate": 0.009999999776482582
         },
-        {},
+        {
+          "decKey": 52,
+          "incKey": 51
+        },
         {
           "decKey": 74,
           "incKey": 76

src/main/java/frc/robot/ControlsMapping.java

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.ctre.CtreDrive.SysIdRoutineType;
+import frc.robot.subsystems.shooter.Shooter;
 import edu.wpi.first.wpilibj2.command.Commands;
 
 public class ControlsMapping {
@@ -21,6 +22,8 @@ public static void mapTeleopCommand() {
 		controller.rightBumper().whileTrue(Drive.getInstance().autoAlign(false));
 		controller.x().whileTrue(Drive.getInstance().autopilotAlign(true));
 		controller.y().whileTrue(Drive.getInstance().autopilotAlign(false));
+
+		controller.rightTrigger().whileTrue(Shooter.getInstance().shoot());
 	}
 
 	public static void mapSysId() {

src/main/java/frc/robot/Robot.java

@@ -21,6 +21,7 @@
 import frc.robot.Constants.Controllers;
 import frc.robot.subsystems.TelemetryManager;
 import frc.robot.subsystems.drive.*;
+import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.vision.VisionDeviceManager;
 
 /**
@@ -46,6 +47,7 @@ public Robot() {
 			VisionDeviceManager.getInstance();
 		}
 		Drive.getInstance();
+		Shooter.getInstance();
 		TelemetryManager.getInstance();
 		FollowPathCommand.warmupCommand().schedule();
 		autoChooser = new AutoSelector();

src/main/java/frc/robot/lib/control/ProfiledPIDVController.java

@@ -55,17 +55,17 @@ public ProfiledPIDVController setMeasurement(double position, double speed) {
 	}
 
 	public double getOutput() {
-		// if (controller.isContinuous) {
-		// 	double errorBound = (controller.maxRange - controller.minRange) / 2.0;
+		if (controller.isContinuous) {
+			double errorBound = (controller.maxRange - controller.minRange) / 2.0;
 
-		// 	double goalDelta =
-		// 		MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound);
-		// 	double setpointDelta =
-		// 		MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound);
+			double goalDelta =
+				MathUtil.inputModulus(goal.position - controller.positionMeasurement, -errorBound, errorBound);
+			double setpointDelta =
+				MathUtil.inputModulus(setpoint.position - controller.positionMeasurement, -errorBound, errorBound);
 
-		// 	goal.position = goalDelta + controller.positionMeasurement;
-		// 	setpoint.position = setpointDelta + controller.positionMeasurement;
-		// }
+			goal.position = goalDelta + controller.positionMeasurement;
+			setpoint.position = setpointDelta + controller.positionMeasurement;
+		}
 
 		// Advance profile by one timestep
 		setpoint = profile.calculate(Constants.DT, setpoint, goal);

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

@@ -0,0 +1,170 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.controls.CoastOut;
+import com.ctre.phoenix6.controls.ControlRequest;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Robot;
+import frc.robot.subsystems.TelemetryManager;
+
+public class Shooter extends SubsystemBase {
+    private static Shooter shooterLeftInstance;
+    private static Shooter shooterRightInstance;
+	public static Shooter getLeftInstance() {
+		if (shooterLeftInstance == null) {
+			shooterLeftInstance = new Shooter(true);
+		}
+		return shooterLeftInstance;
+	}
+
+
+	public static Shooter getRightInstance() {
+		if (shooterRightInstance == null) {
+			shooterRightInstance = new Shooter(false);
+		}
+		return shooterRightInstance;
+	}
+    private final TalonFX topMotor;
+    private final TalonFX bottomMotor;
+	private double lastReadSpeedTop;
+    private double lastReadSpeedBottom;
+	private ControlRequest topRequest = new NeutralOut();
+	private ControlRequest bottomRequest = new NeutralOut();
+
+    private FlywheelSim topSim;
+    private FlywheelSim bottomSim;
+
+    private Shooter(boolean left) {
+        super();
+
+        int bottomID;
+        int topID;
+
+        if (left) {
+            bottomID = ShooterConstants.Motors.BOTTOMLEFT.id;
+            topID = ShooterConstants.Motors.TOPLEFT.id;
+        }
+        else {
+            bottomID = ShooterConstants.Motors.BOTTOMRIGHT.id;
+            topID = ShooterConstants.Motors.TOPRIGHT.id;
+        }
+
+		bottomMotor = new TalonFX(bottomID);
+		bottomMotor.getConfigurator().apply(ShooterConstants.getConfig());
+		bottomMotor.setNeutralMode(NeutralModeValue.Coast);
+
+        topMotor = new TalonFX(topID);
+		topMotor.getConfigurator().apply(ShooterConstants.getConfig());
+		topMotor.setNeutralMode(NeutralModeValue.Coast);
+
+        if (Robot.isSimulation()) {
+            topSim = new FlywheelSim(
+                LinearSystemId.createFlywheelSystem(
+                    DCMotor.getKrakenX60(1),
+                    0.000489000861,
+                    1
+                ), DCMotor.getKrakenX60(1), 0.0);
+            bottomSim = new FlywheelSim(
+                LinearSystemId.createFlywheelSystem(
+                    DCMotor.getKrakenX60(1),
+                    0.000489000861,
+                    1
+                ), DCMotor.getKrakenX60(1), 0.0);
+        }
+		TelemetryManager.getInstance().addSendable(this);
+        setDefaultCommand(stop());
+    }
+
+    @Override
+    public void periodic() {
+        // Read inputs
+        lastReadSpeedTop = topMotor.getVelocity().getValueAsDouble();
+        lastReadSpeedBottom = bottomMotor.getVelocity().getValueAsDouble();
+        topMotor.setControl(topRequest);
+        bottomMotor.setControl(bottomRequest);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        topMotor.getSimState().setSupplyVoltage(12);
+        bottomMotor.getSimState().setSupplyVoltage(12);
+
+        topSim.setInput(topMotor.getSimState().getMotorVoltage());
+        topSim.update(0.020);
+		topMotor.getSimState()
+            .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0);
+        topMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020);
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(topSim.getCurrentDrawAmps()));
+
+        bottomSim.setInput(bottomMotor.getSimState().getMotorVoltage());
+        bottomSim.update(0.020);
+        RoboRioSim.setVInVoltage(
+            BatterySim.calculateDefaultBatteryLoadedVoltage(bottomSim.getCurrentDrawAmps()));
+		bottomMotor.getSimState()
+            .setRotorVelocity(topSim.getAngularVelocityRPM() / 60.0);
+        bottomMotor.getSimState().addRotorPosition(topSim.getAngularVelocityRPM() / 60.0 * 0.020);
+
+    }
+
+    /** Replaces the request */
+    private void setTopRequest(ControlRequest request) {
+        this.topRequest = request;
+    }    
+
+    /** Replaces the request */
+    private void setBottomRequest(ControlRequest request) {
+        this.bottomRequest = request;
+    }
+
+    /** Stops the shooter */
+    public Command stop() {
+        return runOnce(
+            () -> {
+                setTopRequest(new CoastOut());
+                setBottomRequest(new CoastOut());
+            }
+        ).withName("Stopped");
+    }
+
+    public Command shoot(double topSpeed, double bottomSpeed) {
+        return runOnce(() -> {
+            setTopRequest(new VelocityVoltage(topSpeed));
+            setBottomRequest(new VelocityVoltage(bottomSpeed));
+        }).andThen(Commands.repeatingSequence(Commands.none())).withName("Shooting");
+    }
+
+    public Command shoot() {
+        return shoot(512, -512);
+    }
+
+    @Override
+	public void initSendable(SendableBuilder builder) {
+		super.initSendable(builder);
+
+        builder.addDoubleProperty(
+            "TopSpeed", 
+            () -> lastReadSpeedTop, 
+            null);
+		TelemetryManager.makeSendableTalonFX("Top", topMotor, builder);
+
+        builder.addDoubleProperty(
+            "BottomSpeed", 
+            () -> lastReadSpeedBottom, 
+            null);
+		TelemetryManager.makeSendableTalonFX("Bottom", bottomMotor, builder);
+	}
+}

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java

@@ -0,0 +1,41 @@
+package frc.robot.subsystems.shooter;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.configs.VoltageConfigs;
+
+public final class ShooterConstants {		
+    public static final double GEAR_RATIO = 1;
+
+    /** Motor ids */
+    public static enum Motors {
+        TOPLEFT(12),
+        BOTTOMLEFT(13),
+        TOPRIGHT(12),
+        BOTTOMRIGHT(13);
+        public final int id;
+        private Motors(int id) {
+            this.id = id;
+        }
+    }
+
+    /** Config for shooter motors */
+    public static TalonFXConfiguration getConfig() {
+        return new TalonFXConfiguration()
+            .withSlot0(new Slot0Configs()
+                .withKV(0.0)
+                .withKP(0.3 )
+                .withKI(0.001)
+                .withKD(0.0)
+                .withKA(0.1)
+                .withKS(0.1)
+                .withKV(0.1)) // placeholder values
+            .withCurrentLimits(new CurrentLimitsConfigs()
+                .withStatorCurrentLimit(60)
+                .withSupplyCurrentLimit(60))
+            .withVoltage(new VoltageConfigs()
+                .withPeakForwardVoltage(12.0)
+                .withPeakReverseVoltage(-12.0));
+    }
+}