Workshop 10/28

Team3128 · Oct 29, 2023 · 8f42d5c · 8f42d5c
1 parent 6f53758
commit 8f42d5c
Show file tree

Hide file tree

Showing 11 changed files with 262 additions and 29 deletions.
diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path b/src/main/deploy/pathplanner/b_cable_1Cone+1.5Cube+Climb.path
@@ -151,16 +151,16 @@
     },
     {
       "anchorPoint": {
-        "x": 1.834230493113764,
+        "x": 1.82,
         "y": 1.1
       },
       "prevControl": {
-        "x": 3.328467547907319,
-        "y": 0.7980883374961188
+        "x": 3.314237054793555,
+        "y": 0.798088337496119
       },
       "nextControl": {
-        "x": 3.328467547907319,
-        "y": 0.7980883374961188
+        "x": 3.314237054793555,
+        "y": 0.798088337496119
       },
       "holonomicAngle": 180.0,
       "isReversal": true,

diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube.path
@@ -152,15 +152,15 @@
     {
       "anchorPoint": {
         "x": 1.834230493113764,
-        "y": 1.1
+        "y": 1.5
       },
       "prevControl": {
         "x": 3.328467547907319,
-        "y": 0.7980883374961188
+        "y": 1.1980883374961189
       },
       "nextControl": {
         "x": 3.328467547907319,
-        "y": 0.7980883374961188
+        "y": 1.1980883374961189
       },
       "holonomicAngle": 180.0,
       "isReversal": true,
@@ -178,11 +178,11 @@
     },
     {
       "anchorPoint": {
-        "x": 7.080923415319053,
+        "x": 6.0,
         "y": 0.9977988827505433
       },
       "prevControl": {
-        "x": 3.665490370911298,
+        "x": 2.5845669555922446,
         "y": 0.32913967911269143
       },
       "nextControl": null,
@@ -204,7 +204,7 @@
   "isReversed": null,
   "markers": [
     {
-      "position": 2.015234375,
+      "position": 1.4254545454545942,
       "names": [
         "PickupCube"
       ]

diff --git a/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path b/src/main/deploy/pathplanner/b_cable_1Cone+1Cube_V2.path
@@ -0,0 +1,225 @@
+{
+  "waypoints": [
+    {
+      "anchorPoint": {
+        "x": 1.85,
+        "y": 0.5
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.614284574055259,
+        "y": 0.5526832772415535
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [
+          "ScoreConeHigh"
+        ],
+        "executionBehavior": "sequential",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.7075088130204783,
+        "y": 0.6
+      },
+      "prevControl": {
+        "x": 3.0844342575951473,
+        "y": 0.6
+      },
+      "nextControl": {
+        "x": 4.099143568158317,
+        "y": 0.6
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.694433640603393,
+        "y": 0.7
+      },
+      "prevControl": {
+        "x": 5.743020330548507,
+        "y": 0.6049491252537939
+      },
+      "nextControl": {
+        "x": 5.743020330548507,
+        "y": 0.6049491252537939
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.6960733550526776,
+        "y": 0.6
+      },
+      "prevControl": {
+        "x": 4.227107333054388,
+        "y": 0.6
+      },
+      "nextControl": {
+        "x": 3.104340782909878,
+        "y": 0.6
+      },
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 1.834230493113764,
+        "y": 1.1
+      },
+      "prevControl": {
+        "x": 2.472479352948291,
+        "y": 0.6632565618911479
+      },
+      "nextControl": {
+        "x": 2.472479352948291,
+        "y": 0.6632565618911479
+      },
+      "holonomicAngle": 180.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": true,
+      "stopEvent": {
+        "names": [
+          "ScoreCubeHigh"
+        ],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.741815186911401,
+        "y": 0.7089983937457166
+      },
+      "prevControl": {
+        "x": 3.4444932798567436,
+        "y": 0.7089983937457166
+      },
+      "nextControl": {
+        "x": 4.04263511909015,
+        "y": 0.7089983937457166
+      },
+      "holonomicAngle": 0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 6.726469715422112,
+        "y": 1.9897696856769331
+      },
+      "prevControl": {
+        "x": 5.6227746574520605,
+        "y": 0.2234883748754959
+      },
+      "nextControl": {
+        "x": 5.6227746574520605,
+        "y": 0.2234883748754959
+      },
+      "holonomicAngle": 59.0,
+      "isReversal": true,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    },
+    {
+      "anchorPoint": {
+        "x": 3.741815186911401,
+        "y": 0.6289501880002328
+      },
+      "prevControl": {
+        "x": 4.222104421384305,
+        "y": 0.6746920198547948
+      },
+      "nextControl": null,
+      "holonomicAngle": 0.0,
+      "isReversal": false,
+      "velOverride": null,
+      "isLocked": false,
+      "isStopPoint": false,
+      "stopEvent": {
+        "names": [],
+        "executionBehavior": "parallel",
+        "waitBehavior": "none",
+        "waitTime": 0
+      }
+    }
+  ],
+  "maxVelocity": 4.5,
+  "maxAcceleration": 2.3,
+  "isReversed": null,
+  "markers": [
+    {
+      "position": 1.1272727272727252,
+      "names": [
+        "PickupCube"
+      ]
+    },
+    {
+      "position": 2.5090909090909514,
+      "names": [
+        "Neutral"
+      ]
+    },
+    {
+      "position": 5.141818181818192,
+      "names": [
+        "PickupCube"
+      ]
+    }
+  ]
+}
diff --git a/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path b/src/main/deploy/pathplanner/b_mid_1Cone+0.5Cube+Climb.path
@@ -77,15 +77,15 @@
     {
       "anchorPoint": {
         "x": 6.707654896212571,
-        "y": 1.9
+        "y": 1.5
       },
       "prevControl": {
         "x": 6.036038306451613,
-        "y": 2.1836771159653425
+        "y": 1.7836771159653428
       },
       "nextControl": {
         "x": 6.036038306451613,
-        "y": 2.1836771159653425
+        "y": 1.7836771159653428
       },
       "holonomicAngle": 0,
       "isReversal": true,

diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java
@@ -73,7 +73,7 @@ public static class AutoConstants {
         public static final double ANGLE_THRESHOLD = 8; //7, 9
         public static final double VELOCITY_THRESHOLD = 4; //6, 3
         public static final double RAMP_THRESHOLD = 9; //8, 10
-        public static final double DRIVE_SPEED = Units.inchesToMeters(40); //30, 40
+        public static final double DRIVE_SPEED = Units.inchesToMeters(20); //30, 40
 
     }
 
@@ -213,6 +213,12 @@ public static class VisionConstants {
                                                         new Transform2d(
                                                             new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(-11.7)), 
                                                                 Rotation2d.fromDegrees(180)));
+
+        public static final Camera BACK_LEFT = new Camera("BACK_LEFT", true, 0, 0, 0, 
+                                                        new Transform2d(
+                                                            new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(18.89)), 
+                                                                Rotation2d.fromDegrees(180)));
+
         public static final double POSE_THRESH = 100;
 
         public static final Matrix<N3,N1> SVR_STATE_STD = VecBuilder.fill(0.1,0.1,Units.degreesToRadians(3));

diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java
@@ -26,7 +26,7 @@ private void initAutoSelector() {
         final String[] autoStrings = new String[] {
                                             //Blue Autos
                                                 //Cable
-                                                "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1.5Cube+Climb",
+                                                "cable_1Cone+1Cube","cable_1Cone+2Cube", "cable_1Cone+1Cube_V2", "cable_1Cone+1.5Cube+Climb",
                                                 //Mid
                                                 "mid_1Cone+Climb","mid_1Cone+0.5Cube+Climb", "mid_1Cone+1Cube+Climb",
                                                 //Hp

diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java
@@ -45,30 +45,30 @@ public static void initTrajectories() {
         final String[] trajectoryNames = {
                                         //Blue Autos
                                             //Cable
-                                            "b_cable_1Cone+1Cube","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb",
+                                            "b_cable_1Cone+1Cube", "b_cable_1Cone+1Cube_V2","b_cable_1Cone+2Cube", "b_cable_1Cone+2Cube+Climb",
                                             //Mid
-                                            "b_mid_1Cone+Climb","b_mid_1Cone+1Cube+Climb",
+                                            "b_mid_1Cone+Climb","b_mid_1Cone+0.5Cube+Climb", "b_mid_1Cone+1Cube+Climb",
                                             //Hp
                                             "b_hp_1Cone+1Cube","b_cable_1Cone+2Cube",
 
                                         //Red Autos
                                             //Cable
                                             "r_cable_1Cone+1Cube","r_cable_1Cone+2Cube",
                                             //Mid
-                                            "r_mid_1Cone+Climb","r_mid_1Cone+1Cube+Climb",
+                                            "r_mid_1Cone+Climb","r_mid_1Cone+0.5Cube+Climb","r_mid_1Cone+1Cube+Climb",
                                             //Hp
                                             "r_hp_1Cone+1Cube","r_cable_1Cone+2Cube",
                                         };
 
-        CommandEventMap.put("ScoreConeHigh", score(Position.HIGH_CONE, true));
+        CommandEventMap.put("ScoreConeHigh", sequence(score(Position.HIGH_CONE, true)));
 
         CommandEventMap.put("ScoreCubeHigh", score(Position.HIGH_CUBE, true));
 
         CommandEventMap.put("ScoreLow", score(Position.LOW, true));
 
         CommandEventMap.put("PickupCube", pickup(Position.GROUND_CUBE, true));
 
-        CommandEventMap.put("Neutral", retract(Position.NEUTRAL));
+        CommandEventMap.put("Neutral", sequence(retract(Position.NEUTRAL)));
 
         CommandEventMap.put("Balance", new ScheduleCommand(new CmdAutoBalance(true)));
 
@@ -110,6 +110,7 @@ public static CommandBase resetAuto() {
             resetLeds(),
             resetGyro(DriverStation.getAlliance() == Alliance.Red ? 0 : 180),
             runOnce(()-> Manipulator.getInstance().set(-0.4), Manipulator.getInstance()),
+            runOnce(()-> Manipulator.getInstance().isCone = true),
             resetAll(),
             retract(Position.NEUTRAL)
         );

diff --git a/src/main/java/frc/team3128/commands/CmdAutoBalance.java b/src/main/java/frc/team3128/commands/CmdAutoBalance.java
@@ -49,12 +49,12 @@ public void execute() {
         // final double angleVelocity = (advAngle - prevAngle) / 0.02;
         final double angleVelocity = getAngleVelocity(advAngle);
 
-        if (advAngle > RAMP_THRESHOLD) onRamp = true;
+        if (Math.abs(advAngle) > RAMP_THRESHOLD) onRamp = true;
 
         if (Math.abs(advAngle) < ANGLE_THRESHOLD && onRamp) {
             swerve.xlock();
             if (direction == 1) {
-                Manipulator.getInstance().set(-1);
+                // Manipulator.getInstance().set(-1);
             }
             return;
         }