Added gamepiece launcher logic

PhotonVision · Sep 5, 2024 · 1b4e840 · 1b4e840
1 parent e819980
commit 1b4e840
Show file tree

Hide file tree

Showing 3 changed files with 49 additions and 13 deletions.
diff --git a/photonlib-java-examples/poseest/simgui.json b/photonlib-java-examples/poseest/simgui.json
@@ -117,5 +117,43 @@
   },
   "NetworkTables Info": {
     "visible": true
+  },
+  "NetworkTables View": {
+    "visible": false
+  },
+  "Plot": {
+    "Plot <0>": {
+      "plots": [
+        {
+          "backgroundColor": [
+            0.0,
+            0.0,
+            0.0,
+            0.8500000238418579
+          ],
+          "height": 332,
+          "series": [
+            {
+              "color": [
+                0.2980392277240753,
+                0.44705885648727417,
+                0.6901960968971252,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Act Spd (RPM)"
+            },
+            {
+              "color": [
+                0.8666667342185974,
+                0.5176470875740051,
+                0.32156863808631897,
+                1.0
+              ],
+              "id": "NT:/SmartDashboard/GPLauncher Des Spd (RPM)"
+            }
+          ]
+        }
+      ]
+    }
   }
 }
diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java
@@ -111,7 +111,7 @@ public void teleopPeriodic() {
 
         // 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
+        var shouldRun = (curPose.getY() > 2.0 && curPose.getX() < 4.0); // Close enough to blue speaker
         gpLauncher.setRunning(shouldRun);
     }
 

diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/subsystems/GamepieceLauncher.java
@@ -3,24 +3,21 @@
 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.simulation.FlywheelSim;
 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();
     }
 
@@ -29,29 +26,30 @@ public void setRunning(boolean shouldRun) {
     }
 
     public void periodic() {
-        double actSpd =encoder.getRate();
-        double err = curDesSpd - actSpd;
-        curMotorCmd = 1.0 * err;
+        double maxRPM = Units.radiansPerSecondToRotationsPerMinute(DCMotor.getFalcon500(1).freeSpeedRadPerSec);
+        curMotorCmd = curDesSpd / maxRPM;
         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 FlywheelSim launcherSim;
 
     private void simulationInit() {
         motorSim = DCMotor.getFalcon500(1);
-        encoderSim = new EncoderSim(encoder);
+        launcherSim = new FlywheelSim(motorSim, 1.0, 0.002);
     }
 
     public void simulationPeriodic() {
-        var spd = motorSim.getSpeed(0.0, curMotorCmd*RobotController.getBatteryVoltage());
-        encoderSim.setRate(Units.radiansPerSecondToRotationsPerMinute(spd));
+        launcherSim.setInputVoltage(curMotorCmd*RobotController.getBatteryVoltage());
+        launcherSim.update(0.02);
+        var spd = launcherSim.getAngularVelocityRPM();
+        SmartDashboard.putNumber("GPLauncher Act Spd (RPM)", spd);
+
     }
 
 }