wip adding GamepieceLauncher

photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java

@@ -32,12 +32,15 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import frc.robot.subsystems.GamepieceLauncher;
 import frc.robot.subsystems.drivetrain.SwerveDrive;
 
 public class Robot extends TimedRobot {
     private SwerveDrive drivetrain;
     private Vision vision;
 
+    private GamepieceLauncher gpLauncher;
+
     private XboxController controller;
 
 
@@ -47,10 +50,16 @@ public void robotInit() {
         vision = new Vision();
 
         controller = new XboxController(0);
+
+        gpLauncher = new GamepieceLauncher();
     }
 
     @Override
     public void robotPeriodic() {
+        // Update Gamepiece Launcher Subsystem
+        gpLauncher.periodic();
+
+        // Update drivetrain subsystem
         drivetrain.periodic();
 
         // Correct pose estimate with vision measurements
@@ -64,7 +73,7 @@ public void robotPeriodic() {
                             est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
                 });
 
-        // Example only!
+        // Test/Example only!
         // Apply an offset to pose estimator to test vision correction
         // You probably don't want this on a real robot, just delete it.
         if (controller.getBButtonPressed()) {
@@ -99,6 +108,11 @@ public void teleopPeriodic() {
 
         // Command drivetrain motors based on target speeds
         drivetrain.drive(forward, strafe, turn);
+
+        // Calculate whether the gamepiece launcher runs based on our global pose estimate.
+        var curPose = drivetrain.getPose();
+        var shouldRun = (curPose.getX() > 5.0 && curPose.getY() < 5.0); // Close enough to blue speaker
+        gpLauncher.setRunning(shouldRun);
     }
 
     @Override
@@ -113,6 +127,9 @@ public void simulationPeriodic() {
         debugField.getObject("EstimatedRobot").setPose(drivetrain.getPose());
         debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses());
 
+        // Update gamepiece launcher simulation
+        gpLauncher.simulationPeriodic();
+
         // Calculate battery voltage sag due to current draw
         RoboRioSim.setVInVoltage(
                 BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw()));

photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java

@@ -0,0 +1,57 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class GamepieceLauncher {
+
+    private final PWMSparkMax motor;
+    private final Encoder encoder;
+
+    private final double LAUNCH_SPEED_RPM = 2500;
+    private double curDesSpd;
+    private double curMotorCmd = 0.0;
+
+    public GamepieceLauncher() {
+        motor = new PWMSparkMax(8);
+        encoder = new Encoder(16, 17);
+        simulationInit();
+    }
+
+    public void setRunning(boolean shouldRun) {
+        curDesSpd = shouldRun ? LAUNCH_SPEED_RPM : 0.0;
+    }
+
+    public void periodic() {
+        double actSpd =encoder.getRate();
+        double err = curDesSpd - actSpd;
+        curMotorCmd = 1.0 * err;
+        curMotorCmd = MathUtil.clamp(curMotorCmd, 0.0, 1.0);
+        motor.set(curMotorCmd);
+
+        SmartDashboard.putNumber("GPLauncher Des Spd (RPM)", curDesSpd);
+        SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", actSpd);
+
+    }
+
+    // -- SIMULATION SUPPORT
+    private DCMotor motorSim;
+    private EncoderSim encoderSim;
+
+    private void simulationInit() {
+        motorSim = DCMotor.getFalcon500(1);
+        encoderSim = new EncoderSim(encoder);
+    }
+
+    public void simulationPeriodic() {
+        var spd = motorSim.getSpeed(0.0, curMotorCmd*RobotController.getBatteryVoltage());
+        encoderSim.setRate(Units.radiansPerSecondToRotationsPerMinute(spd));
+    }
+
+}