Merge remote-tracking branch 'origin/Levi-CoralScorer' into Oscar-Cor…

…al-Scorer

src/main/deploy/pathplanner/autos/System Test.auto

@@ -0,0 +1,45 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Not Moving"
+                }
+              },
+              {
+                "type": "race",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "Score"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 2.0
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Not Moving.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.2493016654053966,
+        "y": 6.018672965112581
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.719372230184102,
+        "y": 5.978918867512457
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.1,
+    "maxAcceleration": 0.1,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 1200.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/java/frc/robot/RobotContainer.java

@@ -26,6 +26,7 @@
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -40,6 +41,7 @@
 import frc.robot.commands.CoralScorerCommand;
 import frc.robot.commands.DriveCommands;
 import frc.robot.subsystems.CoralScorer.CoralScorer;
+import frc.robot.subsystems.CoralScorer.CoralScorerIO;
 import frc.robot.subsystems.CoralScorer.CoralScorerIOSpark;
 import frc.robot.subsystems.accelerometer.Accelerometer;
 import frc.robot.subsystems.drive.Drive;
@@ -75,11 +77,12 @@ public class RobotContainer {
   private final Drive m_drivebase;
 
   private final Flywheel m_flywheel;
+  private final CoralScorer m_CoralScorer;
+
   // These are "Virtual Subsystems" that report information but have no motors
   private final Accelerometer m_accel;
   private final Vision m_vision;
   private final PowerMonitoring m_power;
-  private final CoralScorer m_CoralScorer = new CoralScorer(new CoralScorerIOSpark());
 
   /** Dashboard inputs ***************************************************** */
   // AutoChoosers for both supported path planning types
@@ -110,6 +113,7 @@ public RobotContainer() {
         // YAGSL drivebase, get config from deploy directory
         m_drivebase = new Drive();
         m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX());
+        m_CoralScorer = new CoralScorer(new CoralScorerIOSpark());
         m_vision =
             switch (Constants.getVisionType()) {
               case PHOTON ->
@@ -134,6 +138,7 @@ public RobotContainer() {
         // Sim robot, instantiate physics sim IO implementations
         m_drivebase = new Drive();
         m_flywheel = new Flywheel(new FlywheelIOSim() {});
+        m_CoralScorer = new CoralScorer(new CoralScorerIO() {});
         m_vision =
             new Vision(
                 m_drivebase::addVisionMeasurement,
@@ -146,6 +151,7 @@ public RobotContainer() {
         // Replayed robot, disable IO implementations
         m_drivebase = new Drive();
         m_flywheel = new Flywheel(new FlywheelIO() {});
+        m_CoralScorer = new CoralScorer(new CoralScorerIO() {});
         m_vision =
             new Vision(m_drivebase::addVisionMeasurement, new VisionIO() {}, new VisionIO() {});
         m_accel = new Accelerometer(m_drivebase.getGyro());
@@ -157,6 +163,9 @@ public RobotContainer() {
     // ``m_drivebase``, as that is automatically monitored.
     m_power = new PowerMonitoring(batteryCapacity, m_flywheel);
 
+    // Idk where this is suppose to go. but I think this works, just setting up auto commands
+    NamedCommands.registerCommand("Score", new CoralScorerCommand(m_CoralScorer, -0.75));
+
     // Set up the SmartDashboard Auto Chooser based on auto type
     switch (Constants.getAutoType()) {
       case PATHPLANNER:
@@ -281,6 +290,19 @@ private void configureBindings() {
                         () -> turnStickX.value()),
                 m_drivebase));
 
+    m_CoralScorer.setDefaultCommand(
+        Commands.run(
+            () ->
+                m_CoralScorer.runVolts(
+                    driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()),
+            m_CoralScorer));
+    driverController
+        .x()
+        .whileTrue(
+            new CoralScorerCommand(
+                m_CoralScorer,
+                driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()));
+
     // Press A button -> BRAKE
     driverController
         .a()

src/main/java/frc/robot/subsystems/CoralScorer/CoralSocrerIOTalonFX.java

@@ -0,0 +1,3 @@
+package frc.robot.subsystems.CoralScorer;
+
+public class CoralSocrerIOTalonFX {}