Merge pull request #9 from RAR1741/LimelightPrototype

Merge "Limelight prototype"
RAR1741 · Apr 17, 2023 · 10044b3 · 10044b3
2 parents 34793e4 + 50c54a6
commit 10044b3
Show file tree

Hide file tree

Showing 13 changed files with 1,036 additions and 80 deletions.
diff --git a/src/main/deploy/pathplanner/RightFar2Piece1-B.path b/src/main/deploy/pathplanner/RightFar2Piece1-B.path
@@ -24,12 +24,12 @@
     },
     {
       "anchorPoint": {
-        "x": 6.508350266700534,
-        "y": 4.41
+        "x": 6.64,
+        "y": 4.58
       },
       "prevControl": {
-        "x": 5.593918504438676,
-        "y": 4.41
+        "x": 5.725568237738142,
+        "y": 4.58
       },
       "nextControl": null,
       "holonomicAngle": 0.0,

diff --git a/src/main/deploy/pathplanner/RightFar2Piece1.path b/src/main/deploy/pathplanner/RightFar2Piece1.path
@@ -24,12 +24,12 @@
     },
     {
       "anchorPoint": {
-        "x": 9.9,
-        "y": 4.41
+        "x": 9.900472149902633,
+        "y": 4.579773357463382
       },
       "prevControl": {
-        "x": 10.814431762261858,
-        "y": 4.41
+        "x": 10.81490391216449,
+        "y": 4.579773357463382
       },
       "nextControl": null,
       "holonomicAngle": 180.0,
@@ -46,7 +46,7 @@
     }
   ],
   "maxVelocity": 2.0,
-  "maxAcceleration": 1.0,
+  "maxAcceleration": 1.5,
   "isReversed": null,
   "markers": []
 }
diff --git a/src/main/deploy/pathplanner/RightFar2Piece2.path b/src/main/deploy/pathplanner/RightFar2Piece2.path
@@ -3,12 +3,12 @@
     {
       "anchorPoint": {
         "x": 9.9,
-        "y": 4.51
+        "y": 4.58
       },
       "prevControl": null,
       "nextControl": {
         "x": 10.969346556449857,
-        "y": 4.51
+        "y": 4.58
       },
       "holonomicAngle": 180.0,
       "isReversal": false,
@@ -24,12 +24,37 @@
     },
     {
       "anchorPoint": {
-        "x": 14.834088058801118,
-        "y": 5.049765799184376
+        "x": 14.141015922656846,
+        "y": 4.427323002215449
       },
       "prevControl": {
-        "x": 14.742872992690431,
-        "y": 4.263738958033471
+        "x": 14.140754571509254,
+        "y": 4.603006977215762
+      },
+      "nextControl": {
+        "x": 14.14182775414162,
+        "y": 3.881598280557672
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 14.661863808102616,
+        "y": 3.664317382454209
+      },
+      "prevControl": {
+        "x": 14.235570566280021,
+        "y": 3.687559549007986
       },
       "nextControl": null,
       "holonomicAngle": 180.0,
@@ -46,7 +71,7 @@
     }
   ],
   "maxVelocity": 2.0,
-  "maxAcceleration": 1.0,
+  "maxAcceleration": 1.5,
   "isReversed": null,
   "markers": []
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -104,7 +104,7 @@ public static class Shoulder {
       public static final double k_minAngle = Units.degreesToRadians(-45.0);
       public static final double k_maxAngle = Units.degreesToRadians(225.0);
 
-      public static final double k_offset = 0.423139;
+      public static final double k_offset = 0.425479;
     }
 
     public static class Elbow {
@@ -115,7 +115,7 @@ public static class Elbow {
       public static final double k_minAngle = Units.degreesToRadians(-360.0);
       public static final double k_maxAngle = Units.degreesToRadians(360.0);
 
-      public static final double k_offset = 0.501521;
+      public static final double k_offset = 0.492567;
     }
 
     public static class Wrist {
@@ -126,19 +126,19 @@ public static class Wrist {
       public static final double k_minAngle = Units.degreesToRadians(-360.0);
       public static final double k_maxAngle = Units.degreesToRadians(360.0);
 
-      public static final double k_offset = 0.338260;
+      public static final double k_offset = 0.195004;
     }
 
     public static enum Preset {
       HOME(new ArmPose(0.0, Constants.Arm.k_homeHeight, null)),
       SCORE_MID_CUBE(new ArmPose(-47.6, 39.66, null)),
       SCORE_HIGH_CUBE(new ArmPose(-59.1, 52.9, null)),
       SCORE_MID_CONE(new ArmPose(-45.58, 47.87, null)),
-      SCORE_HIGH_CONE(new ArmPose(-57.9, 60.3, null)),
+      SCORE_HIGH_CONE(new ArmPose(-53.457489, 62.608301, null)),
       SINGLE_SUBSTATION(new ArmPose(0.0, 0.0, null)),
-      DOUBLE_SUBSTATION(new ArmPose(29.350695, 48.393162, null)),
-      FLOOR_PICKUP(new ArmPose(24.129862, 0.405395, null)),
-      FLOOR_SCORE(new ArmPose(-25.14, 3.39, null));
+      DOUBLE_SUBSTATION(new ArmPose(30.844269, 47.050315, null)),
+      FLOOR_PICKUP(new ArmPose(22.529862, 0.005395, null)),
+      FLOOR_SCORE(new ArmPose(-26.14, 4.39, null));
 
       private ArmPose m_armPose;