diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 4c38993..e286729 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -15,9 +15,9 @@ "RealAutons", "TestAutos" ], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxVel": 2.4, + "defaultMaxAccel": 1.8, + "defaultMaxAngVel": 300.0, + "defaultMaxAngAccel": 400.0, "maxModuleSpeed": 4.5 } diff --git a/src/main/deploy/pathplanner/paths/1Meter.path b/src/main/deploy/pathplanner/paths/1Meter.path index bb8e58c..a194286 100644 --- a/src/main/deploy/pathplanner/paths/1Meter.path +++ b/src/main/deploy/pathplanner/paths/1Meter.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/1Pickup.path b/src/main/deploy/pathplanner/paths/1Pickup.path index fa5c8b1..7968443 100644 --- a/src/main/deploy/pathplanner/paths/1Pickup.path +++ b/src/main/deploy/pathplanner/paths/1Pickup.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/1Reverse.path b/src/main/deploy/pathplanner/paths/1Reverse.path index ed01ed0..837365d 100644 --- a/src/main/deploy/pathplanner/paths/1Reverse.path +++ b/src/main/deploy/pathplanner/paths/1Reverse.path @@ -56,10 +56,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/1toF1.path b/src/main/deploy/pathplanner/paths/1toF1.path index 3e01556..df9210b 100644 --- a/src/main/deploy/pathplanner/paths/1toF1.path +++ b/src/main/deploy/pathplanner/paths/1toF1.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/2Pickup.path b/src/main/deploy/pathplanner/paths/2Pickup.path index 3cbbb81..e8a37fe 100644 --- a/src/main/deploy/pathplanner/paths/2Pickup.path +++ b/src/main/deploy/pathplanner/paths/2Pickup.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/2Reverse.path b/src/main/deploy/pathplanner/paths/2Reverse.path index 5804cf2..3d5162f 100644 --- a/src/main/deploy/pathplanner/paths/2Reverse.path +++ b/src/main/deploy/pathplanner/paths/2Reverse.path @@ -68,10 +68,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3Pickup.path b/src/main/deploy/pathplanner/paths/3Pickup.path index 8978490..706eda2 100644 --- a/src/main/deploy/pathplanner/paths/3Pickup.path +++ b/src/main/deploy/pathplanner/paths/3Pickup.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/3Reverse.path b/src/main/deploy/pathplanner/paths/3Reverse.path index 67c226a..108ec9b 100644 --- a/src/main/deploy/pathplanner/paths/3Reverse.path +++ b/src/main/deploy/pathplanner/paths/3Reverse.path @@ -68,10 +68,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/ALFirst.path b/src/main/deploy/pathplanner/paths/ALFirst.path index e3ac0e8..97f68f6 100644 --- a/src/main/deploy/pathplanner/paths/ALFirst.path +++ b/src/main/deploy/pathplanner/paths/ALFirst.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/AMFirst.path b/src/main/deploy/pathplanner/paths/AMFirst.path index 529367d..943734a 100644 --- a/src/main/deploy/pathplanner/paths/AMFirst.path +++ b/src/main/deploy/pathplanner/paths/AMFirst.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/ARFirst.path b/src/main/deploy/pathplanner/paths/ARFirst.path index 1f035c7..934c927 100644 --- a/src/main/deploy/pathplanner/paths/ARFirst.path +++ b/src/main/deploy/pathplanner/paths/ARFirst.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/ARMidway.path b/src/main/deploy/pathplanner/paths/ARMidway.path index 9f2d23e..e1e795b 100644 --- a/src/main/deploy/pathplanner/paths/ARMidway.path +++ b/src/main/deploy/pathplanner/paths/ARMidway.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/ARSecond.path b/src/main/deploy/pathplanner/paths/ARSecond.path index 2f32376..28ad9d0 100644 --- a/src/main/deploy/pathplanner/paths/ARSecond.path +++ b/src/main/deploy/pathplanner/paths/ARSecond.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/BlueShoot1.path b/src/main/deploy/pathplanner/paths/BlueShoot1.path index 2959a92..40dee40 100644 --- a/src/main/deploy/pathplanner/paths/BlueShoot1.path +++ b/src/main/deploy/pathplanner/paths/BlueShoot1.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/LFirst.path b/src/main/deploy/pathplanner/paths/LFirst.path index ed32f00..af10b45 100644 --- a/src/main/deploy/pathplanner/paths/LFirst.path +++ b/src/main/deploy/pathplanner/paths/LFirst.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MFirst.path b/src/main/deploy/pathplanner/paths/MFirst.path index 872e000..273c3aa 100644 --- a/src/main/deploy/pathplanner/paths/MFirst.path +++ b/src/main/deploy/pathplanner/paths/MFirst.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.5, diff --git a/src/main/deploy/pathplanner/paths/Measuring.path b/src/main/deploy/pathplanner/paths/Measuring.path index 06b8b23..55a2bfd 100644 --- a/src/main/deploy/pathplanner/paths/Measuring.path +++ b/src/main/deploy/pathplanner/paths/Measuring.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/MoveForward.path b/src/main/deploy/pathplanner/paths/MoveForward.path index 9549582..00ea21f 100644 --- a/src/main/deploy/pathplanner/paths/MoveForward.path +++ b/src/main/deploy/pathplanner/paths/MoveForward.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Point1-2.path b/src/main/deploy/pathplanner/paths/Point1-2.path index c06495e..68023fb 100644 --- a/src/main/deploy/pathplanner/paths/Point1-2.path +++ b/src/main/deploy/pathplanner/paths/Point1-2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Point1-3.path b/src/main/deploy/pathplanner/paths/Point1-3.path index 4805308..3b2eec4 100644 --- a/src/main/deploy/pathplanner/paths/Point1-3.path +++ b/src/main/deploy/pathplanner/paths/Point1-3.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Point2-1.path b/src/main/deploy/pathplanner/paths/Point2-1.path index 27bfc23..f4c61bc 100644 --- a/src/main/deploy/pathplanner/paths/Point2-1.path +++ b/src/main/deploy/pathplanner/paths/Point2-1.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Point2-3.path b/src/main/deploy/pathplanner/paths/Point2-3.path index 363d18a..ee01ff9 100644 --- a/src/main/deploy/pathplanner/paths/Point2-3.path +++ b/src/main/deploy/pathplanner/paths/Point2-3.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Point3-1.path b/src/main/deploy/pathplanner/paths/Point3-1.path index 7c3b1c5..3bebf72 100644 --- a/src/main/deploy/pathplanner/paths/Point3-1.path +++ b/src/main/deploy/pathplanner/paths/Point3-1.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Point3-2.path b/src/main/deploy/pathplanner/paths/Point3-2.path index 42c78d7..88f792a 100644 --- a/src/main/deploy/pathplanner/paths/Point3-2.path +++ b/src/main/deploy/pathplanner/paths/Point3-2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/PointF1-F2.path b/src/main/deploy/pathplanner/paths/PointF1-F2.path index 7040b6d..d946c6c 100644 --- a/src/main/deploy/pathplanner/paths/PointF1-F2.path +++ b/src/main/deploy/pathplanner/paths/PointF1-F2.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/PointF2-F1.path b/src/main/deploy/pathplanner/paths/PointF2-F1.path index c0147c8..d4d0d97 100644 --- a/src/main/deploy/pathplanner/paths/PointF2-F1.path +++ b/src/main/deploy/pathplanner/paths/PointF2-F1.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/RFirst.path b/src/main/deploy/pathplanner/paths/RFirst.path index 325c9f2..b68f56a 100644 --- a/src/main/deploy/pathplanner/paths/RFirst.path +++ b/src/main/deploy/pathplanner/paths/RFirst.path @@ -62,10 +62,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/ROut.path b/src/main/deploy/pathplanner/paths/ROut.path index 81dbc8d..ffe45c5 100644 --- a/src/main/deploy/pathplanner/paths/ROut.path +++ b/src/main/deploy/pathplanner/paths/ROut.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0.0, diff --git a/src/main/deploy/pathplanner/paths/Rotate180.path b/src/main/deploy/pathplanner/paths/Rotate180.path index 9400381..e14f4bc 100644 --- a/src/main/deploy/pathplanner/paths/Rotate180.path +++ b/src/main/deploy/pathplanner/paths/Rotate180.path @@ -44,10 +44,10 @@ ], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpinTest.path b/src/main/deploy/pathplanner/paths/SpinTest.path index 3e896cf..dcc6f48 100644 --- a/src/main/deploy/pathplanner/paths/SpinTest.path +++ b/src/main/deploy/pathplanner/paths/SpinTest.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/TestFromBack.path b/src/main/deploy/pathplanner/paths/TestFromBack.path index f240749..6fe8c8d 100644 --- a/src/main/deploy/pathplanner/paths/TestFromBack.path +++ b/src/main/deploy/pathplanner/paths/TestFromBack.path @@ -43,10 +43,10 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path b/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path index 0c54388..6f1a462 100644 --- a/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path +++ b/src/main/deploy/pathplanner/paths/ToSpeakerTEst.path @@ -32,10 +32,10 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 + "maxVelocity": 2.4, + "maxAcceleration": 1.8, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 400.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b03b0b3..6e63de0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -108,8 +108,8 @@ public static final class IntakeConstants { public static final double kv = 5; public static double percentPower = 0.8; - public static final int conveyorSensor = 0; - public static final int shooterSensor = 1; + public static final int conveyorSensor = 1; + public static final int shooterSensor = 0; } public static final class ArmConstants { @@ -120,13 +120,13 @@ public static final class ArmConstants { public static final double armConversion = 0.05; - public static final double kMaxOutput = 0.33; + public static final double kMaxOutput = 0.3; // // Arm PID constants public static final double kP = 0.65; public static final double kI = 0.0; - public static final double kD = 0.035 * 1.10; - public static final double kWeightBasedFF = 0.025 * 1.3 * 1.15 * 0.85; + public static final double kD = 0.035; + public static final double kWeightBasedFF = 0.025 * 1.3 * 1.4; // // Arm PID constants // public static final double kP = 0; @@ -141,7 +141,7 @@ public static final class ArmConstants { public static final double minimumAngle = 0; public static final double maximumAngle = Math.PI; - public static final double uprightAngle = 1.753; // (for gravity calculations for PID) + public static final double uprightAngle = 1.734; // (for gravity calculations for PID) // do not use // public static final double kFF = 0.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bdd57b8..d86095f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -310,9 +310,7 @@ private void configureButtonBindings() { .start() .whileTrue(Commands.startEnd(() -> sIntake.runFull(), sIntake::stop, sIntake)); - driverController - .rightBumper() - .whileTrue(Commands.run(() -> sDrive.slowMode())); + driverController.rightBumper().whileTrue(Commands.run(() -> sDrive.slowMode())); // ! TEST < driverController.a().whileTrue(sDrive.getZeroAuton()); diff --git a/src/main/java/frc/robot/commands/PreRunShooter.java b/src/main/java/frc/robot/commands/PreRunShooter.java index 68bd7c5..8810eb5 100644 --- a/src/main/java/frc/robot/commands/PreRunShooter.java +++ b/src/main/java/frc/robot/commands/PreRunShooter.java @@ -49,7 +49,7 @@ public void execute() { if (intake.getConveyerProximity()) { flywheel.runVelocity(Constants.FlywheelConstants.slowShootingVelocity); } else { - flywheel.runVelocity(100); + flywheel.runVelocity(300); } } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java index 7deb3f4..1b2d1f9 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSparkMax.java @@ -41,8 +41,7 @@ public class ArmIOSparkMax implements ArmIO { private final PIDController mathPid; - private double targetAngle = - Constants.ArmConstants.uprightAngle; // 2.2// Radians, just a default value + private double targetAngle = 0.747; // 2.2// Radians, just a default value private double lastEncoderReading = 0.4; diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index bad756c..a599e9e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -89,7 +89,7 @@ public Drive( () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, new HolonomicPathFollowerConfig( - new PIDConstants(8.0, 0.0, 0.0), + new PIDConstants(6.0, 0.0, 0.3), new PIDConstants(10.0, 0.0, 0.0), CurrentMaxLinearSpeed, DRIVE_BASE_RADIUS, diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java index 064e012..dfc223e 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -59,6 +59,7 @@ public void runVolts(double volts) { /** Run closed loop at the specified velocity. */ public void runVelocity(double velocityRPS) { + velocityRPS = -velocityRPS; io.setVelocity(velocityRPS, ffModel.calculate(velocityRPS)); diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index dc94f07..6a3fb31 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -72,6 +72,7 @@ public void runVolts(double volts) { /** Run closed loop at the percent speed. */ public void runPercentSpeed(double percentSpeed) { + percentSpeed = -percentSpeed; io.setPercentSpeed(percentSpeed); // Log flywheel setpoint