help...

RoboLoCo-5338 · Oct 12, 2024 · 1d5e16e · 1d5e16e
1 parent c1ed3a1
commit 1d5e16e
Show file tree

Hide file tree

Showing 7 changed files with 104 additions and 25 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -1,11 +1,13 @@
 {
-  "robotWidth": 0.9,
-  "robotLength": 0.9,
+  "robotWidth": 0.21,
+  "robotLength": 0.21,
   "holonomicMode": false,
-  "pathFolders": [],
+  "pathFolders": [
+    "Test1 Paths"
+  ],
   "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxVel": 1.0,
+  "defaultMaxAccel": 0.5,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "maxModuleSpeed": 4.5

diff --git a/...in/deploy/pathplanner/autos/New Auto.auto → src/main/deploy/pathplanner/autos/Test1.auto b/...in/deploy/pathplanner/autos/New Auto.auto → src/main/deploy/pathplanner/autos/Test1.auto
@@ -2,10 +2,10 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 2.0,
-      "y": 7.0
+      "x": 1.37,
+      "y": 5.550936522700434
     },
-    "rotation": 0
+    "rotation": 180.0
   },
   "command": {
     "type": "sequential",
@@ -14,7 +14,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Example Path"
+            "pathName": "Path1"
           }
         }
       ]

diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
@@ -32,8 +32,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.25,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },

diff --git a/src/main/deploy/pathplanner/paths/Path1.path b/src/main/deploy/pathplanner/paths/Path1.path
@@ -0,0 +1,68 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.37,
+        "y": 5.550936522700434
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.32,
+        "y": 5.550936522700434
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.82,
+        "y": 5.8
+      },
+      "prevControl": {
+        "x": 1.2426515097537039,
+        "y": 5.8
+      },
+      "nextControl": {
+        "x": 2.397348490246295,
+        "y": 5.8
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.894982565999367,
+        "y": 5.550936522700434
+      },
+      "prevControl": {
+        "x": 2.274197759313787,
+        "y": 5.550936522700434
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.25,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Test1 Paths",
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -76,6 +76,7 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
+    return getPathPlannerCommand();
     /*return new FunctionalCommand(
       () -> {
         RobotContainer.m_xrpDrivetrain.tankDrive(0, 0);
@@ -88,16 +89,16 @@ public Command getAutonomousCommand() {
     );
     */
 
-    return new SequentialCommandGroup(
-      DriveCommands.tankDriveBackwards(2),
-      ServoCommands.servoPresetCommand(1),
-      //idk if i have to add new to the above line
-      DriveCommands.tankTurnDegrees(1, 360),
-      new ParallelCommandGroup(
-        DriveCommands.tankDriveDistance(3),
-        ServoCommands.servoSetAngleCommand(0)
-      )
-    );
+    // return new SequentialCommandGroup(
+    //   DriveCommands.tankDriveBackwards(2),
+    //   ServoCommands.servoPresetCommand(1),
+    //   //idk if i have to add new to the above line
+    //   DriveCommands.tankTurnDegrees(1, 360),
+    //   new ParallelCommandGroup(
+    //     DriveCommands.tankDriveDistance(3),
+    //     ServoCommands.servoSetAngleCommand(0)
+    //   )
+    // );
     //return DriveCommands.tankDriveDistance(5);
     /*return new SequentialCommandGroup(
       DriveCommands.tankDriveDistance(5),
@@ -106,7 +107,9 @@ public Command getAutonomousCommand() {
       */
   }
 
-  public Command getPathPlannerCommand() {
-    return new PathPlannerAuto("New Auto");
-  }
+   public Command getPathPlannerCommand() {
+     return new PathPlannerAuto("Test1");
+   }
+
+
 }
diff --git a/src/main/java/frc/robot/subsystems/XRPDrivetrain.java b/src/main/java/frc/robot/subsystems/XRPDrivetrain.java
@@ -76,6 +76,12 @@ public void arcadeDrive(ChassisSpeeds speed){
     arcadeDrive(Math.hypot(speed.vxMetersPerSecond, speed.vyMetersPerSecond),speed.omegaRadiansPerSecond);
   }
 
+  public double getAverageTurningDistance() {
+    double leftDistance = Math.abs(getLeftDistanceInch());
+    double rightDistance = Math.abs(getRightDistanceInch());
+    return (leftDistance + rightDistance) / 2.0;
+  }
+
   public ChassisSpeeds getCurrentSpeeds(){
     return m_speed;
   }