slower arm peak, alt auto path

FRC-4509-MechBulls · Mar 20, 2024 · 726e47d · 726e47d
1 parent ca42ee5
commit 726e47d
Show file tree

Hide file tree

Showing 4 changed files with 280 additions and 3 deletions.
diff --git a/src/main/deploy/pathplanner/autos/CS-PL-01-02.auto b/src/main/deploy/pathplanner/autos/CS-PL-01-02.auto
@@ -0,0 +1,274 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.5148426719035912,
+      "y": 5.551187918118234
+    },
+    "rotation": 180.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "setObjectiveSpeaker"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shotIntentionAgainstSpeaker"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "enableUseFedPoseIntention"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "readyToShootMode"
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 3.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "waitForArmError"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shootMode"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.4
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "enableAimAssist"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeMode"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "speakerCenterToNote01"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "holdMode"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "note01ToSpeakerCenter"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.3
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "readyToShootMode"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "waitForArmError"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shootMode"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.4
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "disableAimAssist"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "intakeMode"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "speakerCenterToNote02"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.1
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "enableAimAssist"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "holdMode"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "disableAimAssist"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "note02ToSpeakerCenter"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 0.4
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "readyToShootMode"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "race",
+          "data": {
+            "commands": [
+              {
+                "type": "wait",
+                "data": {
+                  "waitTime": 5.0
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "waitForArmError"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shootMode"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -113,7 +113,7 @@ public static class EFCConstants{
 
   public static class ArmConstants{
 
-    public static final double armMaxPower = 0.6; //0.6
+    public static final double armMaxPower = 0.3; //0.6
 
     public static final double armkP = 0.9;
     public static final double armkI = 0.4;
@@ -125,7 +125,7 @@ public static class ArmConstants{
     public static final double armkDBrakeless = 0.0;
 
     public static final double brakeMinAngle = Units.degreesToRadians(-5);
-    public static final double brakeMaxAngle = Units.degreesToRadians(50);
+    public static final double brakeMaxAngle = Units.degreesToRadians(130);
 
 
     public static final double armIZone = Units.degreesToRadians(10);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -158,6 +158,7 @@ private void createAutos(){
     autoChooser.addOption("AS-PL-00",new PathPlannerAuto("AS-PL-00"));
     autoChooser.addOption("SS-PL",new PathPlannerAuto("SS-PL"));
     autoChooser.addOption("wipe",new PathPlannerAuto("wipe"));
+    autoChooser.addOption("CS-PL-01-02",new PathPlannerAuto("CS-PL-01-02"));
 
 
 

diff --git a/src/main/java/frc/robot/subsystems/drive/SwerveModule.java b/src/main/java/frc/robot/subsystems/drive/SwerveModule.java
@@ -59,7 +59,7 @@ public SwerveModule(int driveMotorID, int turningMotorID, boolean driveMotorReve
 
         driveMotor.configNeutralDeadband(driveNeutralDeadband);
 
-        driveMotor.setNeutralMode(NeutralMode.Coast);
+        driveMotor.setNeutralMode(NeutralMode.Brake);
         driveMotor.setInverted(driveMotorReversed);
 
         driveMotor.configPeakOutputForward(maxDrivePower,1000);
@@ -79,6 +79,8 @@ public SwerveModule(int driveMotorID, int turningMotorID, boolean driveMotorReve
         turningMotor.setInverted(true);
 
 
+        driveMotor
+
 
         StatorCurrentLimitConfiguration turningStatorConfig = new StatorCurrentLimitConfiguration(false,40,40,0);
         SupplyCurrentLimitConfiguration turningSupplyConfig = new SupplyCurrentLimitConfiguration(true,20,30,50);