CshCyberhawks · PY44N · Mar 23, 2025 · Mar 10, 2025 · Mar 10, 2025 · Mar 11, 2025
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.3.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
     id 'org.jetbrains.kotlin.jvm'
 }
 

diff --git a/simgui-ds.json b/simgui-ds.json
@@ -90,6 +90,8 @@
     }
   ],
   "robotJoysticks": [
+    {},
+    {},
     {},
     {},
     {

diff --git a/src/main/deploy/pathplanner/autos/3 L4 Left.auto b/src/main/deploy/pathplanner/autos/3 L4 Left.auto
@@ -25,7 +25,7 @@
       ]
     }
   },
-  "resetOdom": true,
+  "resetOdom": false,
   "folder": null,
   "choreoAuto": false
 }
diff --git a/src/main/deploy/pathplanner/autos/Test.auto b/src/main/deploy/pathplanner/autos/Test.auto
@@ -0,0 +1,19 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/1.1.path b/src/main/deploy/pathplanner/paths/1.1.path
@@ -3,43 +3,43 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 8.127663934426229,
-        "y": 6.152817622950819
+        "x": 7.010250000000001,
+        "y": 6.12125
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.185209356966076,
-        "y": 6.018613752832711
+        "x": 6.067795422539848,
+        "y": 5.987046129881892
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.93,
-        "y": 5.2
+        "x": 4.01,
+        "y": 5.18
       },
       "prevControl": {
-        "x": 5.390327868852459,
-        "y": 5.74064549180328
+        "x": 3.255495797837585,
+        "y": 6.530247651884371
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "Reef C Right"
+      "linkedName": "Reef B Left"
     }
   ],
   "rotationTargets": [
     {
       "waypointRelativePos": 0.6593362654938016,
-      "rotationDegrees": -119.99999999999999
+      "rotationDegrees": -59.99999999999999
     }
   ],
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [
     {
       "name": "PrepL4",
-      "waypointRelativePos": 0.5053022269353092,
+      "waypointRelativePos": 0.05,
       "endWaypointRelativePos": null,
       "command": {
         "type": "named",
@@ -50,22 +50,22 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -119.99999999999999
+    "rotation": -59.99999999999999
   },
   "reversed": false,
   "folder": "1 - 3 L4 Left",
   "idealStartingState": {
     "velocity": 0,
     "rotation": 180.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/1.2.path b/src/main/deploy/pathplanner/paths/1.2.path
@@ -50,8 +50,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

diff --git a/src/main/deploy/pathplanner/paths/1.3.path b/src/main/deploy/pathplanner/paths/1.3.path
@@ -50,8 +50,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

diff --git a/src/main/deploy/pathplanner/paths/1.4.path b/src/main/deploy/pathplanner/paths/1.4.path
@@ -50,8 +50,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

diff --git a/src/main/deploy/pathplanner/paths/1.5.path b/src/main/deploy/pathplanner/paths/1.5.path
@@ -50,8 +50,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

diff --git a/src/main/deploy/pathplanner/paths/1.6.path b/src/main/deploy/pathplanner/paths/1.6.path
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
@@ -3,52 +3,57 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.29,
-        "y": 4.27
+        "x": 1.6377996575342464,
+        "y": 7.428317636986301
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.29,
-        "y": 4.27
+        "x": 2.211376093885291,
+        "y": 6.609165592697311
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 1.1867827868852459,
+        "y": 0.9741290983606552
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 0.8870901639344262,
+        "y": 0.4226946721311481
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": null
     }
   ],
-  "rotationTargets": [],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 0.0
+    }
+  ],
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 55.69221215528403
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.04083126241170403
+    "rotation": -51.73601121411935
   },
   "useDefaultConstraints": true
 }
diff --git a/src/main/deploy/pathplanner/paths/Reef Positions.path b/src/main/deploy/pathplanner/paths/Reef Positions.path
@@ -48,16 +48,16 @@
     },
     {
       "anchor": {
-        "x": 4.05,
-        "y": 5.2
+        "x": 4.01,
+        "y": 5.18
       },
       "prevControl": {
-        "x": 3.8157313155445705,
-        "y": 5.605765264099125
+        "x": 3.7757313155445704,
+        "y": 5.585765264099124
       },
       "nextControl": {
-        "x": 4.284268684455429,
-        "y": 4.794234735900876
+        "x": 4.244268684455429,
+        "y": 4.774234735900875
       },
       "isLocked": false,
       "linkedName": "Reef B Left"
@@ -193,8 +193,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 4.0,
+    "maxVelocity": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
@@ -6,8 +6,8 @@
     "1 - 3 L4 Left"
   ],
   "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 4.0,
+  "defaultMaxVel": 1.5,
+  "defaultMaxAccel": 2.0,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,

diff --git a/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt b/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt
@@ -0,0 +1,40 @@
+package cshcyberhawks.lib.commands
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Commands
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+
+//class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : Command() {
+//    private var i = -1
+//    private var currentCommand = Commands.runOnce({})
+//
+//    override fun initialize() {
+//        i = 0
+//        currentCommand = callback(i)
+//        currentCommand.schedule()
+//    }
+//
+//    override fun execute() {
+//        SmartDashboard.putNumber("For Command i", i.toDouble())
+//        SmartDashboard.putBoolean("For Command current finished", currentCommand.isFinished)
+//        if (currentCommand.isFinished) {
+//            println("current command for is finished")
+//        }
+////        println("current command for finished: " + currentCommand.isFinished)
+//        if (currentCommand.isFinished && i < max) {
+//            i++
+//            currentCommand = callback(i)
+//            currentCommand.schedule()
+//        }
+//    }
+//
+//    override fun isFinished(): Boolean {
+//        SmartDashboard.putBoolean("For command finished", i >= max && currentCommand.isFinished)
+//        return i >= max && currentCommand.isFinished
+//    }
+//}
+
+class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : SequentialCommandGroup(
+    *(0..<max).map { callback(it) }.toTypedArray()
+)
-Original file line number
+Diff line change
@@ Expand Up / @@ -90,6 +90,8 @@ @@
         }
       ],
       "robotJoysticks": [
+        {},
+        {},
         {},
         {},
         {
@@ Expand Down @@