diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 5303a099..7d74ae9d 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -8,8 +8,8 @@ "Tests" ], "autoFolders": [], - "defaultMaxVel": 5.0, - "defaultMaxAccel": 4.0, + "defaultMaxVel": 6.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 5.0 diff --git a/shuffleboard.json b/shuffleboard.json index 789c920e..9c7ffe39 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -159,6 +159,27 @@ "_glyph": 148, "_showGlyph": false } + }, + "1,0": { + "size": [ + 5, + 4 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://USB Camera 0", + "_title": "USB Camera 0", + "_glyph": 148, + "_showGlyph": false, + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "HALF", + "compression": -1.0, + "fps": -1, + "imageWidth": 0, + "imageHeight": 0 + } } } } diff --git a/src/main/deploy/pathplanner/autos/6 CBAED.auto b/src/main/deploy/pathplanner/autos/6 CBAED.auto index 37eed8f9..f1abb8a7 100644 --- a/src/main/deploy/pathplanner/autos/6 CBAED.auto +++ b/src/main/deploy/pathplanner/autos/6 CBAED.auto @@ -8,7 +8,13 @@ { "type": "path", "data": { - "pathName": "Preload to C" + "pathName": "Preload to C Close" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Preload to C" } }, { diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path index eb91cb13..ca53b80e 100644 --- a/src/main/deploy/pathplanner/paths/A to D.path +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.8, - "y": 6.94 + "x": 2.70383902591167, + "y": 6.887856966597124 }, "prevControl": null, "nextControl": { - "x": 3.7902680687415704, - "y": 7.079173100960065 + "x": 3.6941070946532406, + "y": 7.027030067557189 }, "isLocked": false, "linkedName": "A" diff --git a/src/main/deploy/pathplanner/paths/A to E.path b/src/main/deploy/pathplanner/paths/A to E.path index 3cf34949..bf4678ec 100644 --- a/src/main/deploy/pathplanner/paths/A to E.path +++ b/src/main/deploy/pathplanner/paths/A to E.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.8, - "y": 6.94 + "x": 2.70383902591167, + "y": 6.887856966597124 }, "prevControl": null, "nextControl": { - "x": 4.91984991243836, - "y": 7.023157661821328 + "x": 4.82368893835003, + "y": 6.971014628418452 }, "isLocked": false, "linkedName": "A" @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 6.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index ffa08e70..c0bf1429 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.78765406878382, - "y": 5.239147655045014 + "x": 2.9260279603180286, + "y": 5.309146116867734 }, "prevControl": null, "nextControl": { - "x": 2.6183483616967234, - "y": 5.861535033255252 + "x": 2.756722253230932, + "y": 5.931533495077972 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 2.8, - "y": 6.94 + "x": 2.70383902591167, + "y": 6.887856966597124 }, "prevControl": { - "x": 2.646912247528199, - "y": 6.50431154103049 + "x": 2.5507512734398694, + "y": 6.452168507627614 }, "nextControl": null, "isLocked": false, @@ -31,21 +31,21 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 60.0, + "rotationDegrees": 52.37639263407269, "rotateFast": true } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 51.76617482255292, + "rotation": 50.90614111377048, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C to B Close.path b/src/main/deploy/pathplanner/paths/C to B Close.path index 01b578f6..e52e881d 100644 --- a/src/main/deploy/pathplanner/paths/C to B Close.path +++ b/src/main/deploy/pathplanner/paths/C to B Close.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.78765406878382, - "y": 5.239147655045014 + "x": 2.9260279603180286, + "y": 5.309146116867734 }, "prevControl": { - "x": 2.4857700960953526, - "y": 5.063860187032356 + "x": 2.6241439876295614, + "y": 5.133858648855076 }, "nextControl": null, "isLocked": false, @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 4d2cbcf0..c509df3a 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.5, - "y": 4.878834526352327 + "x": 2.62197994481459, + "y": 4.794603321400375 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 2.78765406878382, - "y": 5.239147655045014 + "x": 2.9260279603180286, + "y": 5.309146116867734 }, "prevControl": { - "x": 2.5539374447669423, - "y": 4.8009289850133685 + "x": 2.692311336301151, + "y": 4.870927446836089 }, "nextControl": null, "isLocked": false, @@ -31,21 +31,21 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 60.0, + "rotationDegrees": 34.799293349232, "rotateFast": true } ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 60.71172956223985, + "rotation": 43.876697285924436, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path b/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path index 6e19b1f6..95a1284e 100644 --- a/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path +++ b/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.14162084164624, - "y": -0.14032985330927936 + "x": 8.290000000000001, + "y": 0.4 }, "prevControl": { - "x": 5.120084675408034, - "y": -0.14032985330927936 + "x": 5.268463833761794, + "y": 0.39999999999999997 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.55, - "maxAcceleration": 5.5, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Close Preload to C.path b/src/main/deploy/pathplanner/paths/Close Preload to C.path index 35835595..e4ccc948 100644 --- a/src/main/deploy/pathplanner/paths/Close Preload to C.path +++ b/src/main/deploy/pathplanner/paths/Close Preload to C.path @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": -29.392354726969238, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Close Shoot.path b/src/main/deploy/pathplanner/paths/Close Shoot.path new file mode 100644 index 00000000..4a64e859 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Close Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3239288017037572, + "y": 5.531335051274091 + }, + "prevControl": null, + "nextControl": { + "x": 2.323928801703759, + "y": 5.531335051274091 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6978421535190646, + "y": 5.531335051274091 + }, + "prevControl": { + "x": 2.6978421535190646, + "y": 5.531335051274091 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Tests", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Curved Line.path b/src/main/deploy/pathplanner/paths/Curved Line.path index a7bdaed6..ffa21a81 100644 --- a/src/main/deploy/pathplanner/paths/Curved Line.path +++ b/src/main/deploy/pathplanner/paths/Curved Line.path @@ -80,8 +80,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/D to Shoot.path b/src/main/deploy/pathplanner/paths/D to Shoot.path index ba36f36e..357a1275 100644 --- a/src/main/deploy/pathplanner/paths/D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/D to Shoot.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index 5213bed9..135d3fe9 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -28,12 +28,18 @@ "linkedName": "Top Side Shoot" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": 15.0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 6.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path index c31efa7e..82964740 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.508704809076285, - "y": 4.189449101120425 + "x": 5.559068829433955, + "y": 4.954498043255346 }, "isLocked": false, "linkedName": "F" }, { "anchor": { - "x": 3.338562098420493, - "y": 5.052304113867487 + "x": 2.18, + "y": 3.1400677569873663 }, "prevControl": { - "x": 4.81527342684315, - "y": 4.724383578396787 + "x": 3.3717256982662116, + "y": 1.8074229296606745 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -20.559668324391616, + "rotation": -52.04098183775284, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ferry Shot to E.path b/src/main/deploy/pathplanner/paths/Ferry Shot to E.path index 3dee2c2f..180624fe 100644 --- a/src/main/deploy/pathplanner/paths/Ferry Shot to E.path +++ b/src/main/deploy/pathplanner/paths/Ferry Shot to E.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path b/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path index 874a9f14..0101b601 100644 --- a/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.917428261138573, - "y": 1.0335790082691276 + "x": 5.229776385478697, + "y": 0.5379311043522382 }, "isLocked": false, "linkedName": "G" }, { "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 + "x": 1.6279768172071953, + "y": 3.730435267138341 }, "prevControl": { - "x": 3.4288766013429455, - "y": 1.4617693053049925 + "x": 2.271155311541392, + "y": 2.03478287298455 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path b/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path index 6cce4637..93675792 100644 --- a/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 8.14162084164624, - "y": -0.14032985330927936 + "x": 8.290000000000001, + "y": 0.4 }, "prevControl": null, "nextControl": { - "x": 5.932118299133126, - "y": 0.24833522794883411 + "x": 6.080497457486887, + "y": 0.7886650812581135 }, "isLocked": false, "linkedName": "H" }, { "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 + "x": 1.6279768172071953, + "y": 3.730435267138341 }, "prevControl": { - "x": 3.188773241245417, - "y": 2.2398182778317244 + "x": 2.873031071299537, + "y": 2.473701366680523 }, "nextControl": null, "isLocked": false, "linkedName": "HShoot" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": -39.8209274955494, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path b/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path index 83ed8fd4..1d433c2a 100644 --- a/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path +++ b/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 + "x": 1.6279768172071953, + "y": 3.730435267138341 }, "prevControl": null, "nextControl": { - "x": 3.8409597536149493, - "y": 0.7380457221309411 + "x": 3.525217583669071, + "y": 0.9719288109797397 }, "isLocked": false, "linkedName": "HShoot" diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path index 7da2abe1..375f5e97 100644 --- a/src/main/deploy/pathplanner/paths/Mobility.path +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Preload to A.path b/src/main/deploy/pathplanner/paths/Preload to A.path index 89aec544..e57b613f 100644 --- a/src/main/deploy/pathplanner/paths/Preload to A.path +++ b/src/main/deploy/pathplanner/paths/Preload to A.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Preload to C Close.path b/src/main/deploy/pathplanner/paths/Preload to C Close.path new file mode 100644 index 00000000..e9294dd9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Preload to C Close.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3473171105886368, + "y": 5.402699352407253 + }, + "prevControl": null, + "nextControl": { + "x": 1.8501657516135543, + "y": 4.9934039469218545 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3764027015233515, + "y": 4.432084533684738 + }, + "prevControl": { + "x": 1.7800008249589148, + "y": 5.122039645788694 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -32.969403903462045, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Preload to C.path b/src/main/deploy/pathplanner/paths/Preload to C.path index 0ca02525..9ccfa9b6 100644 --- a/src/main/deploy/pathplanner/paths/Preload to C.path +++ b/src/main/deploy/pathplanner/paths/Preload to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.52, - "y": 4.1 + "x": 2.493344245947751, + "y": 4.163118981508619 }, "prevControl": { - "x": 2.4857700960953526, - "y": 4.323757544312242 + "x": 2.4591143420431036, + "y": 4.3868765258208615 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.6, - "rotationDegrees": -40.0, + "rotationDegrees": -35.316048663452285, "rotateFast": false } ], @@ -56,7 +56,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -20.0, + "rotation": -32.93139922764586, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SPEED A.path b/src/main/deploy/pathplanner/paths/SPEED A.path index 76848e8e..5472ca69 100644 --- a/src/main/deploy/pathplanner/paths/SPEED A.path +++ b/src/main/deploy/pathplanner/paths/SPEED A.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/SPEED B.path b/src/main/deploy/pathplanner/paths/SPEED B.path index f5963551..3806d63d 100644 --- a/src/main/deploy/pathplanner/paths/SPEED B.path +++ b/src/main/deploy/pathplanner/paths/SPEED B.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sharp Curved Line.path b/src/main/deploy/pathplanner/paths/Sharp Curved Line.path index e9c74b2d..a9535b05 100644 --- a/src/main/deploy/pathplanner/paths/Sharp Curved Line.path +++ b/src/main/deploy/pathplanner/paths/Sharp Curved Line.path @@ -80,8 +80,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path b/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path index 56b962cf..2150ffeb 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path +++ b/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.2, - "y": 7.43 + "x": 8.118232532761363, + "y": 7.729836086452799 }, "prevControl": { - "x": 6.242200000323567, - "y": 7.081144238920643 + "x": 6.160432533084931, + "y": 7.380980325373443 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Simple Note.path b/src/main/deploy/pathplanner/paths/Simple Note.path index 649b587f..a96b7a1f 100644 --- a/src/main/deploy/pathplanner/paths/Simple Note.path +++ b/src/main/deploy/pathplanner/paths/Simple Note.path @@ -64,8 +64,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Start to H (HGF).path b/src/main/deploy/pathplanner/paths/Start to H (HGF).path index 15eec615..2bbdf4a1 100644 --- a/src/main/deploy/pathplanner/paths/Start to H (HGF).path +++ b/src/main/deploy/pathplanner/paths/Start to H (HGF).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.76, - "y": 3.262 + "x": 1.87, + "y": 3.264181107065918 }, "prevControl": null, "nextControl": { - "x": 2.639123454924323, - "y": 1.1446271693862373 + "x": 3.661009849273592, + "y": 1.8329545447689473 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.14162084164624, - "y": -0.14032985330927936 + "x": 8.290000000000001, + "y": 0.4 }, "prevControl": { - "x": 5.120084675408034, - "y": -0.14032985330927938 + "x": 5.268463833761794, + "y": 0.4 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Straight Line Turning.path b/src/main/deploy/pathplanner/paths/Straight Line Turning.path index 650f6895..16a158be 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Turning.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Turning.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path index 68db48a3..3b116f7c 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0f84573c..f085899c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -207,6 +207,7 @@ private void configureDriverBindings() { driver.getTopButton() .whileTrue(new SwerveDriveAutoFerry(driver)); + // .whileTrue(new SwerveDriveToShootMoving()); // climb driver.getRightButton() @@ -305,8 +306,8 @@ public void configureAutons() { AutonConfig TrackingCBAE = new AutonConfig("Tracking 5 CBAE Podium", FivePieceTrackingCBAE::new, "Preload to C", "C to B", "B to A", "A to E", "E to Shoot"); - AutonConfig CBAED = new AutonConfig("6 CBAED", SixPieceCBAED::new, - "Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); + AutonConfig CBAED = new AutonConfig("5 CBAE", SixPieceCBAED::new, + "Preload to C Close", "Close Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); AutonConfig CHGF = new AutonConfig("4.5 Piece CHGF", FivePieceCHGF::new, "Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)"); @@ -323,11 +324,11 @@ public void configureAutons() { // AutonConfig PodiumCloseCBAE = new AutonConfig("Podium Close 5 Piece CBAE", FivePiecePodiumForwardCBAE::new, // "Forward First Piece to C", "C to B 2", "B to A","A to E", "E to Shoot"); - HGF.registerBlue(autonChooser) + HGF.registerDefaultBlue(autonChooser) .registerRed(autonChooser); CBAED - .registerDefaultBlue(autonChooser) + .registerBlue(autonChooser) .registerRed(autonChooser); CHGF diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 50d66b63..9ba14b32 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -82,7 +82,7 @@ public AmpScoreRoutine() { new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT), new SwerveDriveDriveDirection( - new Vector2D(new Translation2d( + () -> new Vector2D(new Translation2d( Score.DRIVE_AWAY_SPEED, Field.getAllianceAmpTag().getLocation().toPose2d().getRotation()))) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java index 3cf5b4b7..185d5857 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java @@ -3,13 +3,16 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootNoIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.conveyor.ConveyorStop; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.constants.Settings.Auton; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -23,29 +26,36 @@ public SixPieceCBAED(PathPlannerPath... paths) { new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) .andThen(new ShooterPodiumShot()), - SwerveDriveToPose.speakerRelative(-15) - .withTolerance(0.1, 0.1, 3) + SwerveDrive.getInstance().followPathCommand(paths[0]) ), - new ConveyorShootRoutine(), - - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), - new ConveyorShootRoutine(), + new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), + new ConveyorShootNoIntake(), + new WaitCommand(0.55), + new ConveyorStop(), new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot(), + new SwerveDriveToShoot() + .withTolerance(0.05, 7), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot(), + new SwerveDriveToShoot() + .withTolerance(0.05, 7), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + new SwerveDriveToShoot() + .withTolerance(0.05, 5), + new ConveyorShootRoutine(), + + new FollowPathAndIntake(paths[4]), + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot() + .withTolerance(0.033, 7)), - new FollowPathAndIntake(paths[5]), - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) + new FollowPathAndIntake(paths[6]), + new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot() + .withTolerance(0.033, 7)) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java new file mode 100644 index 00000000..6b2eb089 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java @@ -0,0 +1,88 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.conveyor.ConveyorShoot; +import com.stuypulse.robot.commands.conveyor.ConveyorStop; +import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDerivative; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class FollowPathTrackingShoot extends SequentialCommandGroup { + + private final Odometry odometry; + + private final VStream robotPose; + private final VStream robotVelocity; + private final VStream projectedRobotPose; + + private double targetDistance; + private double distanceTolerance; + + public static boolean shouldShoot() { + Translation2d speaker = Field.getAllianceSpeakerPose().getTranslation(); + double distance = speaker + // .minus(projectedRobotPose.get().getTranslation2d()) + .minus(Odometry.getInstance().getPose().getTranslation()) + .getNorm(); + + // return Math.abs(targetDistance - distance) < distanceTolerance; + return distance > 2.25; + } + + public FollowPathTrackingShoot(PathPlannerPath path) { + odometry = Odometry.getInstance(); + + robotPose = VStream.create(() -> new Vector2D(odometry.getPose().getTranslation())); + + robotVelocity = robotPose + .filtered(new VDerivative()); + + projectedRobotPose = robotVelocity + .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) + .add(robotPose); + + distanceTolerance = 0.033; + targetDistance = Alignment.PODIUM_SHOT_DISTANCE.get(); + + addCommands( + new ShooterPodiumShot(), + new ShooterWaitForTarget(), + new ParallelCommandGroup( + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(path), + new SequentialCommandGroup( + new WaitUntilCommand(FollowPathTrackingShoot::shouldShoot), + new ConveyorShoot(), + new WaitCommand(Settings.Conveyor.SHOOT_WAIT_DELAY.get()), + new ConveyorStop(), + new IntakeStop() + ) + ) + ); + } + + public FollowPathTrackingShoot withTolerance(double distanceTolerance) { + this.distanceTolerance = distanceTolerance; + return this; + } + + public FollowPathTrackingShoot withTargetDistance(double target) { + this.targetDistance = target; + return this; + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index dd01cce4..700bafbc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -5,27 +5,41 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Auton; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class FourPieceHGF extends SequentialCommandGroup { public FourPieceHGF(PathPlannerPath... paths) { addCommands( - new ShooterPodiumShot(), + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterPodiumShot()), - new ShooterWaitForTarget() - .withTimeout(1.0), + SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.1, 0.1, 2) + ), + + new ShooterWaitForTarget(), new ConveyorShootRoutine(), + // new ShooterStop(), new FollowPathAndIntake(paths[0]), - new FollowPathAlignAndShoot(paths[1], SwerveDriveToShoot.withHigherDebounce()), + new FollowPathAlignAndShoot(paths[1], new SwerveDriveToShoot() + .withTolerance(0.06, 5)), new FollowPathAndIntake(paths[2]), - new FollowPathAlignAndShoot(paths[3], SwerveDriveToShoot.withHigherDebounce()), + new FollowPathAlignAndShoot(paths[3], new SwerveDriveToShoot() + .withTolerance(0.05, 5)), new FollowPathAndIntake(paths[4]), - new FollowPathAlignAndShoot(paths[5], SwerveDriveToShoot.withHigherDebounce()) + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootNoIntake.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootNoIntake.java new file mode 100644 index 00000000..4b4495ee --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootNoIntake.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.conveyor; + +import com.stuypulse.robot.subsystems.conveyor.Conveyor; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ConveyorShootNoIntake extends InstantCommand { + + private final Conveyor conveyor; + + public ConveyorShootNoIntake() { + conveyor = Conveyor.getInstance(); + + addRequirements(conveyor); + } + + @Override + public void initialize() { + conveyor.toShooter(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index d89a4d70..d8d25463 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -14,6 +14,8 @@ import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction; import com.stuypulse.stuylib.util.StopWatch; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; /** @@ -44,6 +46,10 @@ private LEDInstruction getInstruction() { && amper.isAtTargetHeight(0.15)) return LEDInstructions.GREEN; + if (DriverStation.isAutonomousEnabled() && SmartDashboard.getBoolean("A", false)) { + return LEDInstructions.GREEN; + } + if (intake.hasNote()) { return LEDInstructions.CONTAINS_NOTE; } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java index c2d0e612..c8d0eabe 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.vectors.VStream; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; @@ -10,9 +11,9 @@ public class SwerveDriveDriveDirection extends Command { private final SwerveDrive swerve; - private final Vector2D direction; + private final VStream direction; - public SwerveDriveDriveDirection(Vector2D direction) { + public SwerveDriveDriveDirection(VStream direction) { swerve = SwerveDrive.getInstance(); this.direction = direction; @@ -21,8 +22,8 @@ public SwerveDriveDriveDirection(Vector2D direction) { @Override public void execute() { swerve.setFieldRelativeSpeeds(new ChassisSpeeds( - direction.x, - direction.y, + direction.get().x, + direction.get().y, 0)); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java index 6f983fc6..c542c1fd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -14,6 +14,7 @@ import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AprilTagVision; +import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.math.Angle; @@ -24,6 +25,7 @@ import com.stuypulse.stuylib.streams.numbers.filters.Derivative; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -39,7 +41,7 @@ public static SwerveDriveToShoot withHigherDebounce() { private final SwerveDrive swerve; private final Odometry odometry; - private final PIDController distanceController; + private final Controller distanceController; private final AnglePIDController angleController; private final IStream velocityError; @@ -66,7 +68,11 @@ public SwerveDriveToShoot(Number targetDistance, double debounce) { swerve = SwerveDrive.getInstance(); odometry = Odometry.getInstance(); - distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD); + distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) + .setOutputFilter( + x -> -x, + x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) + ); angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); @@ -81,7 +87,7 @@ public SwerveDriveToShoot(Number targetDistance, double debounce) { isAligned = BStream.create(this::isAligned) .filtered(new BDebounceRC.Rising(debounce)); - distanceTolerance = 0.033; + distanceTolerance = 0.05; angleTolerance = Alignment.ANGLE_TOLERANCE.get(); velocityTolerance = 0.1; @@ -98,11 +104,6 @@ public SwerveDriveToShoot withTolerance(double distanceTolerance, double angleTo return this; } - public SwerveDriveToShoot withDistanceConstants(double p, double i, double d) { - distanceController.setPID(p, i, d); - return this; - } - public SwerveDriveToShoot withRotationConstants(double p, double i, double d) { angleController.setPID(p, i, d); return this; @@ -118,11 +119,16 @@ private Translation2d getTranslationToSpeaker() { return Field.getAllianceSpeakerPose().getTranslation().minus(odometry.getPose().getTranslation()); } + @Override + public void initialize() { + SmartDashboard.putBoolean("A", true); + } + @Override public void execute() { Rotation2d toSpeaker = getTranslationToSpeaker().getAngle(); - double speed = -distanceController.update(getTargetDistance(), distanceToSpeaker.get()); + double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); double rotation = angleController.update( Angle.fromRotation2d(toSpeaker).add(Angle.k180deg), Angle.fromRotation2d(odometry.getPose().getRotation())); @@ -152,5 +158,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { swerve.stop(); + SmartDashboard.putBoolean("A", false); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java new file mode 100644 index 00000000..ca65dfb7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java @@ -0,0 +1,175 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Alignment.Shoot; +import com.stuypulse.robot.subsystems.conveyor.Conveyor; +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.Controller; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VTimedMovingAverage; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +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; + +public class SwerveDriveToShootMoving extends Command { + + public static SwerveDriveToShootMoving withHigherDebounce() { + return new SwerveDriveToShootMoving(Alignment.PODIUM_SHOT_DISTANCE, 0.75); + } + + private final SwerveDrive swerve; + private final Odometry odometry; + private final Conveyor conveyor; + private final Intake intake; + + private final Controller distanceController; + private final AnglePIDController angleController; + + private final IStream distanceToSpeaker; + private final BStream isAligned; + + private final VStream robotPose; + private final VStream projectedRobotPose; + + private final Number targetDistance; + + private double distanceTolerance; + private double angleTolerance; + + public SwerveDriveToShootMoving() { + this(Alignment.PODIUM_SHOT_DISTANCE); + } + + public SwerveDriveToShootMoving(Number targetDistance) { + this(targetDistance, 0.1); + } + + public SwerveDriveToShootMoving(Number targetDistance, double debounce) { + this.targetDistance = targetDistance; + + swerve = SwerveDrive.getInstance(); + odometry = Odometry.getInstance(); + conveyor = Conveyor.getInstance(); + intake = Intake.getInstance(); + + distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) + .setOutputFilter( + x -> -x, + x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) + ); + + angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); + + robotPose = VStream.create(() -> new Vector2D(odometry.getPose().getTranslation())); + + projectedRobotPose = VStream.create(() -> new Vector2D(odometry.getRobotVelocity())) + .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) + .add(robotPose) + .filtered( + new VTimedMovingAverage(0.1), + new VLowPassFilter(0.1) + ); + + distanceToSpeaker = IStream.create(() -> Field.getAllianceSpeakerPose().getTranslation().minus(projectedRobotPose.get().getTranslation2d()).getNorm()); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Both(debounce)); + + distanceTolerance = 0.08; + angleTolerance = Alignment.ANGLE_TOLERANCE.get(); + + addRequirements(swerve); + } + + private double getTargetDistance() { + return SLMath.clamp(targetDistance.doubleValue(), 1, 5); + } + + public SwerveDriveToShootMoving withTolerance(double distanceTolerance, double angleTolerance) { + this.distanceTolerance = distanceTolerance; + this.angleTolerance = angleTolerance; + return this; + } + + public SwerveDriveToShootMoving withRotationConstants(double p, double i, double d) { + angleController.setPID(p, i, d); + return this; + } + + private boolean isAligned() { + return distanceController.isDone(distanceTolerance) + && angleController.isDoneDegrees(angleTolerance); + } + + @Override + public void execute() { + Odometry.getInstance().getField().getObject("Projected Pose") + .setPose(new Pose2d(projectedRobotPose.get().getTranslation2d(), odometry.getPose().getRotation())); + + Rotation2d toSpeaker = Field.getAllianceSpeakerPose().getTranslation() + .minus(projectedRobotPose.get().getTranslation2d()) + .getAngle(); + + double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); + double rotation = angleController.update( + Angle.fromRotation2d(toSpeaker).add(Angle.k180deg), + Angle.fromRotation2d(odometry.getPose().getRotation())); + + Translation2d speeds = new Translation2d( + speed, + toSpeaker); + + if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + swerve.setFieldRelativeSpeeds( + new ChassisSpeeds( + speeds.getX(), + speeds.getY(), + rotation)); + + if (isAligned.get()) { + conveyor.toShooter(); + intake.acquire(); + } else { + conveyor.stop(); + intake.stop(); + } + + SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.plus(Rotation2d.fromDegrees(180)).getDegrees()); + SmartDashboard.putNumber("Alignment/Distance Error", distanceController.getError()); + SmartDashboard.putNumber("Alignment/Distance Output", distanceController.getOutput()); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + intake.stop(); + conveyor.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index 9b473ef0..1fa0d331 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -68,7 +68,7 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction INTAKE = new LEDPulseColor(SLColor.RED); + LEDInstruction INTAKE = new LEDPulseColor(SLColor.BLUE); LEDInstruction PICKUP = new LEDPulseColor(SLColor.RED); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index bc13eff3..188aed3f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -329,10 +329,10 @@ public interface Shooter { 500, new SmartNumber("Shooter/Ferry Feeder RPM", 3000)); - double AT_RPM_EPSILON = 125; + double AT_RPM_EPSILON = 200; SmartNumber RPM_CHANGE_RC = new SmartNumber("Shooter/RPM Change RC", 0.2); - double RPM_CHANGE_DIP_THRESHOLD = 250; + double RPM_CHANGE_DIP_THRESHOLD = 300; public interface Feedforward { double kS = 0.11873; @@ -342,7 +342,7 @@ public interface Feedforward { public interface PID { double kP = 0.00034711; - double kI = 0.0; + double kI = 0; double kD = 0.0; } } @@ -354,7 +354,7 @@ public interface Feeder { public interface Feedforward { double kS = 0.71611; - double kV = 0.0032; + double kV = 0.00335; double kA = 0.076981; } @@ -385,13 +385,13 @@ public interface Conveyor { SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.0); SmartNumber RECALL_DEBOUNCE = new SmartNumber("Conveyor/Recall Delay", 1.0); - SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/Shoot Wait Delay", 0.35); + SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/Shoot Wait Delay", 0.45); SmartNumber AT_FEEDER_WAIT_DELAY = new SmartNumber("Conveyor/At Feeder Wait Delay", 0.5); } public interface Alignment { - double DEBOUNCE_TIME = 0.2; + double DEBOUNCE_TIME = 0.05; SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); @@ -408,6 +408,10 @@ public interface Alignment { SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Trap/Into Chain Speed", 0.25); + double NOTE_TO_GOAL_TIME = 0.4; + + double MAX_ALIGNMENT_SPEED = 2.5; + public interface Translation { SmartNumber kP = new SmartNumber("Alignment/Translation/kP", 5.0); SmartNumber kI = new SmartNumber("Alignment/Translation/kI", 0.0); @@ -415,22 +419,22 @@ public interface Translation { } public interface Rotation { - SmartNumber kP = new SmartNumber("Alignment/Rotation/kP", 6.0); + SmartNumber kP = new SmartNumber("Alignment/Rotation/kP", 4.0); SmartNumber kI = new SmartNumber("Alignment/Rotation/kI", 0.0); SmartNumber kD = new SmartNumber("Alignment/Rotation/kD", 0.0); } public interface Shoot { public interface Translation { - SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 4.0); + SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.05); + SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.8); } public interface Rotation { - SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 8.0); + SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.0); + SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java index 164bca11..507dc03c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java @@ -166,6 +166,10 @@ public void periodic() { voltage = 0; } + if (getTargetHeight() == Settings.Amper.Lift.MIN_HEIGHT && voltage > 0) { + voltage = 0; + } + if (getTargetHeight() == Settings.Amper.Lift.TRAP_SCORE_HEIGHT && voltage < 0.75) { voltage = 0.75; } diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index 6f9ea045..300f2972 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -9,6 +9,7 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.util.LinearRegression; @@ -57,6 +58,9 @@ public static Odometry getInstance() { private Pose2d lastGoodPose; + private Translation2d robotVelocity; + private Translation2d lastPose; + protected Odometry() { SwerveDrive swerve = SwerveDrive.getInstance(); odometry = new SwerveDriveOdometry( @@ -89,6 +93,9 @@ protected Odometry() { thetaRegression = new LinearRegression(Cameras.thetaStdDevs); lastGoodPose = new Pose2d(); + + robotVelocity = new Translation2d(); + lastPose = new Translation2d(); SmartDashboard.putData("Field", field); } @@ -194,9 +201,16 @@ public void periodic() { lastGoodPose = getPose(); } + robotVelocity = getPose().getTranslation().minus(lastPose).div(Settings.DT); + lastPose = getPose().getTranslation(); + updateTelemetry(); } + public Translation2d getRobotVelocity() { + return robotVelocity; + } + private void updateTelemetry() { SmartDashboard.putNumber("Odometry/Odometry/X", odometry.getPoseMeters().getTranslation().getX()); SmartDashboard.putNumber("Odometry/Odometry/Y", odometry.getPoseMeters().getTranslation().getY()); @@ -204,6 +218,8 @@ private void updateTelemetry() { SmartDashboard.putNumber("Odometry/Estimator/X", estimator.getEstimatedPosition().getTranslation().getX()); SmartDashboard.putNumber("Odometry/Estimator/Y", estimator.getEstimatedPosition().getTranslation().getY()); + SmartDashboard.putNumber("Odometry/Estimator/VX", robotVelocity.getX()); + SmartDashboard.putNumber("Odometry/Estimator/VY", robotVelocity.getY()); SmartDashboard.putNumber("Odometry/Estimator/Rotation", estimator.getEstimatedPosition().getRotation().getDegrees()); odometryPose2D.setPose(odometry.getPoseMeters()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 97eff0aa..0908e09b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -60,11 +60,14 @@ protected ShooterImpl() { feederEncoder.setVelocityConversionFactor(Feeder.GEARING); leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() - .add(new PIDController(PID.kP, PID.kI, PID.kD)); + .add(new PIDController(PID.kP, PID.kI, PID.kD) + .setIntegratorFilter(1000, 0.5 / PID.kI)); rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() - .add(new PIDController(PID.kP, PID.kI, PID.kD)); + .add(new PIDController(PID.kP, PID.kI, PID.kD) + .setIntegratorFilter(1000, 0.5 / PID.kI)); feederController = new MotorFeedforward(Feeder.Feedforward.kS, Feeder.Feedforward.kV, Feeder.Feedforward.kA).velocity() - .add(new PIDController(Feeder.PID.kP, Feeder.PID.kI, Feeder.PID.kD)); + .add(new PIDController(Feeder.PID.kP, Feeder.PID.kI, Feeder.PID.kD) + .setIntegratorFilter(1000, 1.0 / PID.kI)); rpmChange = IStream.create(this::getAverageShooterRPM) .filtered(new HighPassFilter(Settings.Shooter.RPM_CHANGE_RC));