diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 6d89aa88..bc3a84c9 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,7 +6,7 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 2.0, + "defaultMaxVel": 15.0, "defaultMaxAccel": 5.0, "defaultMaxAngVel": 180.0, "defaultMaxAngAccel": 180.0, diff --git a/Dashboards/MILFORD_DASH.html b/Dashboards/MILFORD_DASH.html new file mode 100644 index 00000000..cfd6d8b2 --- /dev/null +++ b/Dashboards/MILFORD_DASH.html @@ -0,0 +1,67 @@ + \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Amp.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Amp.auto new file mode 100644 index 00000000..9df65889 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Amp.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6946944060158917, + "y": 6.727333169630711 + }, + "rotation": -120.0013184955057 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Amp 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Amp 2" + } + }, + { + "type": "named", + "data": { + "name": "align-launch" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Amp 3" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Center.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Center.auto new file mode 100644 index 00000000..34fff746 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Center.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.367427722973922, + "y": 5.5866114582541275 + }, + "rotation": 179.5103044068707 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Center 1" + } + }, + { + "type": "named", + "data": { + "name": "align-launch" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Center 2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Safe Blue.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Blue.auto new file mode 100644 index 00000000..391509d9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Blue.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.77430354290908, + "y": 4.303448834817898 + }, + "rotation": 120.48894049983096 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Safe 1 Blue" + } + }, + { + "type": "named", + "data": { + "name": "align-launch" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Safe 2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Simple Shoot Safe Red.auto b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Red.auto new file mode 100644 index 00000000..f56c1571 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Simple Shoot Safe Red.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.77430354290908, + "y": 4.303448834817898 + }, + "rotation": 120.48894049983096 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Simple Shoot Safe 1 Red" + } + }, + { + "type": "named", + "data": { + "name": "align-launch" + } + }, + { + "type": "path", + "data": { + "pathName": "Simple Shoot Safe 2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CenterLine 1.path b/src/main/deploy/pathplanner/paths/CenterLine 1.path index ced121b8..12e7dacd 100644 --- a/src/main/deploy/pathplanner/paths/CenterLine 1.path +++ b/src/main/deploy/pathplanner/paths/CenterLine 1.path @@ -50,7 +50,7 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-2.path b/src/main/deploy/pathplanner/paths/Centerline 1-2.path index 1251dd60..1f7be50f 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-2.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-3.path b/src/main/deploy/pathplanner/paths/Centerline 1-3.path index a2afe3ba..435c51f3 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-3.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-4.path b/src/main/deploy/pathplanner/paths/Centerline 1-4.path index 838acb79..3b06e9d7 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-4.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-4.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Centerline 1-5.path b/src/main/deploy/pathplanner/paths/Centerline 1-5.path index 288f26f8..52d58695 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1-5.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1-5.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-1.path b/src/main/deploy/pathplanner/paths/Close 2-1.path index b3da8a35..5eaa51c0 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-1.path +++ b/src/main/deploy/pathplanner/paths/Close 2-1.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-2.path b/src/main/deploy/pathplanner/paths/Close 2-2.path index e0331722..d79f4a61 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-2.path +++ b/src/main/deploy/pathplanner/paths/Close 2-2.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-3.path b/src/main/deploy/pathplanner/paths/Close 2-3.path index 114fba54..8463c654 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-3.path +++ b/src/main/deploy/pathplanner/paths/Close 2-3.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Close 2-Escape.path b/src/main/deploy/pathplanner/paths/Close 2-Escape.path index c3700968..1a135d72 100644 --- a/src/main/deploy/pathplanner/paths/Close 2-Escape.path +++ b/src/main/deploy/pathplanner/paths/Close 2-Escape.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path b/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path index 6d09c6e6..63a7af51 100644 --- a/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path +++ b/src/main/deploy/pathplanner/paths/Mid 2 Piece 1.path @@ -28,18 +28,24 @@ "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 132.00142152168155, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, - "rotation": 139.02826366648515, + "rotation": 135.73452103426732, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path b/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path index ca9864c5..c3298d79 100644 --- a/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path +++ b/src/main/deploy/pathplanner/paths/Mid 2 Piece 2.path @@ -62,7 +62,7 @@ } ], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path b/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path index 3a134236..f74adcdc 100644 --- a/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path +++ b/src/main/deploy/pathplanner/paths/Mid 2 Piece 3.path @@ -38,7 +38,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Rotate 90.path b/src/main/deploy/pathplanner/paths/Rotate 90.path index c36eceda..7dbceff8 100644 --- a/src/main/deploy/pathplanner/paths/Rotate 90.path +++ b/src/main/deploy/pathplanner/paths/Rotate 90.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path new file mode 100644 index 00000000..72b471fe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6946944060158917, + "y": 6.727333169630711 + }, + "prevControl": null, + "nextControl": { + "x": 0.6946944060158917, + "y": 6.717583411413854 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0554354600395677, + "y": 7.283069387991508 + }, + "prevControl": { + "x": 1.0651852182564236, + "y": 7.283069387991508 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -123.3484442689218, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -119.87932558561347, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path new file mode 100644 index 00000000..5c78a5d8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.06, + "y": 7.28 + }, + "prevControl": null, + "nextControl": { + "x": 1.0261861853889993, + "y": 7.2538201133409395 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9273890376759386, + "y": 6.932078092184688 + }, + "prevControl": { + "x": 1.9273890376759386, + "y": 6.932078092184688 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -134.11859600341796, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -123.35467894844413, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path new file mode 100644 index 00000000..670c8d98 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Amp 3.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.82, + "y": 6.92 + }, + "prevControl": null, + "nextControl": { + "x": 3.82, + "y": 6.92 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.480526124947738, + "y": 6.92 + }, + "prevControl": { + "x": 6.480526124947738, + "y": 6.92 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -151.35337731670384, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path new file mode 100644 index 00000000..ae44b6d8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 1.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3381784483283943, + "y": 5.547612425391122 + }, + "prevControl": null, + "nextControl": { + "x": 2.338178448328395, + "y": 5.547612425391122 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.902364859361549, + "y": 5.547612425391122 + }, + "prevControl": { + "x": 2.902364859361549, + "y": 5.547612425391122 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -160.34617594194665, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 178.70783311375007, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path new file mode 100644 index 00000000..27f7c353 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Center 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.26, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 3.2588808170490466, + "y": 6.629835587462149 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.773018871453421, + "y": 7.07832446543753 + }, + "prevControl": { + "x": 6.773018871453421, + "y": 7.07832446543753 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 177.79740183823418, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -179.67887802899924, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path new file mode 100644 index 00000000..0990d7e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Blue.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.77430354290908, + "y": 4.303448834817898 + }, + "prevControl": null, + "nextControl": { + "x": 0.77430354290908, + "y": 4.27419956016733 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8688904883748028, + "y": 3.1491719040445205 + }, + "prevControl": { + "x": 2.8396412137242346, + "y": 3.1881709369119444 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 137.4528248632394, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 122.23888483747, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path new file mode 100644 index 00000000..360ce985 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 1 Red.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.77430354290908, + "y": 4.303448834817898 + }, + "prevControl": null, + "nextControl": { + "x": 0.77430354290908, + "y": 4.27419956016733 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8688904883748028, + "y": 3.1491719040445205 + }, + "prevControl": { + "x": 2.8396412137242346, + "y": 3.1881709369119444 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 141.52214025945182, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 122.23888483747, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path new file mode 100644 index 00000000..ffb2c456 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Simple Shoot Safe 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.58, + "y": 3.02 + }, + "prevControl": null, + "nextControl": { + "x": 2.63367409925703, + "y": 1.7073465485962325 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.925428779632488, + "y": 0.6665668032190769 + }, + "prevControl": { + "x": 6.200391204201211, + "y": 0.7016492665463968 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 15.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 180.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 134.26332354707648, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/drive 1m.path b/src/main/deploy/pathplanner/paths/drive 1m.path index a4204a30..a82d9acc 100644 --- a/src/main/deploy/pathplanner/paths/drive 1m.path +++ b/src/main/deploy/pathplanner/paths/drive 1m.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, + "maxVelocity": 15.0, "maxAcceleration": 5.0, "maxAngularVelocity": 180.0, "maxAngularAcceleration": 180.0 diff --git a/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json b/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json index 669097e7..9ef40425 100644 --- a/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/controllerproperties.json @@ -2,7 +2,7 @@ "angleJoystickRadiusDeadband": 0.5, "heading": { "p": 0.4, - "i": 0, + "i": 0.1, "d": 0.01 } } \ No newline at end of file diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/backright.json b/src/main/deploy/swerves/KrakenSwerve/modules/backright.json index 7481ed76..09c7aeef 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/backright.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/backright.json @@ -18,7 +18,7 @@ "drive": true, "angle": false }, - "absoluteEncoderOffset": 163, + "absoluteEncoderOffset": 164, "location": { "front": -13, "left": -13 diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json b/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json index 8d6a03c3..4a854b8b 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/frontleft.json @@ -18,7 +18,7 @@ "drive": true, "angle": false }, - "absoluteEncoderOffset": 180, + "absoluteEncoderOffset": 135, "location": { "front": 13, "left": 13 diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json b/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json index a58ed1b5..542ac431 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/frontright.json @@ -18,7 +18,7 @@ "drive": true, "angle": false }, - "absoluteEncoderOffset": 125, + "absoluteEncoderOffset": 58, "location": { "front": 13, "left": -13 diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json index d32da52b..ad1ea915 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json @@ -4,13 +4,13 @@ "angle": 16.8 }, "currentLimit": { - "drive": 40, - "angle": 20 + "drive": 70, + "angle": 40 }, "rampRate": { - "drive": 0.25, - "angle": 0.25 + "drive": 0.05, + "angle": 0.05 }, "wheelGripCoefficientOfFriction": 1.19, - "optimalVoltage": 12 + "optimalVoltage": 10.5 } \ No newline at end of file diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json index 3d4d5c55..d3ab830e 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/pidfproperties.json @@ -1,13 +1,13 @@ { "drive": { "p": 1, - "i": 0, + "i": 0.1, "d": 0, "f": 0, "iz": 0 }, "angle": { - "p": 0.004, + "p": 0.1, "i": 0, "d": 1.5, "f": 0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index efdc93a1..b89741d5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,23 +25,20 @@ public final class Constants { public static final boolean kRightFrontDriveInverted = false; public static final double LOOP_TIME = 0.13; - public static final double ROBOT_MASS = 150 * 0.453592; + public static final double ROBOT_MASS = 115 * 0.453592; public static final Matter CHASSIS = - new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); + new Matter(new Translation3d(0, 0, Units.inchesToMeters(0)), ROBOT_MASS); public static final PIDConstants autoTranslationPID = new PIDConstants(0.7, 0, 0); public static final PIDConstants autoRotationPID = new PIDConstants(0.4, 0, 0.01); public static class Index { - public static final int lowerCANID = 40; public static final int whooperCANID = 41; public static final int upperCANID = 42; - public static final boolean lowerInverted = true; public static final boolean upperInverted = true; public static final boolean whooperInverted = false; public static final int upperBeam = 1; public static final int lowerBeam = 0; - public static final double lowerSpeed = 0.15; public static final double whooperSpeed = 0.9; public static final double upperSpeed = 0.3; } @@ -95,7 +92,13 @@ public static class Climb { // These are break beam sensor IDS public static final int winchLimitLeft = 2; public static final int winchLimitRight = 3; - public static final double motorSpeedFactor = -0.5; + public static final double motorSpeedFactor = -0.9; + } + + public static class Transfer { + public static final boolean motorInverted = true; + public static final double motorSpeed = 0.15; + public static final int motorID = 40; } public static final String limelightName = "limelight"; @@ -105,5 +108,6 @@ public static class OperatorConstants { public static final double LEFT_X_DEADBAND = 0.01; public static final double LEFT_Y_DEADBAND = 0.01; public static final double RIGHT_X_DEADBAND = 0.01; + public static final int TURN_CONSTANT = 6; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ca2b6bec..99a6a76b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,6 +34,7 @@ public void robotInit() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); getAlliance(); + m_robotContainer.lockLauncher(); } private void getAlliance() { @@ -95,7 +96,7 @@ public void teleopInit() { } getAlliance(); m_robotContainer.updatePose(); - m_robotContainer.dropLauncher(); + m_robotContainer.lockLauncher(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f5933b4d..f3d8d28f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AbsoluteDrive; +import frc.robot.commands.AbsoluteDriveAdv; import frc.robot.commands.AlignLaunchAuto; import frc.robot.commands.LaunchWithVelo; import frc.robot.commands.LaunchWithVeloAuton; @@ -28,6 +29,7 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Launcher; import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.Transfer; import java.io.File; /** @@ -45,6 +47,7 @@ public class RobotContainer { private final Index m_index = new Index(); private final Launcher m_launch = new Launcher(); private final Climb m_climb = new Climb(); + private final Transfer m_transfer = new Transfer(); // Replace with CommandPS4Controller or CommandJoystick if needed private final XboxController driver = new XboxController(0); @@ -56,17 +59,25 @@ public RobotContainer() { // Create commands for PathPlanner NamedCommands.registerCommand( "launch", new LaunchWithVeloAuton(m_launch, m_index, Constants.Launch.speedFarSpeaker)); - NamedCommands.registerCommand("intake", new ToggleIntake(m_intake)); - NamedCommands.registerCommand("index", new PrimeIndex(m_index)); + NamedCommands.registerCommand("intake", new ToggleIntake(m_intake, m_transfer)); + NamedCommands.registerCommand("index", new PrimeIndex(m_index, m_transfer)); NamedCommands.registerCommand( - "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 3000, 1)); + "align-launch", new AlignLaunchAuto(m_swerve, m_launch, m_index, 5300, 1)); NamedCommands.registerCommand("reverse intake", m_intake.reverseIntakeCommand()); - CameraServer.startAutomaticCapture(); + CameraServer.startAutomaticCapture(0); + CameraServer.startAutomaticCapture(1); // Configure the trigger bindings configureBindings(); + int povAngle = driver.getPOV(); + + Trigger driverUp = new Trigger(() -> (povAngle >= 315 || povAngle <= 45)); + Trigger driverDown = new Trigger(() -> (povAngle >= 135 && povAngle <= 225)); + Trigger driverLeft = new Trigger(() -> (povAngle >= 225 && povAngle <= 315)); + Trigger driverRight = new Trigger(() -> (povAngle >= 45 && povAngle <= 135)); + AbsoluteDrive closedAbsoluteDrive = new AbsoluteDrive( m_swerve, @@ -78,8 +89,18 @@ public RobotContainer() { () -> -driver.getRightX(), () -> -driver.getRightY()); - m_swerve.setDefaultCommand( - !RobotBase.isSimulation() ? closedAbsoluteDrive : closedAbsoluteDrive); + AbsoluteDriveAdv advancedDrive = + new AbsoluteDriveAdv( + m_swerve, + () -> MathUtil.applyDeadband(-driver.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), + () -> MathUtil.applyDeadband(-driver.getLeftX(), OperatorConstants.LEFT_X_DEADBAND), + () -> MathUtil.applyDeadband(-driver.getRightX(), OperatorConstants.RIGHT_X_DEADBAND), + () -> driverUp.getAsBoolean(), + () -> driverDown.getAsBoolean(), + () -> driverLeft.getAsBoolean(), + () -> driverRight.getAsBoolean()); + + m_swerve.setDefaultCommand(!RobotBase.isSimulation() ? closedAbsoluteDrive : advancedDrive); // -m_index.setDefaultCommand(m_index.manualIntake(coDriver::getRightY)); @@ -92,11 +113,16 @@ public RobotContainer() { m_chooser.addOption("Turn Auto", m_swerve.getAutonomousCommand("Turn Auto")); m_chooser.addOption("Drive and Turn", m_swerve.getAutonomousCommand("drive and turn")); m_chooser.addOption("1 Centerline", m_swerve.getAutonomousCommand("1 Centerline")); - m_chooser.addOption("2 Centerline", m_swerve.getAutonomousCommand("2 Centerline")); - m_chooser.addOption("3 Centerline", m_swerve.getAutonomousCommand("3 Centerline")); - m_chooser.addOption("4 Centerline", m_swerve.getAutonomousCommand("4 Centerline")); m_chooser.addOption("5 Centerline", m_swerve.getAutonomousCommand("5 Centerline")); m_chooser.addOption("Close 2", m_swerve.getAutonomousCommand("Close 2")); + m_chooser.addOption("Just Chill", m_swerve.noAuto()); + m_chooser.addOption( + "Simple Shoot Center", m_swerve.getAutonomousCommand("Simple Shoot Center")); + m_chooser.addOption( + "Simple Shoot Safe Blue", m_swerve.getAutonomousCommand("Simple Shoot Safe Blue")); + m_chooser.addOption("Simple Shoot Amp", m_swerve.getAutonomousCommand("Simple Shoot Amp")); + m_chooser.addOption( + "Simple Shoot Safe Red", m_swerve.getAutonomousCommand("Simple Shoot Safe Red")); SmartDashboard.putData(m_chooser); } @@ -113,10 +139,13 @@ public RobotContainer() { private void configureBindings() { JoystickButton leftBumper = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); - leftBumper.onTrue(new ToggleIntake(m_intake)); + leftBumper.onTrue(new ToggleIntake(m_intake, m_transfer)); + + JoystickButton rb = new JoystickButton(driver, XboxController.Button.kRightBumper.value); + rb.whileTrue(new LaunchWithVelo(m_launch, m_index, -1500, false)); JoystickButton coRB = new JoystickButton(coDriver, XboxController.Button.kRightBumper.value); - coRB.onTrue(new PrimeIndex(m_index)); + coRB.onTrue(new PrimeIndex(m_index, m_transfer)); Trigger lt = new Trigger(() -> driver.getLeftTriggerAxis() >= 0.05); lt.whileTrue(m_swerve.alignCommand()); @@ -125,10 +154,16 @@ private void configureBindings() { rt.whileTrue(new LaunchWithVelo(m_launch, m_index, 5200, false)); JoystickButton x = new JoystickButton(driver, XboxController.Button.kX.value); - x.onTrue(m_swerve.updatePositionCommand()); + x.whileTrue(m_swerve.updatePositionCommand()); + + JoystickButton a = new JoystickButton(driver, XboxController.Button.kA.value); + a.whileTrue(new LaunchWithVelo(m_launch, m_index, 2050, false)); JoystickButton y = new JoystickButton(driver, XboxController.Button.kY.value); y.whileTrue(m_intake.reverseIntakeCommand()); + + JoystickButton b = new JoystickButton(driver, XboxController.Button.kB.value); + b.whileTrue(m_swerve.setRotationCommand(180)); } /** @@ -160,4 +195,8 @@ public void updatePose() { public void dropLauncher() { m_launch.releaseLauncher(); } + + public void lockLauncher() { + m_launch.lockLauncher(); + } } diff --git a/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java new file mode 100644 index 00000000..d035103b --- /dev/null +++ b/src/main/java/frc/robot/commands/AbsoluteDriveAdv.java @@ -0,0 +1,152 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.SwerveSubsystem; +import java.util.List; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import swervelib.SwerveController; +import swervelib.math.SwerveMath; + +/** A more advanced Swerve Control System that has 4 buttons for which direction to face */ +public class AbsoluteDriveAdv extends Command { + + private final SwerveSubsystem swerve; + private final DoubleSupplier vX, vY; + private final DoubleSupplier headingAdjust; + private final BooleanSupplier lookAway, lookTowards, lookLeft, lookRight; + private boolean resetHeading = false; + + /** + * Used to drive a swerve robot in full field-centric mode. vX and vY supply translation inputs, + * where x is torwards/away from alliance wall and y is left/right. Heading Adjust changes the + * current heading after being multipied by a constant. The look booleans are shortcuts to get the + * robot to face a certian direction. Based off of ideas in + * https://www.chiefdelphi.com/t/experiments-with-a-swerve-steering-knob/446172 + * + * @param swerve The swerve drivebase subsystem. + * @param vX DoubleSupplier that supplies the x-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive X is away from the alliance wall. + * @param vY DoubleSupplier that supplies the y-translation joystick input. Should be in the range + * -1 to 1 with deadband already accounted for. Positive Y is towards the left wall when + * looking through the driver station glass. + * @param headingAdjust DoubleSupplier that supplies the component of the robot's heading angle + * that should be adjusted. Should range from -1 to 1 with deadband already accounted for. + * @param lookAway Face the robot towards the opposing alliance's wall in the same direction the + * driver is facing + * @param lookTowards Face the robot towards the driver + * @param lookLeft Face the robot left + * @param lookRight Face the robot right + */ + public AbsoluteDriveAdv( + SwerveSubsystem swerve, + DoubleSupplier vX, + DoubleSupplier vY, + DoubleSupplier headingAdjust, + BooleanSupplier lookAway, + BooleanSupplier lookTowards, + BooleanSupplier lookLeft, + BooleanSupplier lookRight) { + this.swerve = swerve; + this.vX = vX; + this.vY = vY; + this.headingAdjust = headingAdjust; + this.lookAway = lookAway; + this.lookTowards = lookTowards; + this.lookLeft = lookLeft; + this.lookRight = lookRight; + + addRequirements(swerve); + } + + @Override + public void initialize() { + resetHeading = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double headingX = 0; + double headingY = 0; + + // These are written to allow combinations for 45 angles + // Face Away from Drivers + if (lookAway.getAsBoolean()) { + headingY = -1; + } + // Face Right + if (lookRight.getAsBoolean()) { + headingX = 1; + } + // Face Left + if (lookLeft.getAsBoolean()) { + headingX = -1; + } + // Face Towards the Drivers + if (lookTowards.getAsBoolean()) { + headingY = 1; + } + + // Prevent Movement After Auto + if (resetHeading) { + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { + // Get the curret Heading + Rotation2d currentHeading = swerve.getHeading(); + + // Set the Current Heading to the desired Heading + headingX = currentHeading.getSin(); + headingY = currentHeading.getCos(); + } + // Dont reset Heading Again + resetHeading = false; + } + + ChassisSpeeds desiredSpeeds = + swerve.getTargetSpeeds(vX.getAsDouble(), vY.getAsDouble(), headingX, headingY); + + // Limit velocity to prevent tippy + Translation2d translation = SwerveController.getTranslation2d(desiredSpeeds); + translation = + SwerveMath.limitVelocity( + translation, + swerve.getFieldVelocity(), + swerve.getPose(), + Constants.LOOP_TIME, + Constants.ROBOT_MASS, + List.of(Constants.CHASSIS), + swerve.getSwerveDriveConfiguration()); + SmartDashboard.putNumber("LimitedTranslation", translation.getX()); + SmartDashboard.putString("Translation", translation.toString()); + + // Make the robot move + if (headingX == 0 && headingY == 0 && Math.abs(headingAdjust.getAsDouble()) > 0) { + resetHeading = true; + swerve.drive( + translation, + (Constants.OperatorConstants.TURN_CONSTANT * -headingAdjust.getAsDouble()), + true); + } else { + swerve.drive(translation, desiredSpeeds.omegaRadiansPerSecond, true); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/LaunchWithVelo.java b/src/main/java/frc/robot/commands/LaunchWithVelo.java index ac90270e..721fe2d5 100644 --- a/src/main/java/frc/robot/commands/LaunchWithVelo.java +++ b/src/main/java/frc/robot/commands/LaunchWithVelo.java @@ -42,7 +42,11 @@ public void initialize() { @Override public void execute() { if (launcher.readyToLaunch(launchVelo)) { - index.feed(); + if (launchVelo > 0) { + index.feed(); + } else { + index.reverseFeed(); + } } } diff --git a/src/main/java/frc/robot/commands/PrimeIndex.java b/src/main/java/frc/robot/commands/PrimeIndex.java index f42f71a9..a32c7886 100644 --- a/src/main/java/frc/robot/commands/PrimeIndex.java +++ b/src/main/java/frc/robot/commands/PrimeIndex.java @@ -6,9 +6,11 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Index; +import frc.robot.subsystems.Transfer; public class PrimeIndex extends Command { private final Index index; + private final Transfer transfer; private boolean inUpper; private boolean wasRunning; @@ -17,8 +19,9 @@ public class PrimeIndex extends Command { * * @param subsystem The subsystem used by this command. */ - public PrimeIndex(Index index) { + public PrimeIndex(Index index, Transfer transfer) { this.index = index; + this.transfer = transfer; inUpper = false; wasRunning = index.isRunning(); // Use addRequirements() here to declare subsystem dependencies. @@ -28,12 +31,9 @@ public PrimeIndex(Index index) { // Called when the command is initially scheduled. @Override public void initialize() { - if (!wasRunning) { - System.out.println("Index is initializing"); - index.start(); - } else { - index.stop(); - } + System.out.println("Index is initializing"); + index.start(); + transfer.start(); } // Called every time the scheduler runs while the command is scheduled. @@ -41,7 +41,7 @@ public void initialize() { public void execute() { if (!inUpper && index.isInUpper()) { inUpper = true; - index.setLower(0); + transfer.stop(); } } @@ -49,15 +49,12 @@ public void execute() { @Override public void end(boolean interrupted) { index.stop(); + transfer.stop(); } // Returns true when the command should end. @Override public boolean isFinished() { - if (wasRunning) { - return true; - } else { - return index.isPrimed(); - } + return index.isPrimed(); } } diff --git a/src/main/java/frc/robot/commands/ToggleIntake.java b/src/main/java/frc/robot/commands/ToggleIntake.java index 73afa229..f8de9ace 100644 --- a/src/main/java/frc/robot/commands/ToggleIntake.java +++ b/src/main/java/frc/robot/commands/ToggleIntake.java @@ -6,13 +6,16 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Transfer; public class ToggleIntake extends Command { private Intake intake; + private Transfer transfer; /** Creates a new ToggleIntake. */ - public ToggleIntake(Intake intake) { + public ToggleIntake(Intake intake, Transfer transfer) { this.intake = intake; + this.transfer = transfer; addRequirements(intake); } @@ -21,6 +24,11 @@ public ToggleIntake(Intake intake) { public void initialize() { System.out.println("Intake is initializing"); intake.toggle(); + if (intake.isRunning()) { + transfer.start(); + } else { + transfer.stop(); + } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/Index.java b/src/main/java/frc/robot/subsystems/Index.java index 1e47632a..ee2c5064 100644 --- a/src/main/java/frc/robot/subsystems/Index.java +++ b/src/main/java/frc/robot/subsystems/Index.java @@ -12,8 +12,6 @@ import frc.robot.Constants; public class Index extends SubsystemBase { - private CANSparkMax motorLower = - new CANSparkMax(Constants.Index.lowerCANID, CANSparkLowLevel.MotorType.kBrushless); private CANSparkMax motorWhooper = new CANSparkMax(Constants.Index.whooperCANID, CANSparkLowLevel.MotorType.kBrushless); private CANSparkMax motorUpper = @@ -25,20 +23,17 @@ public class Index extends SubsystemBase { double speed = 0.95; public Index() { - motorLower.setInverted(Constants.Index.lowerInverted); motorWhooper.setInverted(Constants.Index.whooperInverted); motorUpper.setInverted(Constants.Index.upperInverted); } private void set(double power) { - motorLower.set(power); motorWhooper.set(power); motorUpper.set(power - 0.5); currentSpeed = power; } public void run() { - runLower(); runWhooper(); runUpper(); } @@ -47,12 +42,12 @@ public void feed() { motorUpper.set(0.9); } - public void setUpper(double speed) { - motorUpper.set(speed); + public void reverseFeed() { + motorUpper.set(-0.9); } - public void setLower(double speed) { - motorLower.set(speed); + public void setUpper(double speed) { + motorUpper.set(speed); } public void setWhooper(double speed) { @@ -67,10 +62,6 @@ public void runWhooper() { setWhooper(Constants.Index.whooperSpeed); } - public void runLower() { - setLower(Constants.Index.lowerSpeed); - } - public boolean isRunning() { return !(motorUpper.get() == 0); } @@ -80,7 +71,6 @@ public void start() { } public void stop() { - motorLower.set(0); motorWhooper.set(0); motorUpper.set(0); } @@ -121,6 +111,7 @@ public void periodic() { @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation + System.out.println(isPrimed()); if (isPrimed() == true) { stop(); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index be5df587..0e5e96ae 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -6,6 +6,7 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; +import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -21,8 +22,10 @@ public class Intake extends SubsystemBase { private CANSparkMax motorRight = new CANSparkMax(Constants.Intake.rightCANID, CANSparkLowLevel.MotorType.kBrushless); + private SlewRateLimiter limiter = new SlewRateLimiter(0.5); + double speed = Constants.Intake.speed; - double currentSpeed = 0.0; + double targetSpeed = 0; public Intake() { motorLeft.setInverted(Constants.Intake.leftInverted); @@ -32,14 +35,11 @@ public Intake() { } private void set(double power) { - motorLeft.set(power); - motorCenter.set(power); - motorRight.set(power); - currentSpeed = power; + targetSpeed = power; } public void toggle() { - if (currentSpeed == 0.0) { + if (targetSpeed == 0.0) { start(); } else { stop(); @@ -59,13 +59,17 @@ public void stop() { } public boolean isRunning() { - return !(motorLeft.get() == 0); + return !(targetSpeed == 0); } @Override public void periodic() { // This method will be called once per scheduler run SmartDashboard.putBoolean("Intake Running", isRunning()); + + motorLeft.set(limiter.calculate(targetSpeed)); + motorRight.set(limiter.calculate(targetSpeed)); + motorCenter.set(limiter.calculate(targetSpeed)); } public Command reverseIntakeCommand() { diff --git a/src/main/java/frc/robot/subsystems/Launcher.java b/src/main/java/frc/robot/subsystems/Launcher.java index df78a992..b94ec3d0 100644 --- a/src/main/java/frc/robot/subsystems/Launcher.java +++ b/src/main/java/frc/robot/subsystems/Launcher.java @@ -210,6 +210,10 @@ public void releaseLauncher() { angle.setIdleMode(IdleMode.kCoast); } + public void lockLauncher() { + angle.setIdleMode(IdleMode.kBrake); + } + public Command switchAngleCommand() { return this.runOnce(() -> switchAngle()); } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index b8c854cd..ebf051bb 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -52,7 +52,7 @@ public class SwerveSubsystem extends SubsystemBase /** * Maximum speed of the robot in meters per second, used to limit acceleration. */ - public double maximumSpeed = Units.feetToMeters(14.5); + public double maximumSpeed = Units.feetToMeters(30); /** * Initialize {@link SwerveDrive} with the directory provided. @@ -86,7 +86,7 @@ public SwerveSubsystem(File directory) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection(true); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. setupPathPlanner(); @@ -207,7 +207,7 @@ public Command driveToPose(Pose2d pose) public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, DoubleSupplier headingY) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. return run(() -> { double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth controll out double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth controll out @@ -230,7 +230,7 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat */ public Command simDriveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { - // swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. return run(() -> { // Make the robot move driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(translationX.getAsDouble(), @@ -543,10 +543,11 @@ public void setRotation(double angle) { swerveDrive.drive(desired); } - public void resetToLimelight() { + public void + resetToLimelight() { boolean hasTarget = LimelightHelpers.getTV(Constants.limelightName); - while (!hasTarget) { - hasTarget = LimelightHelpers.getTV(Constants.limelightName); + if (!hasTarget) { + return; } Pose2d pose; if (Robot.alliance == Alliance.Red && hasTarget) { @@ -639,7 +640,7 @@ public Command alignCommand() { } public Command updatePositionCommand() { - return this.runOnce(() -> resetToLimelight()); + return this.run(() -> resetToLimelight()); } public Command zeroGyroCommand(){ @@ -649,4 +650,12 @@ public Command zeroGyroCommand(){ public Command setRotationCommand(double rotation){ return this.run(() -> setRotation(Units.degreesToRadians(rotation))); } + + public void nullFunction() { + return; + } + + public Command noAuto() { + return this.runOnce(() -> nullFunction()); + } } diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java new file mode 100644 index 00000000..3ab72904 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Transfer.java @@ -0,0 +1,45 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Transfer extends SubsystemBase { + /** Creates a new Transfer. */ + CANSparkMax motor = new CANSparkMax(Constants.Transfer.motorID, MotorType.kBrushless); + + private double currentSpeed; + + public Transfer() { + motor.setInverted(Constants.Transfer.motorInverted); + currentSpeed = 0; + } + + public void start() { + motor.set(Constants.Transfer.motorSpeed); + currentSpeed = Constants.Transfer.motorSpeed; + } + + public void stop() { + motor.set(0); + currentSpeed = 0; + } + + public void toggle() { + if (currentSpeed == 0.0) { + start(); + } else { + stop(); + } + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f4..b849e926 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json index 8f7fdfda..c280eee6 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl.json @@ -1,7 +1,7 @@ { "fileName": "yagsl.json", "name": "YAGSL", - "version": "2024.4.8.5", + "version": "2024.4.8.6", "frcYear": "2024", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", - "version": "2024.4.8.5" + "version": "2024.4.8.6" } ], "jniDependencies": [],