on controller shooter pid + paths

.pathplanner/settings.json

@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.88,
+  "robotLength": 0.88,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.03
+}

src/main/deploy/pathplanner/autos/Right 1S 2A.auto

@@ -0,0 +1,74 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.1134340217398382,
+      "y": 6.525338178881486
+    },
+    "rotation": 42.79740183823424
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Preloaded+Pickup"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 2.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Amp Handoff"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 1 to amp"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Amp Note Shot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Leftmost Center Note"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 1 to amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Right 2 Amp.auto

@@ -0,0 +1,50 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Speaker Note Shot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Preloaded+Pickup"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Amp Handoff"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Note 1 to amp"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Amp Note Shot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Right 3 Amp.auto

@@ -0,0 +1,43 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.1134340217398382,
+      "y": 6.525338178881486
+    },
+    "rotation": 33.13560954871888
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Preloaded+Pickup"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 1 to amp"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Leftmost Center Note"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Note 1 to amp"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Leftmost Center Note.path

@@ -0,0 +1,102 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.347017520241028,
+        "y": 6.993104356575341
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.446268037830382,
+        "y": 7.110045900999739
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.071755505338439,
+        "y": 7.297152372084174
+      },
+      "prevControl": {
+        "x": 5.077752377722138,
+        "y": 7.262069908751458
+      },
+      "nextControl": {
+        "x": 6.725659674531382,
+        "y": 7.320231342764915
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.63877220062539,
+        "y": 7.425788070951012
+      },
+      "prevControl": {
+        "x": 6.621380764133118,
+        "y": 7.460870534278332
+      },
+      "nextControl": {
+        "x": 8.656163637117663,
+        "y": 7.390705607623692
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.545818145767345,
+        "y": 6.642279723305886
+      },
+      "prevControl": {
+        "x": 6.323179825848877,
+        "y": 7.12758713266797
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 172.50414236027015,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 0.85,
+      "maxWaypointRelativePos": 1.35,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0
+      }
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": -179.23610153906992,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Note 1 to Speaker.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2654580294915574,
+        "y": 6.291455090032687
+      },
+      "prevControl": {
+        "x": 1.7098358983042754,
+        "y": 6.525338178881486
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -145.88552705465875,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 179.55484341368705,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Note 1 to amp.path

@@ -0,0 +1,63 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.353014392638472,
+        "y": 6.993104356579084
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.768306670516475,
+        "y": 7.004798511021523
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8384715971711143,
+        "y": 7.624588696470841
+      },
+      "prevControl": {
+        "x": 1.7950792079196904,
+        "y": 7.315863019194643
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "New Event Marker",
+      "waypointRelativePos": 0,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": []
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 90.0,
+    "rotateFast": true
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -178.36342295838335,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Preloaded+Pickup.path

@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1134340217398382,
+        "y": 6.525338178881486
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.148516485067158,
+        "y": 7.191904982100564
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.3179319293111518,
+        "y": 6.981410202136645
+      },
+      "prevControl": {
+        "x": 1.8033891338437944,
+        "y": 6.981410202136645
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 40.97173633351488,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -80,11 +80,13 @@ public class RobotContainer {
 
         intake = new Intake();
 
-        shooter = new Shooter();
+        indexer = new Indexer();
+
+        shooter = new Shooter(indexer.getBeamBreak());
 
         climber = new Climber(shooter.getShooterAngle());
 
-        indexer = new Indexer();
+        
 
         // An example Named Command, doesn't need to remain once we start actually adding real things
         // ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -7,7 +7,7 @@ import edu.wpi.first.math.util.Units;
 public final class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 4.6;
+    public static final double kMaxSpeedMetersPerSecond = 4.1;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     public static final double kDirectionSlewRate = 2.4; // radians per second

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -10,8 +10,7 @@ public class ShooterConstants {
     public static final double kShooterI = 0.0;
     public static final double kShooterD = 0.0;
 
-    public static final double kSShooterFF = 0.0;
+    public static final double kShooterFF = 0.0;
-    public static final double kVShooterFF = 0.0;
 
     public static final double kShooterPivotP = 0.0;
     public static final double kShooterPivotI = 0.0;

src/main/java/frc/robot/constants/SwerveModuleConstants.java

@@ -49,6 +49,6 @@ public class SwerveModuleConstants {
     public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake;
     public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake;
 
-    public static final int kDrivingMotorCurrentLimit = 60; // amps
+    public static final int kDrivingMotorCurrentLimit = 65; // amps
     public static final int kTurningMotorCurrentLimit = 30; // amps
 }

src/main/java/frc/robot/subsystems/Climber.java

@@ -15,7 +15,6 @@ public class Climber extends SubsystemBase {
 
     private DoubleSupplier shooterAngle;
 
-    //TODO What tells the climber to stop climbing when it achieves the target height?
     public Climber(DoubleSupplier shooterAngle) {
         if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
             motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);

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

@@ -5,12 +5,12 @@ import java.util.function.DoubleSupplier;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -28,54 +28,49 @@ public class Shooter extends SubsystemBase{
 
     private Encoder pivotEncoder;
 
-    private PIDController topShooterPID;
+    private SparkPIDController topPID;
-    private SimpleMotorFeedforward topShooterFF;
+    private SparkPIDController bottomPID;
-
-    private PIDController bottomShooterPID;
-    private SimpleMotorFeedforward bottomShooterFF;
 
     private PIDController shooterPivotPID;
     private ArmFeedforward shooterPivotFF;
 
     private DigitalInput shooterBeamBreak;
 
-    public Shooter(){
+    public Shooter(BooleanSupplier indexerBeamBreak){
         topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
         bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
-        
+        shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
-        topShooter.setSmartCurrentLimit(40);
-        topShooter.burnFlash();
-
-        bottomShooter.setSmartCurrentLimit(40);
-        bottomShooter.burnFlash();
         
         topEncoder = topShooter.getEncoder();
         bottomEncoder = bottomShooter.getEncoder();
 
-        shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
+        topPID = topShooter.getPIDController();
+        topPID.setFeedbackDevice(topEncoder);
+        bottomPID = bottomShooter.getPIDController();
+        bottomPID.setFeedbackDevice(bottomEncoder);
 
         shooterPivot.setSmartCurrentLimit(50);
-        shooterPivot.burnFlash();
+        topShooter.setSmartCurrentLimit(40);
+        bottomShooter.setSmartCurrentLimit(40);
 
         pivotEncoder = new Encoder(0, 1);
         pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
 
         shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
 
-        //TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed
+        bottomShooter.burnFlash();
-        topShooterPID = new PIDController(
+        shooterPivot.burnFlash();
-            ShooterConstants.kShooterP, 
+        topShooter.burnFlash();
-            ShooterConstants.kShooterI, 
-            ShooterConstants.kShooterD
-        );
-        bottomShooterPID = new PIDController(
-            ShooterConstants.kShooterP, 
-            ShooterConstants.kShooterI, 
-            ShooterConstants.kShooterD
-        );
 
-        topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
+        topPID.setP(ShooterConstants.kShooterP);
-        topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
+        topPID.setI(ShooterConstants.kShooterI);
+        topPID.setD(ShooterConstants.kShooterD);
+        topPID.setFF(ShooterConstants.kShooterFF);
+
+        bottomPID.setP(ShooterConstants.kShooterP);
+        bottomPID.setI(ShooterConstants.kShooterI);
+        bottomPID.setI(ShooterConstants.kShooterD);
+        bottomPID.setFF(ShooterConstants.kShooterFF);
 
         shooterPivotPID = new PIDController(
             ShooterConstants.kShooterPivotP,
@@ -83,28 +78,26 @@ public class Shooter extends SubsystemBase{
             ShooterConstants.kShooterPivotD
         );
 
-        shooterPivotFF = new ArmFeedforward(
-            ShooterConstants.kSShooterPivotFF,
-            ShooterConstants.kGShooterPivotFF, 
-            ShooterConstants.kVShooterPivotFF
-        );
     }
 
     public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
         return run(()-> {
-            shooterPivot.setIdleMode(IdleMode.kBrake);
+            angleAndSpeedControl(setpointAngle, topRPM, bottomRPM);
+        });
+    }
+
+    private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){
+        shooterPivot.setIdleMode(IdleMode.kBrake);
 
             shooterPivot.setVoltage(
                 shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + 
                 shooterPivotFF.calculate(setpointAngle, 0.0));
 
-            topShooter.setVoltage(topShooterPID.calculate(topEncoder.getVelocity(), topRPM)+topShooterFF.calculate(topRPM));
+            topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
-
+            bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
-            bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM));
-        });
     }
 
-    public Command recieveNote(){
+    public Command ampHandoff(){
         return run(() -> {
             shooterPivot.setIdleMode(IdleMode.kBrake);
 
@@ -112,12 +105,10 @@ public class Shooter extends SubsystemBase{
                 shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + 
                 shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
             
-            if(shooterBeamBreak.get()){
+            if(shooterBeamBreak.get() || shooterBeamBreak.get()){
-                topShooter.set(0.25);
+                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 1000.0, 1000.0);
-                bottomShooter.set(0.25);
             }else{
-                topShooter.set(0.);
+                angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0);
-                bottomShooter.set(0.); 
             }
         });
     }

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.1.2",
+    "version": "2024.2.3",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,7 +12,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.1.2"
+            "version": "2024.2.3"
         }
     ],
     "jniDependencies": [],
@@ -20,7 +20,7 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.1.2",
+            "version": "2024.2.3",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,

vendordeps/REVLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2024.2.0",
+    "version": "2024.2.1",
     "frcYear": "2024",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2024.2.0"
+            "version": "2024.2.1"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.0",
+            "version": "2024.2.1",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -37,7 +37,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2024.2.0",
+            "version": "2024.2.1",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -55,7 +55,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2024.2.0",
+            "version": "2024.2.1",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,