Comp/ne district

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

@@ -49,7 +49,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Amp midline note path"
+                  "pathName": "Amp midline note path 1"
                 }
               },
               {
@@ -80,7 +80,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Amp midline note path"
+                  "pathName": "Amp midline note path 2"
                 }
               },
               {

src/main/deploy/pathplanner/autos/Source auto.auto

@@ -24,7 +24,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Source midline pickup"
+                  "pathName": "Source midline pickup 1"
                 }
               },
               {
@@ -55,7 +55,7 @@
               {
                 "type": "path",
                 "data": {
-                  "pathName": "Source midline pickup"
+                  "pathName": "Source midline pickup 2"
                 }
               },
               {

src/main/deploy/pathplanner/paths/Amp midline note path 2.path

@@ -0,0 +1,91 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.05,
+        "y": 6.991369417831561
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.822691702444745,
+        "y": 6.628333484912334
+      },
+      "isLocked": false,
+      "linkedName": "Amp shooting"
+    },
+    {
+      "anchor": {
+        "x": 8.35,
+        "y": 5.796864090161846
+      },
+      "prevControl": {
+        "x": 8.35,
+        "y": 5.846864090161846
+      },
+      "nextControl": {
+        "x": 8.35,
+        "y": 5.310275550794204
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.35,
+        "y": 3.1385041942694443
+      },
+      "prevControl": {
+        "x": 8.35,
+        "y": 3.7623672132546333
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Amp midline pickup end"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7,
+      "rotationDegrees": 159.4149015548405,
+      "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 1.9499999999999997,
+      "rotationDegrees": 180.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 0.8,
+      "maxWaypointRelativePos": 1.8499999999999996,
+      "constraints": {
+        "maxVelocity": 1.5,
+        "maxAcceleration": 5.0,
+        "maxAngularVelocity": 360.0,
+        "maxAngularAcceleration": 720.0
+      }
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Amp",
+  "previewStartingState": {
+    "rotation": -152.74467162505698,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Source midline pickup 2.path

@@ -0,0 +1,102 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.44283883432246,
+        "y": 4.578937089400569
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.8598500563620233,
+        "y": 1.7097821356840999
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.775848356261618,
+        "y": 1.8971555204166035
+      },
+      "prevControl": {
+        "x": 4.838023331510841,
+        "y": 1.8971555204166035
+      },
+      "nextControl": {
+        "x": 6.771517371227376,
+        "y": 1.8971555204166035
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.29,
+        "y": 2.4475648380683346
+      },
+      "prevControl": {
+        "x": 8.29,
+        "y": 2.3975648380683348
+      },
+      "nextControl": {
+        "x": 8.29,
+        "y": 2.8556829478470425
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.29,
+        "y": 4.871708003045107
+      },
+      "prevControl": {
+        "x": 8.29,
+        "y": 3.606783564708028
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Source midline pickup end"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.6,
+      "rotationDegrees": -160.10051919689218,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "New Constraints Zone",
+      "minWaypointRelativePos": 1.6500000000000001,
+      "maxWaypointRelativePos": 3.0,
+      "constraints": {
+        "maxVelocity": 2.0,
+        "maxAcceleration": 4.0,
+        "maxAngularVelocity": 360.0,
+        "maxAngularAcceleration": 720.0
+      }
+    }
+  ],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Source",
+  "previewStartingState": {
+    "rotation": 180.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -222,16 +222,19 @@ private void configOperatorBindings() {
       )
     );
 
-    m_operatorController.povUp().onTrue(new ZeroShoulder());
+    m_operatorController.back().onTrue(new ZeroShoulder());
     m_operatorController.povLeft().onTrue(new FeedShot());
     m_operatorController.povRight().onTrue(new SetShoulderPosition(50.0, false));
 
     // Trap score.
     m_operatorController.b().onTrue(new SetElevatorPosition(Elevator.MAX_EXTENSION_INCHES, false, false));
     m_operatorController.b().onFalse(new SetElevatorPosition(0.0, true, false));
 
-    m_operatorController.y().onTrue(new SetShoulderPosition(-9.0, false));  // TODO: test trap shoulder angle.
+    m_operatorController.y().onTrue(new SetShoulderPosition(0.0, false));
     m_operatorController.y().onFalse(new SetShoulderPosition(Shoulder.MIN_ANGLE_DEGREES, false));
+
+    m_operatorController.povUp().onTrue(new SetShoulderPosition(-30.0, false));
+    m_operatorController.povUp().onFalse(new SetShoulderPosition(Shoulder.MIN_ANGLE_DEGREES, false));
   }
 
   // Use this to pass the autonomous command to the main Robot.java class.

src/main/java/frc/robot/commands/ZeroShoulder.java

@@ -8,7 +8,7 @@
 import frc.robot.subsystems.Shoulder;
 
 public class ZeroShoulder extends Command {
-    private static final double ZERO_POSITION_DEGREES = 56.75;
+    private static final double ZERO_POSITION_DEGREES = 58.0;
 
     private final Shoulder m_shoulder;
 

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

@@ -23,7 +23,7 @@ public class Elevator extends SubsystemBase implements Loggable {
   private static Elevator instance;
 
   // Physical properties.
-  public static final double MAX_EXTENSION_INCHES = 15.0;
+  public static final double MAX_EXTENSION_INCHES = 15.5;
   private static final double OUTPUT_SPROCKET_PITCH_RADIUS_INCHES = 1.751 / 2.0;
   private static final double GEAR_RATIO = 9.0 * 4.0;
 

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

@@ -61,7 +61,7 @@ public class Shoulder extends SubsystemBase implements Loggable {
 
     private final InterpolatingDoubleTreeMap m_autoAimFunc;
     private static final double[] AUTO_AIM_DISTS =  {51.6, 69.0, 79.8, 92.6, 99.9, 112.6, 119.3, 126.7, 135.0, 145.3, 158.0, 178.4};
-    private static final double[] AUTO_AIM_ANGLES = {50.0, 43.0, 38.0, 34.0, 33.0, 31.0,  28.5, 29.0,  26.5,  25.5,  24.0,  23.2};
+    private static final double[] AUTO_AIM_ANGLES = {51.0, 45.5, 39.5, 35.0, 33.5, 31.5,  31.0,  30.5,  29.5,  27.5,  26.0,  25.0};
 
     public static Shoulder getInstance() {
         if (instance == null) {