diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 1ea565f..7d55eed 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -2,8 +2,16 @@ "robotWidth": 0.4723384, "robotLength": 0.4631436, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "Left Paths", + "Middle Paths", + "Right Paths" + ], + "autoFolders": [ + "Left", + "Middle", + "Right" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, diff --git a/.vscode/settings.json b/.vscode/settings.json index 22b7cc8..4ed39e9 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,5 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx32G -Xms100m -Xlog:disable" } diff --git a/src/main/deploy/pathplanner/autos/ Four Note Auto.auto b/src/main/deploy/pathplanner/autos/ Four Note Auto.auto new file mode 100644 index 0000000..0422286 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ Four Note Auto.auto @@ -0,0 +1,175 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Start" + } + }, + { + "type": "named", + "data": { + "name": "Intake Down" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Leave Subwoofer" + } + }, + { + "type": "named", + "data": { + "name": "Rollers In" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Up" + } + }, + { + "type": "named", + "data": { + "name": "Apriltag Align" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Rollers Out" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Subwoofer-Right" + } + }, + { + "type": "named", + "data": { + "name": "Rollers In" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake Down" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Up" + } + }, + { + "type": "named", + "data": { + "name": "Apriltag Align" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Rollers Out" + } + }, + { + "type": "named", + "data": { + "name": "Intake Down" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Subwoofer-Left" + } + }, + { + "type": "named", + "data": { + "name": "Rollers In" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Up" + } + }, + { + "type": "named", + "data": { + "name": "Apriltag Align" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Rollers Out" + } + } + ] + } + }, + "folder": "Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Note Auto.auto b/src/main/deploy/pathplanner/autos/Left 2 Note Auto.auto new file mode 100644 index 0000000..db880b2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Note Auto.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6933452953924437, + "y": 6.569779302183426 + }, + "rotation": 57.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Start" + } + }, + { + "type": "named", + "data": { + "name": "Intake Down" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Leave Left" + } + }, + { + "type": "named", + "data": { + "name": "Rollers In" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left-Subwoofer" + } + }, + { + "type": "named", + "data": { + "name": "Intake Up" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Rollers Out" + } + } + ] + } + }, + "folder": "Left", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right Two Note.auto b/src/main/deploy/pathplanner/autos/Right Two Note.auto new file mode 100644 index 0000000..cea64c5 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right Two Note.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": "Right", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootLeave.auto b/src/main/deploy/pathplanner/autos/ShootLeave.auto new file mode 100644 index 0000000..994f12f --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootLeave.auto @@ -0,0 +1,31 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6, + "y": 4.5 + }, + "rotation": -60.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Speaker" + } + }, + { + "type": "path", + "data": { + "pathName": "Leave Zone Side" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootPickup.auto b/src/main/deploy/pathplanner/autos/ShootPickup.auto new file mode 100644 index 0000000..9773530 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootPickup.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.1, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Speaker" + } + }, + { + "type": "named", + "data": { + "name": "Intake Down" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMiddleNote" + } + }, + { + "type": "named", + "data": { + "name": "Rollers In" + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Two Note Auto.auto b/src/main/deploy/pathplanner/autos/Two Note Auto.auto new file mode 100644 index 0000000..1436c52 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Two Note Auto.auto @@ -0,0 +1,75 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.15, + "y": 5.55 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Auto Start" + } + }, + { + "type": "named", + "data": { + "name": "Intake Down" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ToMiddleNote" + } + }, + { + "type": "named", + "data": { + "name": "Rollers In" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake Up" + } + }, + { + "type": "named", + "data": { + "name": "Apriltag Align" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Feed Note" + } + } + ] + } + }, + "folder": "Middle", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1M Leave Subwoofer.path b/src/main/deploy/pathplanner/paths/A1M Leave Subwoofer.path new file mode 100644 index 0000000..b304fda --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A1M Leave Subwoofer.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1126849299961952, + "y": 5.55076982040085 + }, + "prevControl": null, + "nextControl": { + "x": 2.112684929996195, + "y": 5.55076982040085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 5.55076982040085 + }, + "prevControl": { + "x": 1.9, + "y": 5.55076982040085 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1M To Subwoofer.path b/src/main/deploy/pathplanner/paths/A1M To Subwoofer.path new file mode 100644 index 0000000..d1a1ac6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A1M To Subwoofer.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9045123807922586, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.2909289603721312, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2, + "y": 5.52155524239874 + }, + "prevControl": { + "x": 1.4339194150520078, + "y": 5.52155524239874 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A2M Left-Subwoofer.path b/src/main/deploy/pathplanner/paths/A2M Left-Subwoofer.path new file mode 100644 index 0000000..1a7fd8b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A2M Left-Subwoofer.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 4.1 + }, + "prevControl": null, + "nextControl": { + "x": 1.7846202240495885, + "y": 4.090040920295366 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2, + "y": 5.55 + }, + "prevControl": { + "x": 1.7819073875449822, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A2M Right-Subwoofer.path b/src/main/deploy/pathplanner/paths/A2M Right-Subwoofer.path new file mode 100644 index 0000000..1bf9f52 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A2M Right-Subwoofer.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.89, + "y": 6.99 + }, + "prevControl": null, + "nextControl": { + "x": 3.8899999999999997, + "y": 6.99 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2, + "y": 5.55 + }, + "prevControl": { + "x": 3.099571710686488, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Leave Left.path b/src/main/deploy/pathplanner/paths/Leave Left.path new file mode 100644 index 0000000..8dfa573 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Leave Left.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.69, + "y": 6.57 + }, + "prevControl": null, + "nextControl": { + "x": 1.0563812283116707, + "y": 7.249007821838753 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.918404239090931, + "y": 7.038212764014687 + }, + "prevControl": { + "x": 1.9229831326995024, + "y": 7.073345273652031 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.25, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Left Paths", + "previewStartingState": { + "rotation": 57.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Leave Right.path b/src/main/deploy/pathplanner/paths/Leave Right.path new file mode 100644 index 0000000..ba62d26 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Leave Right.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6699236223008809, + "y": 4.543804579763226 + }, + "prevControl": null, + "nextControl": { + "x": 1.6699236223008804, + "y": 4.543804579763226 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8481392198162414, + "y": 4.098792791023527 + }, + "prevControl": { + "x": 1.8481392198162414, + "y": 4.098792791023527 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": 0.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Right Paths", + "previewStartingState": { + "rotation": -57.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Leave Subwoofer.path b/src/main/deploy/pathplanner/paths/Leave Subwoofer.path new file mode 100644 index 0000000..ae09e4d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Leave Subwoofer.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2, + "y": 5.55076982040085 + }, + "prevControl": null, + "nextControl": { + "x": 2.1999999999999997, + "y": 5.55076982040085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 5.55076982040085 + }, + "prevControl": { + "x": 1.9, + "y": 5.55076982040085 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Leave Zone Side.path b/src/main/deploy/pathplanner/paths/Leave Zone Side.path new file mode 100644 index 0000000..1fc32b4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Leave Zone Side.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6, + "y": 4.5 + }, + "prevControl": null, + "nextControl": { + "x": 1.364463269462635, + "y": 3.04150734427894 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4167873741108443, + "y": 1.6178230554689212 + }, + "prevControl": { + "x": 2.168014807489211, + "y": 2.0402020118262376 + }, + "nextControl": { + "x": 4.674067005787224, + "y": 1.1925667094607344 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.687840240540902, + "y": 0.7395762539302737 + }, + "prevControl": { + "x": 6.569231156475887, + "y": 0.7395762539302737 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -60.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left-Subwoofer.path b/src/main/deploy/pathplanner/paths/Left-Subwoofer.path new file mode 100644 index 0000000..210eeaf --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left-Subwoofer.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.92, + "y": 7.04 + }, + "prevControl": null, + "nextControl": { + "x": 2.0166698250657547, + "y": 6.991369417831561 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.69, + "y": 6.57 + }, + "prevControl": { + "x": 1.3374413054104268, + "y": 6.698598504187022 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 56.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Left Paths", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftNoteScore.path b/src/main/deploy/pathplanner/paths/LeftNoteScore.path new file mode 100644 index 0000000..5a03bc4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftNoteScore.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.1595077424389078, + "y": 6.388089893296904 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1, + "y": 5.55 + }, + "prevControl": { + "x": 1.7619855059529936, + "y": 6.064525282203719 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleNoteScore.path b/src/main/deploy/pathplanner/paths/MiddleNoteScore.path new file mode 100644 index 0000000..272c53f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MiddleNoteScore.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 1.9, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1, + "y": 5.55 + }, + "prevControl": { + "x": 2.1, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right-Subwoofer.path b/src/main/deploy/pathplanner/paths/Right-Subwoofer.path new file mode 100644 index 0000000..0dbba9f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right-Subwoofer.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.85, + "y": 4.1 + }, + "prevControl": null, + "nextControl": { + "x": 1.8500000000000005, + "y": 4.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.67, + "y": 4.54 + }, + "prevControl": { + "x": 1.67, + "y": 4.54 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": -57.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -57.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Right Paths", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightNoteScore.path b/src/main/deploy/pathplanner/paths/RightNoteScore.path new file mode 100644 index 0000000..67aef9c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightNoteScore.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.9, + "y": 4.1 + }, + "prevControl": null, + "nextControl": { + "x": 2.325912399572547, + "y": 4.548393961652788 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1, + "y": 5.55 + }, + "prevControl": { + "x": 1.6510490678639014, + "y": 5.084586745750069 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Subwoofer-Left.path b/src/main/deploy/pathplanner/paths/Subwoofer-Left.path new file mode 100644 index 0000000..401c8ee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Subwoofer-Left.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2, + "y": 5.55076982040085 + }, + "prevControl": null, + "nextControl": { + "x": 2.1999999999999997, + "y": 5.55076982040085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9045123807922586, + "y": 4.099779112962734 + }, + "prevControl": { + "x": 0.9666120399856463, + "y": 4.021873571623774 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Subwoofer-Right.path b/src/main/deploy/pathplanner/paths/Subwoofer-Right.path new file mode 100644 index 0000000..7158b04 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Subwoofer-Right.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2, + "y": 5.55076982040085 + }, + "prevControl": null, + "nextControl": { + "x": 2.1999999999999997, + "y": 5.55076982040085 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.885035995457519, + "y": 6.9920223351715975 + }, + "prevControl": { + "x": 0.9854642847710309, + "y": 6.9920223351715975 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Middle Paths", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToLeftNote.path b/src/main/deploy/pathplanner/paths/ToLeftNote.path new file mode 100644 index 0000000..da2fe41 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ToLeftNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 1.4753997075561718, + "y": 6.6561862853455445 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 7.0 + }, + "prevControl": { + "x": 1.9653689757829964, + "y": 7.025974412309185 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToMiddleNote.path b/src/main/deploy/pathplanner/paths/ToMiddleNote.path new file mode 100644 index 0000000..50a5d10 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ToMiddleNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 2.1, + "y": 5.55 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": { + "x": 1.9, + "y": 5.55 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ToRightNote.path b/src/main/deploy/pathplanner/paths/ToRightNote.path new file mode 100644 index 0000000..9a80ebc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ToRightNote.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.1, + "y": 5.55 + }, + "prevControl": null, + "nextControl": { + "x": 1.530867926600718, + "y": 4.687064509264155 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8990839963661905, + "y": 4.1 + }, + "prevControl": { + "x": 1.8990839963661905, + "y": 4.1 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 604813f..640c381 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.revrobotics.CANSparkBase.IdleMode; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Rotation2d; @@ -15,7 +16,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import frc.robot.subsystems.SwerveLib.COTSTalonFXSwerveConstants; import frc.robot.subsystems.SwerveLib.SwerveModuleConstants; /** @@ -30,19 +30,14 @@ public final class Constants { public static final class SwerveConstants { public static final double stickDeadband = 0.1; - public static final COTSTalonFXSwerveConstants chosenModule = - COTSTalonFXSwerveConstants.SDS.MK4i.KrakenX60(COTSTalonFXSwerveConstants.SDS.MK4i.driveRatios.L2); - - /* Drivetrain Constants */ - public static final double trackWidth = Units.inchesToMeters(18.596); - public static final double wheelBase = Units.inchesToMeters(18.234); - public static final double wheelDiameter = Units.inchesToMeters(4.0); + public static final double trackWidth = Units.inchesToMeters(27); + public static final double wheelBase = Units.inchesToMeters(27); + public static final double wheelDiameter = Units.inchesToMeters(3.75); public static final double wheelCircumference = wheelDiameter * Math.PI; - public static final double driveGearRatio = chosenModule.driveGearRatio; // 6.75:1 - //NOTE: the angle gear ratio provided by the manufactor is different than the one we had down previously... might need to change it - public static final double angleGearRatio = chosenModule.angleGearRatio; // 12.8:1 (150.0 / 7.0); + public static final double driveGearRatio = (6.75 / 1.0); // 6.75:1 + public static final double angleGearRatio = (150.0 / 7.0); // 12.8:1 public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics( @@ -55,20 +50,13 @@ public static final class SwerveConstants { public static final double voltageComp = 12.0; /* Swerve Current Limiting */ - public static final int angleContinuousCurrentLimit = 35; - public static final int angleCurrentThreshold = 40; - public static final double angleCurrentThresholdTime = 0.1; - public static final boolean angleEnableCurrentLimit = true; - - public static final int driveContinuousCurrentLimit = 35; //original threshold: 80 - public static final int driveCurrentThreshold = 60; - public static final double driveCurrentThresholdTime = 0.1; - public static final boolean driveEnableCurrentLimit = true; + public static final int angleContinuousCurrentLimit = 20; + public static final int driveContinuousCurrentLimit = 80; /* Angle Motor PID Values */ - public static final double angleKP = chosenModule.angleKP; - public static final double angleKI = chosenModule.angleKI; - public static final double angleKD = chosenModule.angleKD; + public static final double angleKP = 0.01; + public static final double angleKI = 0.0; + public static final double angleKD = 0.0; public static final double angleKFF = 0.0; /* Drive Motor PID Values */ @@ -78,8 +66,8 @@ public static final class SwerveConstants { public static final double driveKFF = 0.0; /* Drive Motor Characterization Values */ - public static final double driveKS = 0.17972; - public static final double driveKV = 7.50715; //2.74490 + public static final double driveKS = 0.58453; //0.22005 + public static final double driveKV = 2.40269; //2.74490 public static final double driveKA = 0; //have to tune manually @@ -95,23 +83,20 @@ public static final class SwerveConstants { /* Swerve Profiling Values */ public static final double maxSpeed = 4.5; // meters per second - public static final double maxAngularVelocity = 11.5; + public static final double maxAngularVelocity = Math.PI; public static final double maxAcceleration = 1; - public static final double maxAngularAcceleration = Math.PI; - - public static final double openLoopRamp = 0.25; - public static final double closedLoopRamp = 0.0; + public static final double maxAngularAcceleration = Math.PI/6; - /* Neutral Modes */ - public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Brake; - public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake; + /* Neutral Modes */ + public static final IdleMode angleNeutralMode = IdleMode.kBrake; + public static final IdleMode driveNeutralMode = IdleMode.kBrake; /* Motor Inverts */ - public static final InvertedValue driveInvert = chosenModule.driveMotorInvert; //counterclockwise - public static final InvertedValue angleInvert = chosenModule.angleMotorInvert; //counterclockwise + public static final boolean driveInvert = false; + public static final boolean angleInvert = true; /* Angle Encoder Invert */ - public static final SensorDirectionValue cancoderInvert = chosenModule.cancoderInvert; //counterclockwise + public static final boolean canCoderInvert = false; /* Module Specific Constants */ /* @@ -126,7 +111,7 @@ public static final class Mod0 { public static final int driveMotorID = 2; public static final int angleMotorID = 3; public static final int canCoderID = 0; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(143 - 45); //43 + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(223); //43 public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } @@ -136,7 +121,7 @@ public static final class Mod1 { public static final int driveMotorID = 8; public static final int angleMotorID = 1; public static final int canCoderID = 1; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(84); //344 + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(344); //344 public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } @@ -146,7 +131,7 @@ public static final class Mod2 { public static final int driveMotorID = 4; public static final int angleMotorID = 5; public static final int canCoderID = 2; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(324); //282 + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(87); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } @@ -156,16 +141,20 @@ public static final class Mod3 { public static final int driveMotorID = 6; public static final int angleMotorID = 7; public static final int canCoderID = 3; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(298); + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(73.5); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } + } + public static class OperatorConstants { + public static final int kDriverControllerPort = 0; + public static int kGunnerControllerPort = 1; } - public static class VisionConstants{ - public static String cameraNameAprilTag = "Back"; + public static String cameraNameAprilTagFront = "Front"; + public static String cameraNameAprilTagBack = "Back"; public static String cameraNameObject = "Front"; //constraints @@ -192,23 +181,19 @@ public static class VisionConstants{ public static final Transform3d CAMERA_TO_Robot = new Transform3d(); } - - public static class OperatorConstants { - public static final int kGunnerControllerPort = 1; - public static final int kDriverControllerPort = 0; - } // Intake constants public static class IntakeConstants{ public static final int pivotID = 9; public static final int rollerID = 10; - public static final double intakeUpSpeed = 0.5; - public static final double intakeDownSpeed = -0.5; + public static final double intakeUpSpeed = 0.3; + public static final double intakeDownSpeed = -0.3; public static final double rollerSpeedIntake = 0.5; - public static double rollerSpeedOuttake = 0.4; + public static double rollerSpeedOuttake = 0.7; public static final int IRport = 0; } + // Shooter constants public static class ShooterConstants{ public static final String leftSpeedKey = "Left Speed"; @@ -216,8 +201,8 @@ public static class ShooterConstants{ public static final int shooterLeftID = 11; public static final int shooterRightID = 12; - public static double shooterLeftSpeedSpeaker = -.65; - public static double shooterRightSpeedSpeaker = .70; //0.9 + public static double shooterLeftSpeedSpeaker = -.3; + public static double shooterRightSpeedSpeaker = .35; //0.9 public static double shooterLeftSpeedAmp = -0.3; public static double shooterRightSpeedAmp = 0.3; } @@ -225,7 +210,7 @@ public static class ShooterConstants{ public static class ClimbConstants { public static final int climbLeftID = 13; public static final int climbRightID = 14; - public static final double climbLeftSpeed = -0.6; - public static final double climbRightSpeed = 0.6; + public static final double climbLeftSpeed = 0.3; + public static final double climbRightSpeed = -0.3; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b9c9b68..7843dfc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.subsystems.SwerveLib.CTREConfigs; +import frc.robot.util.ShuffleboardTabs; /* * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -22,6 +23,7 @@ public class Robot extends TimedRobot { public static CTREConfigs ctreConfigs; private Command m_autonomousCommand; private RobotContainer m_robotContainer; + private ShuffleboardTabs shuffleboard_tabs; public static Alliance initAllianceColor = Alliance.Blue; @@ -45,6 +47,8 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + shuffleboard_tabs = new ShuffleboardTabs(); + shuffleboard_tabs.initButton(); if (!Preferences.containsKey(Constants.ShooterConstants.leftSpeedKey)){ Preferences.setDouble(Constants.ShooterConstants.leftSpeedKey, Constants.ShooterConstants.shooterLeftSpeedSpeaker); } @@ -62,6 +66,7 @@ public void robotInit() { */ @Override public void robotPeriodic() { + shuffleboard_tabs.updateButtons(); checkDSUpdate(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5158ff5..5af0f10 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,8 +3,15 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot; +import java.util.List; + import javax.swing.SpinnerDateModel; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; + import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -14,15 +21,15 @@ import frc.robot.commands.DriveCommands.DefaultDrive; import frc.robot.commands.DriveCommands.FieldOrientedDrive; import frc.robot.commands.DriveCommands.LeaveZone; -import frc.robot.commands.DriveCommands.TurnWhenLeft; -import frc.robot.commands.DriveCommands.TurnWhenRight; import frc.robot.commands.IntakeCommands.AutoIntake; +import frc.robot.commands.IntakeCommands.FeedNote; import frc.robot.commands.IntakeCommands.IntakeDown; import frc.robot.commands.IntakeCommands.IntakeUp; import frc.robot.commands.IntakeCommands.MoveRollersIn; import frc.robot.commands.IntakeCommands.MoveRollersOut; import frc.robot.commands.ShooterCommands.AutoShootAmp; import frc.robot.commands.ShooterCommands.AutoShootSpeaker; +import frc.robot.commands.ShooterCommands.AutoShootStart; import frc.robot.commands.ShooterCommands.ShootNoteAmp; import frc.robot.commands.ShooterCommands.ShootNoteSpeaker; import frc.robot.commands.ShooterCommands.ShooterLeftMotor; @@ -31,15 +38,16 @@ import frc.robot.commands.ShooterCommands.SpeedUpSpeaker; import frc.robot.commands.TrajectoryCommands.AlignAmp; import frc.robot.commands.TrajectoryCommands.AlignSpeaker; +import frc.robot.commands.TrajectoryCommands.AlignSpeakerManual; import frc.robot.commands.TrajectoryCommands.FollowNote; import frc.robot.commands.TrajectoryCommands.MoveToNote; import frc.robot.commands.TrajectoryCommands.RunOnTheFly; import frc.robot.commands.TrajectoryCommands.TrajectoryCreation; import frc.robot.Controls.ControlMap; import frc.robot.commands.CharacterizationCommands.FeedForwardCharacterization; +import frc.robot.commands.CharacterizationCommands.forwardMeter; import frc.robot.commands.CharacterizationCommands.FeedForwardCharacterization.FeedForwardCharacterizationData; -import frc.robot.commands.ClimbCommands.CilmbDown; -import frc.robot.commands.ClimbCommands.ClimbUp; +import frc.robot.commands.ClimbCommands.ClimbTeleop; import frc.robot.subsystems.Climb; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Intake; @@ -47,6 +55,7 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.TrajectoryConfiguration; import frc.robot.subsystems.Vision; +import frc.robot.util.PathPlannerUtil; public class RobotContainer { //Subsystem declerations @@ -66,8 +75,8 @@ public class RobotContainer { private final AlignSpeaker m_alignSpeaker = new AlignSpeaker(m_poseEstimator, m_traj, m_vision); private final MoveToNote m_justMove = new MoveToNote(m_drivetrain, m_vision); private final LeaveZone m_leaveZone = new LeaveZone(m_drivetrain); - private final TurnWhenLeft m_TurnWhenLeft = new TurnWhenLeft(m_drivetrain); - private final TurnWhenRight m_TurnWhenRight = new TurnWhenRight(m_drivetrain); + private final forwardMeter m_forwardMeter = new forwardMeter(m_drivetrain, m_poseEstimator, m_traj, m_vision, 0); + private final AlignSpeakerManual m_manualAlign = new AlignSpeakerManual(m_drivetrain, m_vision); // Drive command private final DefaultDrive m_defaultDrive = new DefaultDrive( m_drivetrain, @@ -89,12 +98,15 @@ public class RobotContainer { private final SpeedUpSpeaker m_speedUpSpeaker = new SpeedUpSpeaker(m_shooter); private final SpeedUpAmp m_speedUpAmp = new SpeedUpAmp(m_shooter); private final AutoIntake autoIntake = new AutoIntake(m_drivetrain, m_vision, m_intake); - private final ClimbUp m_climbUp = new ClimbUp(m_climb); - private final CilmbDown m_climbDown = new CilmbDown(m_climb); + private final ClimbTeleop m_climbUp = new ClimbTeleop(m_climb); + private final AutoShootStart m_autoStart = new AutoShootStart(m_shooter, m_intake); + private final FeedNote m_feedNote = new FeedNote(m_intake); //Drive subsystems declarations private final SendableChooser m_chooser = new SendableChooser<>(); + + // Speaker auto command // private final Command scoreSpeaker = new SequentialCommandGroup( // new ParallelCommandGroup(m_alignSpeaker).alongWith(m_speedUpSpeaker) @@ -115,41 +127,29 @@ public class RobotContainer { // m_autoShootSpeaker // .andThen(m_trajectoryConfig.followPathGui("Leave Zone Subwoofer")) // ); - // private final SequentialCommandGroup shootandTurnLeft = new SequentialCommandGroup( - // m_autoShootSpeaker - // .andThen(m_TurnWhenLeft) - // .andThen(m_leaveZone) - // ); - - // private final SequentialCommandGroup shootAndTurnRight = new SequentialCommandGroup( - // m_autoShootSpeaker - // .andThen(m_TurnWhenRight) - // .andThen(m_leaveZone) - // ); - - private boolean isFieldOriented = true; public RobotContainer() { + configureAutoBuilderCommands(); // Configure the trigger bindings configureButtonBindings(); configureShuffleBoardBindings(); configureDefaultCommands(); + } private void configureShuffleBoardBindings(){ m_chooser.addOption("Run on Fly", m_runOnTheFly); m_chooser.addOption("Move to Note", m_moveToNote); m_chooser.addOption("Leave Zone", m_leaveZone); - m_chooser.addOption("Turn when Left (TESTING ONLY)", m_TurnWhenLeft); - - //m_chooser.addOption("Score Speaker", scoreSpeaker); + m_chooser.addOption("forward Meter", m_forwardMeter); //m_chooser.addOption("Score Amp", scoreAmp); //m_chooser.addOption("Auto Intake", autoIntake); m_chooser.addOption("auto speaker", m_autoShootSpeaker); m_chooser.addOption("auto amp", m_autoShootAmp); m_chooser.addOption("align speaker", m_alignSpeaker); + m_chooser.addOption("Align Manual", m_manualAlign); // m_chooser.addOption("Leave Starting Zone Subwoofer", m_trajectoryConfig.followPathGui("Leave Zone Subwoofer")); // m_chooser.addOption("Score and Leave", scoreAndLeave); m_chooser.addOption("Swerve Characterization", new FeedForwardCharacterization( @@ -158,6 +158,14 @@ private void configureShuffleBoardBindings(){ new FeedForwardCharacterizationData("drive"), m_drivetrain::runCharacterizationVolts, m_drivetrain::getCharacterizationVelocity)); + + List autos = PathPlannerUtil.getExistingPaths(); + for (String auto : autos) { + m_chooser.addOption(auto, AutoBuilder.buildAuto(auto)); + } + + + SmartDashboard.putData(m_chooser); } @@ -167,16 +175,15 @@ private void configureButtonBindings() { ControlMap.m_driverController.leftBumper().onTrue(new InstantCommand(() -> m_drivetrain.zeroGyro())); ControlMap.m_driverController.b().toggleOnTrue(m_defaultDrive); ControlMap.m_driverController.a().onTrue(m_alignSpeaker); + ControlMap.m_driverController.x().onTrue(m_alignAmp); // Gunner button bindings ControlMap.m_gunnerController.a().whileTrue(m_intakeDown); ControlMap.m_gunnerController.b().whileTrue(m_intakeUp); - ControlMap.m_gunnerController.leftBumper().whileTrue(m_moveRollersOut); - ControlMap.m_gunnerController.rightBumper().whileTrue(m_moveRollersIn); + ControlMap.m_gunnerController.y().whileTrue(m_moveRollersOut); + ControlMap.m_gunnerController.x().whileTrue(m_moveRollersIn); ControlMap.m_gunnerController.leftTrigger().whileTrue(m_shootAmp); - ControlMap.m_gunnerController.rightTrigger().whileTrue(m_shootSpeaker); - ControlMap.m_gunnerController.y().whileTrue(m_climbDown); - ControlMap.m_gunnerController.x().whileTrue(m_climbUp); + ControlMap.m_gunnerController.rightTrigger().toggleOnTrue((m_shootSpeaker)); // // FOR TESTING, REMOVE FOR COMP // ControlMap.m_gunnerController.leftBumper().whileTrue(m_shootLeftMotor); @@ -191,7 +198,29 @@ private void configureDefaultCommands(){ () -> -ControlMap.m_driverController.getLeftY(), () -> -ControlMap.m_driverController.getLeftX(), () -> -ControlMap.m_driverController.getRightX())); + m_climb.setDefaultCommand( + new ClimbTeleop(m_climb) + ); } + + private void configureAutoBuilderCommands(){ + //Intake (MANUAL) + NamedCommands.registerCommand("Intake Down", m_intakeDown); + NamedCommands.registerCommand("Intake Up", m_intakeUp); + NamedCommands.registerCommand("Rollers Out", m_moveRollersOut); + NamedCommands.registerCommand("Rollers In", m_moveRollersIn); + NamedCommands.registerCommand("Auto Start", m_autoStart); + NamedCommands.registerCommand("Feed Note", m_feedNote); + NamedCommands.registerCommand("Apriltag Align", m_alignSpeaker); + + //Shooter (MANUAL) + NamedCommands.registerCommand("Shooter On", m_shootSpeaker); + + //Autonomous Commands + NamedCommands.registerCommand("Auto Speaker", m_autoShootSpeaker); + } + + public Command getAutonomousCommand() { return m_chooser.getSelected(); diff --git a/src/main/java/frc/robot/SwerveModule.java b/src/main/java/frc/robot/SwerveModule.java deleted file mode 100644 index ded23d8..0000000 --- a/src/main/java/frc/robot/SwerveModule.java +++ /dev/null @@ -1,181 +0,0 @@ -package frc.robot; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import frc.robot.subsystems.SwerveLib.Conversions; -import edu.wpi.first.math.controller.PIDController; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DutyCycle; -import frc.robot.Constants; -import frc.robot.Robot; -import frc.robot.subsystems.SwerveLib.OnboardModuleState; -import frc.robot.subsystems.SwerveLib.SwerveModuleConstants; - -public class SwerveModule { - public int moduleNumber; - private Rotation2d lastAngle; - private Rotation2d desiredAngle; - - private TalonFX angleMotor; - private TalonFX driveMotor; - private CANcoder angleEncoder; - - private final VelocityVoltage driveVelocity = new VelocityVoltage(0); - private final PositionVoltage anglePosition = new PositionVoltage(0); - private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0); - - - private final PIDController regController = - new PIDController(Constants.SwerveConstants.angleKP, - Constants.SwerveConstants.angleKI, - Constants.SwerveConstants.angleKD); - - private final SimpleMotorFeedforward feedforward = - new SimpleMotorFeedforward( - Constants.SwerveConstants.driveKS, - Constants.SwerveConstants.driveKV, - Constants.SwerveConstants.driveKA); - - private final PIDController turnFeedback = - new PIDController(0.1, 0.0, 0.0, 0.02); - - public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { - this.moduleNumber = moduleNumber; - this.desiredAngle = moduleConstants.desiredAngle; - - /* Angle Encoder Config */ - angleEncoder = new CANcoder(moduleConstants.cancoderID); - configAngleEncoder(); - - /* Angle Motor Config */ - angleMotor = new TalonFX(moduleConstants.angleMotorID); - configAngleMotor(); - - /* Drive Motor Config */ - driveMotor = new TalonFX(moduleConstants.driveMotorID); - configDriveMotor(); - - lastAngle = getState().angle; - - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - } - - /* Desired state for each swerve module. takes in speed and angle. If its openLoop, that means it is in teleop */ - public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { - // Custom optimize command, since default WPILib optimize assumes continuous controller which - // REV and CTRE are not - System.out.println(desiredState.angle); - desiredState = OnboardModuleState.optimize(desiredState, getState().angle); - System.out.println(desiredState.angle); - setAngle(desiredState); - setSpeed(desiredState, isOpenLoop); - } - - /* Reset wheel orientation to forward */ - public void resetToAbsolute() { - double absolutePosition = getCanCoder().getRotations() - desiredAngle.getRotations(); - angleMotor.setPosition(absolutePosition); - System.out.println(absolutePosition); - } - - /* Settings for Angle Encoder */ - private void configAngleEncoder() { - angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCanCoderConfig, 0.1); - } - - /* Settings for Angle Motor */ - private void configAngleMotor() { - angleMotor.getConfigurator().apply(Robot.ctreConfigs.swerveAngleFXConfig, 0.1); - resetToAbsolute(); - } - - /* Settings for Drive Motor */ - private void configDriveMotor() { - driveMotor.getConfigurator().apply(Robot.ctreConfigs.swerveDriveFXConfig, 0.1); - driveMotor.getConfigurator().setPosition(0.0); - } - - /* Gets the current position of the swerve module. This is an estimate */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition( - Conversions.rotationsToMeters(driveMotor.getPosition().getValue(), Constants.SwerveConstants.wheelCircumference), - Rotation2d.fromRotations(angleMotor.getPosition().getValue()) - ); - } - - /* Sets the speed of the swerve module. If it's openLoop, then it takes in a percentage, otherwise, it calculates and runs a PID */ - private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { - if (isOpenLoop) { - driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.SwerveConstants.maxSpeed; - driveMotor.setControl(driveDutyCycle); - //double percentOutput = desiredState.speedMetersPerSecond / Constants.SwerveConstants.maxSpeed; - //driveMotor.set(percentOutput); - } else { - //creating a feedforwad with a desired velocity - driveVelocity.Velocity = Conversions.MPSToRPS(desiredState.speedMetersPerSecond, Constants.SwerveConstants.wheelCircumference); - driveVelocity.FeedForward = feedforward.calculate(desiredState.speedMetersPerSecond); - driveMotor.setControl(driveVelocity); - } - } - - /* Sets the angle of the swerve module. */ - private void setAngle(SwerveModuleState desiredState) { - Rotation2d angle = - (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.01)) - ? lastAngle - : desiredState.angle; - System.out.println(anglePosition.withPosition(desiredState.angle.getRotations()).Position); - // angleMotor.set(desiredState.angle.getRotations()); - angleMotor.setControl(anglePosition.withPosition(desiredState.angle.getRotations())); //degrees because of the conversion factor - lastAngle = angle; - - // double turnAngleError = Math.abs(angle.getDegrees() - integratedAngleEncoder.getPosition()); - - // double pidOut = regController.calculate(integratedAngleEncoder.getPosition(), angle.getDegrees()); - // // if robot is not moving, stop the turn motor oscillating - // if (turnAngleError < Constants.Swerve.stickDeadband - // && Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) - // pidOut = 0; - - // angleMotor.setVoltage(pidOut * RobotController.getBatteryVoltage()); - - // angleMotor.set(pidOut); - } - - - private Rotation2d getAngle() { - return Rotation2d.fromDegrees(angleMotor.getPosition().getValueAsDouble() * Constants.SwerveConstants.angleConversionFactor); - } - - public Rotation2d getCanCoder() { - return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValueAsDouble()); - } - - public SwerveModuleState getState() { - return new SwerveModuleState( - Conversions.RPSToMPS(driveMotor.getVelocity().getValueAsDouble(), Constants.SwerveConstants.wheelCircumference), getAngle()); - } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return (2 * Math.PI) / 60 * driveMotor.getVelocity().getValueAsDouble(); - } - - public void runCharacterization(double volts) { - setTurnVoltage(turnFeedback.calculate(getAngle().getRadians(), 0.0)); - setDriveVoltage(volts); - } - - public void setDriveVoltage(double volts) { - driveMotor.setVoltage(volts); - } - - public void setTurnVoltage(double volts) { - angleMotor.setVoltage(volts); - } -} diff --git a/src/main/java/frc/robot/commands/Characterization/FeedForwardCharacterization.java b/src/main/java/frc/robot/commands/Characterization/FeedForwardCharacterization.java deleted file mode 100644 index 9d5ddc5..0000000 --- a/src/main/java/frc/robot/commands/Characterization/FeedForwardCharacterization.java +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.commands.Characterization; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; -import java.util.LinkedList; -import java.util.List; -import java.util.function.BiConsumer; -import java.util.function.Consumer; -import java.util.function.Supplier; - -public class FeedForwardCharacterization extends Command { - private static final double startDelaySecs = 2.0; - private static final double rampRateVoltsPerSec = 0.2; - - private final boolean forwards; - private final boolean isDrive; - - private final FeedForwardCharacterizationData dataPrimary; - private final FeedForwardCharacterizationData dataSecondary; - private final Consumer voltageConsumerSimple; - private final BiConsumer voltageConsumerDrive; - private final Supplier velocitySupplierPrimary; - private final Supplier velocitySupplierSecondary; - - private final Timer timer = new Timer(); - - /** Creates a new FeedForwardCharacterization for a differential drive. */ - public FeedForwardCharacterization( - Subsystem drive, - boolean forwards, - FeedForwardCharacterizationData leftData, - FeedForwardCharacterizationData rightData, - BiConsumer voltageConsumer, - Supplier leftVelocitySupplier, - Supplier rightVelocitySupplier) { - addRequirements(drive); - this.forwards = forwards; - this.isDrive = true; - this.dataPrimary = leftData; - this.dataSecondary = rightData; - this.voltageConsumerSimple = null; - this.voltageConsumerDrive = voltageConsumer; - this.velocitySupplierPrimary = leftVelocitySupplier; - this.velocitySupplierSecondary = rightVelocitySupplier; - } - - /** Creates a new FeedForwardCharacterization for a simple subsystem. */ - public FeedForwardCharacterization( - Subsystem subsystem, - boolean forwards, - FeedForwardCharacterizationData data, - Consumer voltageConsumer, - Supplier velocitySupplier) { - addRequirements(subsystem); - this.forwards = forwards; - this.isDrive = false; - this.dataPrimary = data; - this.dataSecondary = null; - this.voltageConsumerSimple = voltageConsumer; - this.voltageConsumerDrive = null; - this.velocitySupplierPrimary = velocitySupplier; - this.velocitySupplierSecondary = null; - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - timer.reset(); - timer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (timer.get() < startDelaySecs) { - if (isDrive) { - voltageConsumerDrive.accept(0.0, 0.0); - } else { - voltageConsumerSimple.accept(0.0); - } - } else { - double voltage = (timer.get() - startDelaySecs) * rampRateVoltsPerSec * (forwards ? 1 : -1); - if (isDrive) { - voltageConsumerDrive.accept(voltage, voltage); - } else { - voltageConsumerSimple.accept(voltage); - } - dataPrimary.add(velocitySupplierPrimary.get(), voltage); - if (isDrive) { - dataSecondary.add(velocitySupplierSecondary.get(), voltage); - } - } - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - if (isDrive) { - voltageConsumerDrive.accept(0.0, 0.0); - } else { - voltageConsumerSimple.accept(0.0); - } - timer.stop(); - dataPrimary.print(); - if (isDrive) { - dataSecondary.print(); - } - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } - - public static class FeedForwardCharacterizationData { - private final String name; - private final List velocityData = new LinkedList<>(); - private final List voltageData = new LinkedList<>(); - - public FeedForwardCharacterizationData(String name) { - this.name = name; - } - - public void add(double velocity, double voltage) { - if (Math.abs(velocity) > 1E-4) { - velocityData.add(Math.abs(velocity)); - voltageData.add(Math.abs(voltage)); - } - } - - public void print() { - if (velocityData.size() == 0 || voltageData.size() == 0) { - return; - } - - PolynomialRegression regression = - new PolynomialRegression( - velocityData.stream().mapToDouble(Double::doubleValue).toArray(), - voltageData.stream().mapToDouble(Double::doubleValue).toArray(), - 1); - - System.out.println("FF Characterization Results (" + name + "):"); - System.out.println("\tCount=" + Integer.toString(velocityData.size()) + ""); - System.out.println(String.format("\tR2=%.5f", regression.R2())); - System.out.println(String.format("\tkS=%.5f", regression.beta(0))); - System.out.println(String.format("\tkV=%.5f", regression.beta(1))); - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/Characterization/OwnCharacterization.java b/src/main/java/frc/robot/commands/Characterization/OwnCharacterization.java deleted file mode 100644 index fa3a750..0000000 --- a/src/main/java/frc/robot/commands/Characterization/OwnCharacterization.java +++ /dev/null @@ -1,32 +0,0 @@ -// 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.Characterization; - -import edu.wpi.first.wpilibj2.command.Command; - -public class OwnCharacterization extends Command { - /** Creates a new OwnCharacterization. */ - public OwnCharacterization() { - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // 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/Characterization/PolynomialRegression.java b/src/main/java/frc/robot/commands/Characterization/PolynomialRegression.java deleted file mode 100644 index 1175187..0000000 --- a/src/main/java/frc/robot/commands/Characterization/PolynomialRegression.java +++ /dev/null @@ -1,212 +0,0 @@ -// Copyright (c) 2023 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package frc.robot.commands.Characterization; - -import Jama.Matrix; -import Jama.QRDecomposition; - -// NOTE: This file is available at -// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html - -/** - * The {@code PolynomialRegression} class performs a polynomial regression on an set of N - * data points (yi, xi). That is, it fits a polynomial - * y = β0 + β1 x + β2 - * x2 + ... + βd xd (where - * y is the response variable, x is the predictor variable, and the - * βi are the regression coefficients) that minimizes the sum of squared - * residuals of the multiple regression model. It also computes associated the coefficient of - * determination R2. - * - *

This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is - * neither the fastest nor the most numerically stable way to perform the polynomial regression. - * - * @author Robert Sedgewick - * @author Kevin Wayne - */ -public class PolynomialRegression implements Comparable { - private final String variableName; // name of the predictor variable - private int degree; // degree of the polynomial regression - private Matrix beta; // the polynomial regression coefficients - private double sse; // sum of squares due to error - private double sst; // total sum of squares - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name - * of the predictor variable. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree) { - this(x, y, degree, "n"); - } - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @param variableName the name of the predictor variable - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { - this.degree = degree; - this.variableName = variableName; - - int n = x.length; - QRDecomposition qr = null; - Matrix matrixX = null; - - // in case Vandermonde matrix does not have full rank, reduce degree until it - // does - while (true) { - - // build Vandermonde matrix - double[][] vandermonde = new double[n][this.degree + 1]; - for (int i = 0; i < n; i++) { - for (int j = 0; j <= this.degree; j++) { - vandermonde[i][j] = Math.pow(x[i], j); - } - } - matrixX = new Matrix(vandermonde); - - // find least squares solution - qr = new QRDecomposition(matrixX); - if (qr.isFullRank()) break; - - // decrease degree and try again - this.degree--; - } - - // create matrix from vector - Matrix matrixY = new Matrix(y, n); - - // linear regression coefficients - beta = qr.solve(matrixY); - - // mean of y[] values - double sum = 0.0; - for (int i = 0; i < n; i++) sum += y[i]; - double mean = sum / n; - - // total variation to be accounted for - for (int i = 0; i < n; i++) { - double dev = y[i] - mean; - sst += dev * dev; - } - - // variation not accounted for - Matrix residuals = matrixX.times(beta).minus(matrixY); - sse = residuals.norm2() * residuals.norm2(); - } - - /** - * Returns the {@code j}th regression coefficient. - * - * @param j the index - * @return the {@code j}th regression coefficient - */ - public double beta(int j) { - // to make -0.0 print as 0.0 - if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; - return beta.get(j, 0); - } - - /** - * Returns the degree of the polynomial to fit. - * - * @return the degree of the polynomial to fit - */ - public int degree() { - return degree; - } - - /** - * Returns the coefficient of determination R2. - * - * @return the coefficient of determination R2, which is a real number between - * 0 and 1 - */ - public double R2() { - if (sst == 0.0) return 1.0; // constant function - return 1.0 - sse / sst; - } - - /** - * Returns the expected response {@code y} given the value of the predictor variable {@code x}. - * - * @param x the value of the predictor variable - * @return the expected response {@code y} given the value of the predictor variable {@code x} - */ - public double predict(double x) { - // horner's method - double y = 0.0; - for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); - return y; - } - - /** - * Returns a string representation of the polynomial regression model. - * - * @return a string representation of the polynomial regression model, including the best-fit - * polynomial and the coefficient of determination R2 - */ - public String toString() { - StringBuilder s = new StringBuilder(); - int j = degree; - - // ignoring leading zero coefficients - while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; - - // create remaining terms - while (j >= 0) { - if (j == 0) s.append(String.format("%.10f ", beta(j))); - else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); - else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); - j--; - } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); - - // replace "+ -2n" with "- 2n" - return s.toString().replace("+ -", "- "); - } - - /** Compare lexicographically. */ - public int compareTo(PolynomialRegression that) { - double EPSILON = 1E-5; - int maxDegree = Math.max(this.degree(), that.degree()); - for (int j = maxDegree; j >= 0; j--) { - double term1 = 0.0; - double term2 = 0.0; - if (this.degree() >= j) term1 = this.beta(j); - if (that.degree() >= j) term2 = that.beta(j); - if (Math.abs(term1) < EPSILON) term1 = 0.0; - if (Math.abs(term2) < EPSILON) term2 = 0.0; - if (term1 < term2) return -1; - else if (term1 > term2) return +1; - } - return 0; - } - - /** - * Unit tests the {@code PolynomialRegression} data type. - * - * @param args the command-line arguments - */ - public static void main(String[] args) { - double[] x = {10, 20, 40, 80, 160, 200}; - double[] y = {100, 350, 1500, 6700, 20160, 40000}; - PolynomialRegression regression = new PolynomialRegression(x, y, 3); - - System.out.println(regression); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/CharacterizationCommands/forwardMeter.java b/src/main/java/frc/robot/commands/CharacterizationCommands/forwardMeter.java new file mode 100644 index 0000000..c084b0b --- /dev/null +++ b/src/main/java/frc/robot/commands/CharacterizationCommands/forwardMeter.java @@ -0,0 +1,67 @@ +// 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.CharacterizationCommands; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.TrajectoryCommands.TrajectoryCreation; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.PoseEstimator; +import frc.robot.subsystems.Vision; + +public class forwardMeter extends Command { + private final Drivetrain driveSystem; + private final Vision m_vision; + private final PoseEstimator poseEstimatorSystem; + private final TrajectoryCreation m_traj; + private final double translation; + + private Command controllerCommand = Commands.none(); + + /** Creates a new RunOnTheFly. */ + public forwardMeter(Drivetrain d, PoseEstimator p, TrajectoryCreation traj, Vision v, + double y) { + // Use addRequirements() here to declare subsystem dependencies. + driveSystem = d; + poseEstimatorSystem = p; + m_traj = traj; + m_vision = v; + translation = y; + + + addRequirements(d, v, p); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + PathPlannerPath path = m_traj.ForwardMeter(poseEstimatorSystem); + + controllerCommand = AutoBuilder.followPath(path); + controllerCommand.initialize(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + controllerCommand.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + controllerCommand.end(interrupted); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return controllerCommand.isFinished(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ClimbCommands/ClimbTeleop.java b/src/main/java/frc/robot/commands/ClimbCommands/ClimbTeleop.java new file mode 100644 index 0000000..f82e17e --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimbCommands/ClimbTeleop.java @@ -0,0 +1,65 @@ +// 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.ClimbCommands; + +import java.util.ResourceBundle.Control; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.Controls.ControlMap; +import frc.robot.subsystems.Climb; + +public class ClimbTeleop extends Command { + /** Creates a new ClimbUp. */ + private final Climb m_climb; + public ClimbTeleop(Climb c) { + m_climb = c; + addRequirements(c); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_climb.setLeftSpeed(0); + m_climb.setRightSpeed(0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double leftSpeed = ControlMap.m_gunnerController.getLeftY(); + double rightSpeed = ControlMap.m_gunnerController.getRightY(); + if(leftSpeed < -Constants.SwerveConstants.stickDeadband) { + leftSpeed = -Constants.ClimbConstants.climbLeftSpeed; + } else if(leftSpeed > Constants.SwerveConstants.stickDeadband) { + leftSpeed = Constants.ClimbConstants.climbLeftSpeed; + } else { + leftSpeed = 0; + } + if(rightSpeed < -Constants.SwerveConstants.stickDeadband) { + rightSpeed = -Constants.ClimbConstants.climbRightSpeed; + } else if(rightSpeed > Constants.SwerveConstants.stickDeadband) { + rightSpeed = Constants.ClimbConstants.climbRightSpeed; + } else { + rightSpeed = 0; + } + m_climb.setLeftSpeed(leftSpeed); + m_climb.setRightSpeed(rightSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_climb.setLeftSpeed(0); + m_climb.setRightSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ClimbCommands/ClimbUp.java b/src/main/java/frc/robot/commands/ClimbCommands/ClimbUp.java deleted file mode 100644 index 2aec387..0000000 --- a/src/main/java/frc/robot/commands/ClimbCommands/ClimbUp.java +++ /dev/null @@ -1,43 +0,0 @@ -// 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.ClimbCommands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.Climb; - -public class ClimbUp extends Command { - /** Creates a new ClimbUp. */ - private final Climb m_climb; - public ClimbUp(Climb c) { - m_climb = c; - addRequirements(c); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_climb.setSpeed(0, 0); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_climb.setSpeed(Constants.ClimbConstants.climbLeftSpeed, Constants.ClimbConstants.climbRightSpeed); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_climb.setSpeed(0, 0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/DriveCommands/LeaveZone.java b/src/main/java/frc/robot/commands/DriveCommands/LeaveZone.java index 143df4a..df32080 100644 --- a/src/main/java/frc/robot/commands/DriveCommands/LeaveZone.java +++ b/src/main/java/frc/robot/commands/DriveCommands/LeaveZone.java @@ -30,7 +30,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drivetrain.driveRobotRelative(new Translation2d(0.5, 0), 0, true); + m_drivetrain.driveRobotRelative(new Translation2d(1, 0), 0, true); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ClimbCommands/CilmbDown.java b/src/main/java/frc/robot/commands/IntakeCommands/FeedNote.java similarity index 57% rename from src/main/java/frc/robot/commands/ClimbCommands/CilmbDown.java rename to src/main/java/frc/robot/commands/IntakeCommands/FeedNote.java index b968c7e..50de93b 100644 --- a/src/main/java/frc/robot/commands/ClimbCommands/CilmbDown.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/FeedNote.java @@ -2,42 +2,48 @@ // 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.ClimbCommands; +package frc.robot.commands.IntakeCommands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.subsystems.Climb; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; -public class CilmbDown extends Command { - /** Creates a new ClimbUp. */ - private final Climb m_climb; - public CilmbDown(Climb c) { - m_climb = c; - addRequirements(c); +public class FeedNote extends Command { + private Intake m_intake; + private Timer time = new Timer(); + /** Creates a new AutoShootSpeaker. */ + public FeedNote(Intake i) { + m_intake = i; + addRequirements(i); // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override public void initialize() { - m_climb.setSpeed(0, 0); + time.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_climb.setSpeed(-Constants.ClimbConstants.climbLeftSpeed, -Constants.ClimbConstants.climbRightSpeed); + m_intake.moveRollers(Constants.IntakeConstants.rollerSpeedOuttake); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_climb.setSpeed(0, 0); + time.stop(); + time.reset(); + m_intake.moveRollers(0); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + // return count >= 10; + return time.get() >= 2; } } diff --git a/src/main/java/frc/robot/commands/ShooterCommands/AutoShootSpeaker.java b/src/main/java/frc/robot/commands/ShooterCommands/AutoShootSpeaker.java index 5c69095..fee5dea 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands/AutoShootSpeaker.java +++ b/src/main/java/frc/robot/commands/ShooterCommands/AutoShootSpeaker.java @@ -44,7 +44,7 @@ public void execute() { // time.start(); // } // if(m_shooter.getCurrent() < 16 && spikeDone) { - if(time.get() >= 8) { + if(time.get() >= 5) { m_shooter.shoot(Constants.ShooterConstants.shooterLeftSpeedSpeaker, Constants.ShooterConstants.shooterRightSpeedSpeaker); m_intake.moveRollers(Constants.IntakeConstants.rollerSpeedOuttake); } else { @@ -68,6 +68,6 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { // return count >= 10; - return time.get() >= 10; + return time.get() >= 7; } } diff --git a/src/main/java/frc/robot/commands/ShooterCommands/AutoShootStart.java b/src/main/java/frc/robot/commands/ShooterCommands/AutoShootStart.java new file mode 100644 index 0000000..216ca40 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShooterCommands/AutoShootStart.java @@ -0,0 +1,72 @@ +// 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.ShooterCommands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; + +public class AutoShootStart extends Command { + private Shooter m_shooter; + private Intake m_intake; + private boolean spikeDone; + private int count = 0; + private Timer time = new Timer(); + /** Creates a new AutoShootSpeaker. */ + public AutoShootStart(Shooter s, Intake i) { + m_shooter = s; + m_intake = i; + addRequirements(s, i); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + time.start(); + spikeDone = false; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // if(m_intake.getIRSensor() < 0.3) { + // count++; + // } else { + // count = 0; + // } + // if(m_shooter.getCurrent() > 30) { + // spikeDone = true; + // time.start(); + // } + // if(m_shooter.getCurrent() < 16 && spikeDone) { + if(time.get() >= 5) { + m_shooter.shoot(Constants.ShooterConstants.shooterLeftSpeedSpeaker, Constants.ShooterConstants.shooterRightSpeedSpeaker); + m_intake.moveRollers(Constants.IntakeConstants.rollerSpeedOuttake); + } else { + m_shooter.shoot(Constants.ShooterConstants.shooterLeftSpeedSpeaker, Constants.ShooterConstants.shooterRightSpeedSpeaker); + } + // } else { + // m_shooter.shoot(Constants.ShooterConstants.shooterLeftSpeedSpeaker, Constants.ShooterConstants.shooterRightSpeedSpeaker); + // } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + time.stop(); + time.reset(); + m_intake.moveRollers(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + // return count >= 10; + return time.get() >= 7; + } +} diff --git a/src/main/java/frc/robot/commands/TrajectoryCommands/AlignSpeakerManual.java b/src/main/java/frc/robot/commands/TrajectoryCommands/AlignSpeakerManual.java new file mode 100644 index 0000000..4fa0f70 --- /dev/null +++ b/src/main/java/frc/robot/commands/TrajectoryCommands/AlignSpeakerManual.java @@ -0,0 +1,67 @@ +// 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.TrajectoryCommands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Vision; + +public class AlignSpeakerManual extends Command { + private Drivetrain m_drivetrain; + private Vision m_vision; + private double initialAngle; + private double goalAngle; + private double initialMeters; + private double goalMeters; + /** Creates a new AlignSpeakerManual. */ + public AlignSpeakerManual(Drivetrain d, Vision v) { + m_drivetrain = d; + m_vision = v; + addRequirements(d, v); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + Pose2d tagPose = m_vision.getTagPose(); + initialAngle = m_drivetrain.getNavxAngle().getDegrees() - tagPose.getRotation().getDegrees(); + initialMeters = m_drivetrain.getEncoderMeters(); + goalMeters = Math.sqrt( Math.pow(tagPose.getX(), 2) + Math.pow(tagPose.getY(), 2) ); + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { + goalAngle = 180; + } else { + goalAngle = 0; + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double curPos = goalMeters - (Math.abs(m_drivetrain.getEncoderMeters() - initialMeters)); + double curAngle = 0; + if(Math.abs(goalAngle - (m_drivetrain.getNavxAngle().getDegrees() - initialAngle)) > 0.1) { + curAngle = goalAngle - (m_drivetrain.getNavxAngle().getDegrees() - initialAngle); + } + double xSpeed = Math.cos(Units.degreesToRadians(initialAngle)) * curPos; + double ySpeed = Math.sin(Units.degreesToRadians(initialAngle)) * curPos; + m_drivetrain.drive(new Translation2d(xSpeed, ySpeed), Units.degreesToRadians(curAngle) * 2, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_drivetrain.drive(new Translation2d(), 0, true); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return goalMeters - (Math.abs(m_drivetrain.getEncoderMeters() - initialMeters)) < 0.1; } +} diff --git a/src/main/java/frc/robot/commands/TrajectoryCommands/FollowNote.java b/src/main/java/frc/robot/commands/TrajectoryCommands/FollowNote.java index 370c1cc..8ce3081 100644 --- a/src/main/java/frc/robot/commands/TrajectoryCommands/FollowNote.java +++ b/src/main/java/frc/robot/commands/TrajectoryCommands/FollowNote.java @@ -50,8 +50,7 @@ public void initialize() { controllerCommand = AutoBuilder.followPath(path); controllerCommand.initialize(); } - //we dont want to update using apriltag during the trajectory - poseEstimatorSystem.isUsingAprilTag(false); + } @@ -68,7 +67,6 @@ public void execute() { public void end(boolean interrupted) { System.out.println("--------------------DONE------------------"); controllerCommand.end(interrupted); - poseEstimatorSystem.isUsingAprilTag(true); System.out.println(poseEstimatorSystem.getCurrentPose().getX()); System.out.println(poseEstimatorSystem.getCurrentPose().getY()); diff --git a/src/main/java/frc/robot/commands/TrajectoryCommands/MoveToNote.java b/src/main/java/frc/robot/commands/TrajectoryCommands/MoveToNote.java index 249e7c0..fd9c0a3 100644 --- a/src/main/java/frc/robot/commands/TrajectoryCommands/MoveToNote.java +++ b/src/main/java/frc/robot/commands/TrajectoryCommands/MoveToNote.java @@ -33,9 +33,9 @@ public void initialize() { initialPos = m_drivetrain.getEncoderMeters(); if(m_vision.hasTarget()) { goalAngle = -m_vision.getTargetYaw(); - goalPos = m_vision.getNoteRange() - 1; //1 m in front of note + goalPos = m_vision.getNoteRange() - 0.5; //1 m in front of note } - m_drivetrain.driveRobotRelative(new Translation2d(), 0, false); + m_drivetrain.drive(new Translation2d(), 0, true); } // Called every time the scheduler runs while the command is scheduled. @@ -48,18 +48,18 @@ public void execute() { } double xSpeed = Math.cos(Units.degreesToRadians(initialAngle + goalAngle)) * curPos; double ySpeed = Math.sin(Units.degreesToRadians(initialAngle + goalAngle)) * curPos; - m_drivetrain.drive(new Translation2d(xSpeed, ySpeed), Units.degreesToRadians(curAngle), false); + m_drivetrain.drive(new Translation2d(xSpeed, ySpeed), Units.degreesToRadians(curAngle) * 2, true); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_drivetrain.driveRobotRelative(new Translation2d(0, 0), 0, false); + m_drivetrain.drive(new Translation2d(), 0, true); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return goalPos - (Math.abs(m_drivetrain.getEncoderMeters() - initialPos)) < 0.1; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TrajectoryCommands/RunOnTheFly.java b/src/main/java/frc/robot/commands/TrajectoryCommands/RunOnTheFly.java index 1d2cd04..2fc8c8b 100644 --- a/src/main/java/frc/robot/commands/TrajectoryCommands/RunOnTheFly.java +++ b/src/main/java/frc/robot/commands/TrajectoryCommands/RunOnTheFly.java @@ -41,7 +41,7 @@ public RunOnTheFly(Drivetrain d, PoseEstimator p, TrajectoryCreation traj, Visio public void initialize() { PathPlannerPath path = m_traj.onthefly(poseEstimatorSystem, m_vision, translation); - + controllerCommand = AutoBuilder.followPath(path); controllerCommand.initialize(); } diff --git a/src/main/java/frc/robot/commands/TrajectoryCommands/TrajectoryCreation.java b/src/main/java/frc/robot/commands/TrajectoryCommands/TrajectoryCreation.java index 6e86ad5..62233a0 100644 --- a/src/main/java/frc/robot/commands/TrajectoryCommands/TrajectoryCreation.java +++ b/src/main/java/frc/robot/commands/TrajectoryCommands/TrajectoryCreation.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; @@ -130,27 +131,26 @@ public PathPlannerPath alignSpeaker(PoseEstimator estimation, Vision vision) { double tagX = 0; double tagY = 0; Rotation2d tagAngle = new Rotation2d(); - double xChange = -Units.inchesToMeters(91); + double xChange = Units.inchesToMeters(54); double yChange = 0; - - if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { - tagX = vision.return_tag_pose(7).getX() + Units.inchesToMeters(91); - tagY = vision.return_tag_pose(7).getY();; - tagAngle = new Rotation2d(180); - xChange = 1; - } else { - tagX = vision.return_tag_pose(4).getX() - Units.inchesToMeters(91); - tagY = vision.return_tag_pose(4).getY();; - tagAngle = new Rotation2d(0); - xChange = -1; - } + + tagX = vision.return_tag_pose(7).getX(); + tagY = vision.return_tag_pose(7).getY(); + tagAngle = new Rotation2d(0); - double offset = Constants.SwerveConstants.trackWidth / 2; + // double offset = Constants.SwerveConstants.trackWidth / 2; + + // List bezierPoints = PathPlannerPath.bezierFromPoses( + // new Pose2d(x, y, angle), + // new Pose2d(tagX + xChange, tagY + yChange, tagAngle) + // ); + + System.out.println(tagX + xChange); List bezierPoints = PathPlannerPath.bezierFromPoses( - new Pose2d(x, y, angle), - new Pose2d(tagX + xChange, tagY + yChange, tagAngle) - ); + new Pose2d(x, y, angle), + new Pose2d(tagX + xChange, tagY, tagAngle) + ); // Create the path using the bezier points created above PathPlannerPath path = new PathPlannerPath( @@ -173,25 +173,26 @@ public PathPlannerPath alignAmp(PoseEstimator estimation, Vision vision) { double tagX = 0; double tagY = 0; Rotation2d tagAngle = new Rotation2d(); - double xChange = 0; - double yChange = 0; - - if(DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) { - tagX = vision.return_tag_pose(6).getX(); - tagY = vision.return_tag_pose(6).getY();; - tagAngle = new Rotation2d(90); - } else { - tagX = vision.return_tag_pose(5).getX(); - tagY = vision.return_tag_pose(5).getY();; - tagAngle = new Rotation2d(90); - } + double xChange = Constants.SwerveConstants.trackWidth/2; + double yChange = Constants.SwerveConstants.trackWidth/2; + + tagX = vision.return_tag_pose(6).getX(); + tagY = vision.return_tag_pose(6).getY(); + tagAngle = new Rotation2d(3 * Math.PI/2); - double offset = Constants.SwerveConstants.trackWidth / 2; + // double offset = Constants.SwerveConstants.trackWidth / 2; + + // List bezierPoints = PathPlannerPath.bezierFromPoses( + // new Pose2d(x, y, angle), + // new Pose2d(tagX + xChange, tagY + yChange, tagAngle) + // ); + + System.out.println(tagX + xChange); List bezierPoints = PathPlannerPath.bezierFromPoses( - new Pose2d(x, y, angle), - new Pose2d(tagX + xChange, tagY + yChange, tagAngle) - ); + new Pose2d(x, y, angle), + new Pose2d(tagX + xChange, tagY - yChange, tagAngle) + ); // Create the path using the bezier points created above PathPlannerPath path = new PathPlannerPath( @@ -265,14 +266,14 @@ public PathPlannerPath onthefly(PoseEstimator estimation, Vision vision, double double y = estimatedPose.getY(); Rotation2d angle = estimatedPose.getRotation(); - PhotonPipelineResult result = vision.getApriltagCamera(2).getLatestResult(); + PhotonPipelineResult result = vision.getApriltagCamera().getLatestResult(); int id; double tagX = 0; double tagY = 0; Rotation2d tagAngle = new Rotation2d(); if(result.hasTargets()){ - id = vision.getApriltagCamera(2).getLatestResult().getBestTarget().getFiducialId(); + id = vision.getApriltagCamera().getLatestResult().getBestTarget().getFiducialId(); Pose2d tagPose = vision.return_tag_pose(id).toPose2d(); @@ -429,23 +430,29 @@ public PathPlannerPath noteOnTheFly(PoseEstimator estimation, Vision vision, Dri if (hasTargets){ //get the current RELATIVE notespace - Transform2d notespace = vision.getNoteSpace(); - //double offset = Units.inchesToMeters(10); //center offset - //add whatever translations to it - notespace = new Transform2d(notespace.getX() - 0.5, notespace.getY(), notespace.getRotation()); - //transform the notespace to field relative coords. The angle is in estimatedPose, and the transformation is done by this angle. - Pose2d transformedPose = estimatedPose.transformBy(notespace); + Transform3d notespace = vision.getNoteSpace().plus(vision.getRobotToCam()); + Transform2d notespace2d = new Transform2d(new Translation2d(notespace.getX(), notespace.getY()), new Rotation2d()); + Pose2d camPose = estimatedPose.transformBy(notespace2d); + // Transform2d notespace2d; + // double offset = Units.inchesToMeters(10); //center offset + // //add whatever translations to it + // //notespace = new Transform2d(notespace.getX() - (0.5 * Math.cos(drivetrain.getNavxAngle().getDegrees())), notespace.getY() - 0.2 + (0.2 * -Math.sin(drivetrain.getNavxAngle().getDegrees())), notespace.getRotation()); + // notespace = notespace.plus(new Transform3d(new Translation3d(-0.5, 0,0), new Rotation3d())); + // notespace2d = new Transform2d(new Translation2d(notespace.getX(), notespace.getY()), new Rotation2d()); + // //transform the notespace to field relative coords. The angle is in estimatedPose, and the transformation is done by this angle. + // Transform2d cameraToCenter = new Transform2d(new Translation2d(vision.getRobotToCam(3).getX(), vision.getRobotToCam(3).getY()), new Rotation2d(Units.degreesToRadians(vision.getRobotToCam(3).getRotation().getAngle()))); + // Pose2d transformedPose = estimatedPose.transformBy(notespace2d.plus(cameraToCenter)); //this is assuming that the current angle in the transformation is 0 degrees. // transformedPose.rotateBy(new Rotation2d( // Units.degreesToRadians(-vision.getTargetYaw() + drivetrain.getNavxAngle().getDegrees())) // ); - double endLocationX = transformedPose.getX(); - double endLocationY = (transformedPose.getY()); //- (2 * notespace.getY()) + 0.075 + double endLocationX = camPose.getX(); + double endLocationY = (camPose.getY()); //- (2 * notespace.getY()) + 0.075 System.out.println("---------------TRANSFORMATIONS----------------"); System.out.println("End Location X: " + endLocationX); System.out.println("End Location Y: " + endLocationY); - System.out.println("Transformed By (angle): " + transformedPose.getRotation().getDegrees()); + System.out.println("Transformed By (angle): " + camPose.getRotation().getDegrees()); List bezierPoints = PathPlannerPath.bezierFromPoses( new Pose2d(x, y, angle), // new Pose2d(x - 0.3,y,angle), @@ -453,9 +460,8 @@ public PathPlannerPath noteOnTheFly(PoseEstimator estimation, Vision vision, Dri new Pose2d(endLocationX, endLocationY, angle) ); - PathPlannerPath path = new PathPlannerPath( - bezierPoints, - constraints, + PathPlannerPath path = new PathPlannerPath(bezierPoints, + new PathConstraints(Constants.SwerveConstants.maxSpeed, Constants.SwerveConstants.maxAcceleration, Constants.SwerveConstants.maxAngularVelocity, Constants.SwerveConstants.maxAngularAcceleration), new GoalEndState(0, angle)); path.preventFlipping = true; //prevents the path from being flipped once the coords are reached return path; @@ -464,7 +470,6 @@ public PathPlannerPath noteOnTheFly(PoseEstimator estimation, Vision vision, Dri System.out.println("NO TARGETS"); return null; } - diff --git a/src/main/java/frc/robot/subsystems/Climb.java b/src/main/java/frc/robot/subsystems/Climb.java index 23cf9b2..56b235e 100644 --- a/src/main/java/frc/robot/subsystems/Climb.java +++ b/src/main/java/frc/robot/subsystems/Climb.java @@ -6,6 +6,7 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.SparkLimitSwitch; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -27,13 +28,35 @@ public static Climb getInstance(){ return instance; } - public void setSpeed(double speedLeft, double speedRight) { + public boolean getLeftLimitStateDown(){ + return leftClimb.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed).isPressed(); + } + + // return limit switch states + public boolean getRightLimitStateDown(){ + return rightClimb.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed).isPressed(); + } + + public boolean getLeftLimitStateUp(){ + return leftClimb.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed).isPressed(); + } + + // return limit switch states + public boolean getRightLimitStateUp(){ + return rightClimb.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyClosed).isPressed(); + } + + public void setLeftSpeed(double speedLeft) { leftClimb.set(speedLeft); + } + + public void setRightSpeed(double speedRight) { rightClimb.set(speedRight); } @Override public void periodic() { // This method will be called once per scheduler run + } } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index aeedafa..1b06f96 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -3,15 +3,24 @@ import java.util.function.Consumer; import com.kauailabs.navx.frc.AHRS; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; + import edu.wpi.first.math.controller.PIDController; 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.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import frc.robot.SwerveModule; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -19,19 +28,21 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.I2C; import frc.robot.Constants; +import frc.robot.subsystems.SwerveLib.SwerveModule; public class Drivetrain extends SubsystemBase { - private static Drivetrain drivetrain = null; + private static Drivetrain driveSubsystem = null; private SwerveModule[] mSwerveMods; private static AHRS navx; private Rotation2d navxAngleOffset; - private Field2d field; + private Field2d fieldLayout; private boolean isCharacterizing = false; private double characterizationVolts = 0.0; + private StructArrayPublisher publisher; public Drivetrain() { mSwerveMods = @@ -41,35 +52,34 @@ public Drivetrain() { new SwerveModule(2, Constants.SwerveConstants.Mod2.constants), new SwerveModule(3, Constants.SwerveConstants.Mod3.constants) }; + + publisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("MyStates", SwerveModuleState.struct).publish(); navx = new AHRS(I2C.Port.kMXP); navxAngleOffset = new Rotation2d(); navx.reset(); - field = new Field2d(); - SmartDashboard.putData("Field", field); + fieldLayout = new Field2d(); + SmartDashboard.putData("Field", fieldLayout); } public static Drivetrain getInstance(){ - if (drivetrain == null){ - drivetrain = new Drivetrain(); - } - return drivetrain; - } - - public void setDriveVoltage(double volts) { - for(SwerveModule mod : mSwerveMods) { - mod.setDriveVoltage(volts); + if (driveSubsystem == null){ + driveSubsystem = new Drivetrain(); } + return driveSubsystem; } + //Drives field relative public void drive( Translation2d translation, double rotation, boolean isOpenLoop) { SwerveModuleState[] swerveModuleStates = Constants.SwerveConstants.swerveKinematics.toSwerveModuleStates( ChassisSpeeds.fromFieldRelativeSpeeds( translation.getX(), translation.getY(), rotation, getNavxAngle())); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.SwerveConstants.maxSpeed); for (SwerveModule mod : mSwerveMods) { @@ -77,6 +87,8 @@ public void drive( } } + + //drives robot relative public void driveRobotRelative( Translation2d translation, double rotation, boolean isOpenLoop) { SwerveModuleState[] swerveModuleStates = @@ -90,6 +102,7 @@ public void driveRobotRelative( } } + //used for autonomous driving public void autoDrive(ChassisSpeeds speeds) { SwerveModuleState[] swerveModuleStates = Constants.SwerveConstants.swerveKinematics.toSwerveModuleStates(speeds); @@ -101,19 +114,6 @@ public void autoDrive(ChassisSpeeds speeds) { } } - public void wheelTest(ChassisSpeeds speeds, int n){ - SwerveModuleState[] swerveModuleStates = - Constants.SwerveConstants.swerveKinematics.toSwerveModuleStates(speeds); - - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.SwerveConstants.maxSpeed); - - for (SwerveModule mod : mSwerveMods) { - if (mod.moduleNumber == n){ - mod.setDesiredState(swerveModuleStates[mod.moduleNumber], false); - } - } - } - /* Used by SwerveControllerCommand in Auto */ public void setModuleStates(SwerveModuleState[] desiredStates) { SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.SwerveConstants.maxSpeed); @@ -131,6 +131,10 @@ public SwerveModuleState[] getStates() { return states; } + public SwerveModule[] getModules(){ + return mSwerveMods; + } + public SwerveModulePosition[] getPositions() { SwerveModulePosition[] positions = new SwerveModulePosition[4]; for (SwerveModule mod : mSwerveMods) { @@ -139,15 +143,6 @@ public SwerveModulePosition[] getPositions() { return positions; } - public double getEncoderMeters() { - double sum = 0.0; - SwerveModulePosition[] positions = getPositions(); - for (SwerveModulePosition pos : positions) { - sum += pos.distanceMeters; - } - return sum / 4.0; - } - public ChassisSpeeds getChassisSpeeds() { ChassisSpeeds result = Constants.SwerveConstants.swerveKinematics.toChassisSpeeds(getStates()); return result; @@ -160,6 +155,12 @@ public void zeroGyro() { public Rotation2d getNavxAngle(){ return Rotation2d.fromDegrees(-navx.getAngle()); } + + public double getEncoderMeters() { + SwerveModulePosition[] positions = getPositions(); + return positions[0].distanceMeters; + } + // setter for setting the navxAngleOffset public void setNavxAngleOffset(Rotation2d angle){ @@ -195,9 +196,11 @@ public double getCharacterizationVelocity() { @Override public void periodic() { - for (SwerveModule mod : mSwerveMods) { - SmartDashboard.putNumber( "Mod " + mod.moduleNumber + " angle", mod.getCanCoder().getDegrees()); - } + publisher.set(getStates()); + // for (SwerveModule mod : mSwerveMods) { + // SmartDashboard.putNumber( + // "Mod " + mod.moduleNumber + " velocity", mod.getCharacterizationVelocity()); + // } SmartDashboard.putNumber("Current Angle", navx.getAngle()); if (isCharacterizing) { // Run in characterization mode diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 6b892fe..222f67e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -68,9 +68,9 @@ public double getIRSensor(){ @Override public void periodic() { - SmartDashboard.putBoolean("limit forward", getIntakeLimitStateForward()); - SmartDashboard.putBoolean("limit reverse", getIntakeLimitStateReverse()); - SmartDashboard.putNumber("IR Sensor", getIRSensor()); + // SmartDashboard.putBoolean("limit forward", getIntakeLimitStateForward()); + // SmartDashboard.putBoolean("limit reverse", getIntakeLimitStateReverse()); + // SmartDashboard.putNumber("IR Sensor", getIRSensor()); // This method will be called once per scheduler run } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PoseEstimator.java b/src/main/java/frc/robot/subsystems/PoseEstimator.java index 1787673..d5a99bc 100644 --- a/src/main/java/frc/robot/subsystems/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/PoseEstimator.java @@ -3,21 +3,28 @@ // the WPILib BSD license file in the root directory of this project. package frc.robot.subsystems; -import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.targeting.PhotonTrackedTarget; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -26,19 +33,19 @@ import frc.robot.Constants; import frc.robot.Robot; import frc.robot.RobotContainer; -import frc.robot.commands.TrajectoryCommands.TrajectoryCreation; -import frc.robot.commands.TrajectoryCommands.FollowNote; public class PoseEstimator extends SubsystemBase { /** Creates a new PoseEstimator. */ - SwerveDrivePoseEstimator m_DrivePoseEstimator; - PhotonPoseEstimator m_PhotonPoseEstimatorFront; - PhotonPoseEstimator m_PhotonPoseEstimatorBack; - Vision m_vision; - Drivetrain m_drivetrain; - private Field2d m_field; - private boolean updateWithAprilTags; - + SwerveDrivePoseEstimator drivePoseEstimator; + PhotonPoseEstimator photonPoseEstimatorFront; + PhotonPoseEstimator photonPoseEstimatorBack; + Vision visionSubsystem; + Drivetrain driveSubsystem; + private Field2d fieldLayout; + Pose2d poseA = new Pose2d(); + Pose2d poseB = new Pose2d(); +StructPublisher publisher; +StructArrayPublisher arrayPublisher; public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(651.25); public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(323.25); @@ -53,23 +60,28 @@ public class PoseEstimator extends SubsystemBase { static PoseEstimator estimator = null; public PoseEstimator() { - m_drivetrain = Drivetrain.getInstance(); - m_vision = Vision.getVisionInstance(); - m_field = new Field2d(); - updateWithAprilTags = true; - SmartDashboard.putData("Field", m_field); + driveSubsystem = Drivetrain.getInstance(); + visionSubsystem = Vision.getVisionInstance(); + fieldLayout = new Field2d(); + SmartDashboard.putData("Field", fieldLayout); - m_DrivePoseEstimator = new SwerveDrivePoseEstimator( + drivePoseEstimator = new SwerveDrivePoseEstimator( Constants.SwerveConstants.swerveKinematics, - m_drivetrain.getNavxAngle(), - m_drivetrain.getPositions(), + driveSubsystem.getNavxAngle(), + driveSubsystem.getPositions(), new Pose2d(), stateStdDevs, visionMeasurementStdDevs ); - m_PhotonPoseEstimatorBack = m_vision.getVisionPose(); - m_PhotonPoseEstimatorFront = m_vision.getVisionPose2(); + // photonPoseEstimatorFront = visionSubsystem.getVisionPoseFront(); + photonPoseEstimatorBack = visionSubsystem.getVisionPoseBack(); + + publisher = NetworkTableInstance.getDefault() + .getStructTopic("MyPose", Pose2d.struct).publish(); +arrayPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("MyPoseArray", Pose2d.struct).publish(); + } public static PoseEstimator getPoseEstimatorInstance() { @@ -79,25 +91,24 @@ public static PoseEstimator getPoseEstimatorInstance() { return estimator; } - public void isUsingAprilTag(boolean b){ - updateWithAprilTags = b; - } - public void updateEachPoseEstimator(PhotonPoseEstimator poseEstimator, int camID){ + /* + pre-condition: poseEstimator != null and camID == 1 or camID == 2 + 1 = FRONT APRILTAG CAMERA + 2 = BACK APRILTAG CAMERA + */ - if(m_vision.getApriltagCamera(camID).getLatestResult().hasTargets()) { + public void updateEachPoseEstimator(PhotonPoseEstimator poseEstimator, int camID){ + if(visionSubsystem.getApriltagCamera().getLatestResult().hasTargets()) { poseEstimator.update().ifPresent(estimatedRobotPose -> { - System.out.println(1); var estimatedPose = estimatedRobotPose.estimatedPose; - System.out.println(estimatedRobotPose.targetsUsed.size()); + // m_DrivePoseEstimator.addVisionMeasurement(estimatedPose.toPose2d(), FIELD_LENGTH_METERS); // Make sure we have a new measurement, and that it's on the field - //current issue: once the robot sees an apriltag, hasTarget returns false?????? - // if(m_vision.getApriltagCamera(camID).getLatestResult().hasTargets()){ - if (m_vision.getApriltagCamera(camID).getLatestResult().getBestTarget().getFiducialId() >= 0){ - + if (visionSubsystem.getApriltagCamera().getLatestResult().getBestTarget().getFiducialId() >= 0){ + if ( estimatedRobotPose.timestampSeconds != previousPipelineTimestamp && estimatedPose.getX() >= 0.0 && estimatedPose.getX() <= FIELD_LENGTH_METERS @@ -105,29 +116,17 @@ public void updateEachPoseEstimator(PhotonPoseEstimator poseEstimator, int camID if (estimatedRobotPose.targetsUsed.size() >= 1) { for (PhotonTrackedTarget target : estimatedRobotPose.targetsUsed) { - Pose3d targetPose = m_vision.return_tag_pose(target.getFiducialId()); + Pose3d targetPose = visionSubsystem.return_tag_pose(target.getFiducialId()); Transform3d bestTarget = target.getBestCameraToTarget(); Pose3d camPose; - if (camID == 2){ - // Transform3d robotToCam = new Transform3d(bestTarget.getTranslation().plus(m_vision.getRobotToCam(camID).getTranslation()), new Rotation3d(0,0,0)); - // camPose = targetPose.transformBy(robotToCam.inverse()); - camPose = targetPose.transformBy(bestTarget.inverse().plus(m_vision.getRobotToCam(camID))); //.plus(new Transform3d(robotToCam, new Rotation3d(0,0,0))); - if(camPose.getRotation().toRotation2d().getDegrees() < 180) { - camPose.rotateBy( new Rotation3d(0, 0, 180)); - //camPose.plus(new Transform3d(new Translation3d(0,0,0), new Rotation3d(0, 0, 180))); - } else { - camPose.rotateBy( new Rotation3d(0, 0, -180)); - //camPose.plus(new Transform3d(new Translation3d(0,0,0), new Rotation3d(0, 0, -180))); - } - } - else { - camPose = targetPose.transformBy(bestTarget.inverse().plus(m_vision.getRobotToCam(camID))); //.plus(new Transform3d(robotToCam, new Rotation3d(0,0,0))); - } - System.out.println(camPose.toPose2d()); - // //checking from the camera to the tag is less than 4 + //Adding vision measurements from the center of the robot to the apriltag. Back camera should already be inverted + // camPose = targetPose.transformBy(bestTarget.inverse().plus(visionSubsystem.getRobotToCam().inverse())); //.plus(new Transform3d(robotToCam, new Rotation3d(0,0,0))); + camPose = targetPose.transformBy(bestTarget.inverse().plus(new Transform3d(new Translation3d(0.0,0.0,0.0), new Rotation3d(0,Units.degreesToRadians(35),Math.PI)))); //.plus(new Transform3d(robotToCam, new Rotation3d(0,0,0))); + + //checking the tags ambiguity. The lower the ambiguity, the more accurate the pose is if (target.getPoseAmbiguity() <= .2) { previousPipelineTimestamp = estimatedRobotPose.timestampSeconds; - m_DrivePoseEstimator.addVisionMeasurement(camPose.toPose2d(), estimatedRobotPose.timestampSeconds); + drivePoseEstimator.addVisionMeasurement(camPose.toPose2d(), estimatedRobotPose.timestampSeconds); } } } @@ -135,7 +134,7 @@ public void updateEachPoseEstimator(PhotonPoseEstimator poseEstimator, int camID else { previousPipelineTimestamp = estimatedRobotPose.timestampSeconds; - m_DrivePoseEstimator.addVisionMeasurement(estimatedPose.toPose2d(), estimatedRobotPose.timestampSeconds); + drivePoseEstimator.addVisionMeasurement(estimatedPose.toPose2d(), estimatedRobotPose.timestampSeconds); } } } @@ -148,29 +147,38 @@ public void updateEachPoseEstimator(PhotonPoseEstimator poseEstimator, int camID @Override public void periodic() { - m_DrivePoseEstimator.update(m_drivetrain.getNavxAngle(), m_drivetrain.getPositions()); + + //updates the drivePoseEstimator with with Navx angle and current wheel positions + drivePoseEstimator.update(driveSubsystem.getNavxAngle(), driveSubsystem.getPositions()); - if(m_PhotonPoseEstimatorBack != null){ - updateEachPoseEstimator(m_PhotonPoseEstimatorBack, 2); - } - if (m_PhotonPoseEstimatorFront != null){ - updateEachPoseEstimator(m_PhotonPoseEstimatorFront, 1); + //update each individual pose estimator + // if (photonPoseEstimatorFront != null){ + // updateEachPoseEstimator(photonPoseEstimatorFront, 1); + // } + if(photonPoseEstimatorBack != null){ + updateEachPoseEstimator(photonPoseEstimatorBack, 2); } - m_field.setRobotPose(getCurrentPose()); - SmartDashboard.putNumber("PoseEstimator X", getCurrentPose().getX()); - SmartDashboard.putNumber("PoseEstimator Y", getCurrentPose().getY()); - SmartDashboard.putNumber("PoseEstimator Angle", getCurrentPose().getRotation().getDegrees()); + publisher.set(drivePoseEstimator.getEstimatedPosition()); + //arrayPublisher.set(new Pose3d[] {poseA, poseB}); + + //updates the robot pose in the field simulation + fieldLayout.setRobotPose(getCurrentPose()); + + // SmartDashboard.putNumber("PoseEstimator X", getCurrentPose().getX()); + // SmartDashboard.putNumber("PoseEstimator Y", getCurrentPose().getY()); + // SmartDashboard.putNumber("PoseEstimator Angle", getCurrentPose().getRotation().getDegrees()); + // SmartDashboard.putNumber("PoseEstimator Radians", getCurrentPose().getRotation().getRadians()); } public Pose2d getCurrentPose() { - return m_DrivePoseEstimator.getEstimatedPosition(); + return drivePoseEstimator.getEstimatedPosition(); } public void setCurrentPose(Pose2d newPose) { - m_DrivePoseEstimator.resetPosition(m_drivetrain.getNavxAngle(), m_drivetrain.getPositions(), newPose); + drivePoseEstimator.resetPosition(driveSubsystem.getNavxAngle(), driveSubsystem.getPositions(), newPose); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index e344bbd..3b27632 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -75,11 +75,11 @@ public void periodic() { // SmartDashboard.putNumber("Speed Right Amp", Constants.ShooterConstants.shooterRightSpeedAmp); // SmartDashboard.putNumber("Outtake speed", Constants.IntakeConstants.rollerSpeedOuttake); - Constants.ShooterConstants.shooterLeftSpeedSpeaker = SmartDashboard.getNumber("Speed Left Speaker", Constants.ShooterConstants.shooterLeftSpeedSpeaker); - Constants.ShooterConstants.shooterRightSpeedSpeaker = SmartDashboard.getNumber("Speed Right Speaker", Constants.ShooterConstants.shooterRightSpeedSpeaker); - Constants.ShooterConstants.shooterLeftSpeedAmp = SmartDashboard.getNumber("Speed Left Amp", Constants.ShooterConstants.shooterLeftSpeedAmp); - Constants.ShooterConstants.shooterRightSpeedAmp = SmartDashboard.getNumber("Speed Right Amp", Constants.ShooterConstants.shooterRightSpeedAmp); - Constants.IntakeConstants.rollerSpeedOuttake = SmartDashboard.getNumber("Outtake speed", Constants.IntakeConstants.rollerSpeedOuttake); + // Constants.ShooterConstants.shooterLeftSpeedSpeaker = SmartDashboard.getNumber("Speed Left Speaker", Constants.ShooterConstants.shooterLeftSpeedSpeaker); + // Constants.ShooterConstants.shooterRightSpeedSpeaker = SmartDashboard.getNumber("Speed Right Speaker", Constants.ShooterConstants.shooterRightSpeedSpeaker); + // Constants.ShooterConstants.shooterLeftSpeedAmp = SmartDashboard.getNumber("Speed Left Amp", Constants.ShooterConstants.shooterLeftSpeedAmp); + // Constants.ShooterConstants.shooterRightSpeedAmp = SmartDashboard.getNumber("Speed Right Amp", Constants.ShooterConstants.shooterRightSpeedAmp); + // Constants.IntakeConstants.rollerSpeedOuttake = SmartDashboard.getNumber("Outtake speed", Constants.IntakeConstants.rollerSpeedOuttake); // This method will be called once per scheduler run } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveLib/COTSTalonFXSwerveConstants.java b/src/main/java/frc/robot/subsystems/SwerveLib/COTSTalonFXSwerveConstants.java deleted file mode 100644 index 4f92c8a..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveLib/COTSTalonFXSwerveConstants.java +++ /dev/null @@ -1,310 +0,0 @@ -package frc.robot.subsystems.SwerveLib; - - -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.SensorDirectionValue; - -import edu.wpi.first.math.util.Units; - -/* Contains values and required settings for common COTS swerve modules. */ -public class COTSTalonFXSwerveConstants { - public final double wheelDiameter; - public final double wheelCircumference; - public final double angleGearRatio; - public final double driveGearRatio; - public final double angleKP; - public final double angleKI; - public final double angleKD; - public final InvertedValue driveMotorInvert; - public final InvertedValue angleMotorInvert; - public final SensorDirectionValue cancoderInvert; - - public COTSTalonFXSwerveConstants(double wheelDiameter, double angleGearRatio, double driveGearRatio, double angleKP, double angleKI, double angleKD, InvertedValue driveMotorInvert, InvertedValue angleMotorInvert, SensorDirectionValue cancoderInvert){ - this.wheelDiameter = wheelDiameter; - this.wheelCircumference = wheelDiameter * Math.PI; - this.angleGearRatio = angleGearRatio; - this.driveGearRatio = driveGearRatio; - this.angleKP = angleKP; - this.angleKI = angleKI; - this.angleKD = angleKD; - this.driveMotorInvert = driveMotorInvert; - this.angleMotorInvert = angleMotorInvert; - this.cancoderInvert = cancoderInvert; - } - - /** West Coast Products */ - public static final class WCP { - /** West Coast Products - SwerveX Standard*/ - public static final class SwerveXStandard{ - /** West Coast Products - SwerveX Standard (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (396 / 35) : 1 */ - double angleGearRatio = ((396.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** West Coast Products - SwerveX Standard (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (396 / 35) : 1 */ - double angleGearRatio = ((396.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** WCP SwerveX Standard X1 - 10 Tooth - (7.85 : 1) */ - public static final double X1_10 = (7.85 / 1.0); - - /** WCP SwerveX Standard X1 - 11 Tooth - (7.13 : 1) */ - public static final double X1_11 = (7.13 / 1.0); - - /** WCP SwerveX Standard X1 - 12 Tooth - (6.54 : 1) */ - public static final double X1_12 = (6.54 / 1.0); - - /** WCP SwerveX Standard X2 - 10 Tooth - (6.56 : 1) */ - public static final double X2_10 = (6.56 / 1.0); - - /** WCP SwerveX Standard X2 - 11 Tooth - (5.96 : 1) */ - public static final double X2_11 = (5.96 / 1.0); - - /** WCP SwerveX Standard X2 - 12 Tooth - (5.46 : 1) */ - public static final double X2_12 = (5.46 / 1.0); - - /** WCP SwerveX Standard X3 - 12 Tooth - (5.14 : 1) */ - public static final double X3_12 = (5.14 / 1.0); - - /** WCP SwerveX Standard X3 - 13 Tooth - (4.75 : 1) */ - public static final double X3_13 = (4.75 / 1.0); - - /** WCP SwerveX Standard X3 - 14 Tooth - (4.41 : 1) */ - public static final double X3_14 = (4.41 / 1.0); - } - } - - /** West Coast Products - SwerveX Flipped*/ - public static final class SwerveXFlipped{ - /** West Coast Products - SwerveX Flipped (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (468 / 35) : 1 */ - double angleGearRatio = ((468.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** West Coast Products - SwerveX Flipped (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (468 / 35) : 1 */ - double angleGearRatio = ((468.0 / 35.0) / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** WCP SwerveX Flipped X1 - 10 Tooth - (8.10 : 1) */ - public static final double X1_10 = (8.10 / 1.0); - - /** WCP SwerveX Flipped X1 - 11 Tooth - (7.36 : 1) */ - public static final double X1_11 = (7.36 / 1.0); - - /** WCP SwerveX Flipped X1 - 12 Tooth - (6.75 : 1) */ - public static final double X1_12 = (6.75 / 1.0); - - /** WCP SwerveX Flipped X2 - 10 Tooth - (6.72 : 1) */ - public static final double X2_10 = (6.72 / 1.0); - - /** WCP SwerveX Flipped X2 - 11 Tooth - (6.11 : 1) */ - public static final double X2_11 = (6.11 / 1.0); - - /** WCP SwerveX Flipped X2 - 12 Tooth - (5.60 : 1) */ - public static final double X2_12 = (5.60 / 1.0); - - /** WCP SwerveX Flipped X3 - 10 Tooth - (5.51 : 1) */ - public static final double X3_10 = (5.51 / 1.0); - - /** WCP SwerveX Flipped X3 - 11 Tooth - (5.01 : 1) */ - public static final double X3_11 = (5.01 / 1.0); - - /** WCP SwerveX Flipped X3 - 12 Tooth - (4.59 : 1) */ - public static final double X3_12 = (4.59 / 1.0); - } - } - } - - /** Swerve Drive Specialities */ - public static final class SDS { - /** Swerve Drive Specialties - MK3 Module*/ - public static final class MK3{ - /** Swerve Drive Specialties - MK3 Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK3 Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK3 - (8.16 : 1) */ - public static final double Standard = (8.16 / 1.0); - /** SDS MK3 - (6.86 : 1) */ - public static final double Fast = (6.86 / 1.0); - } - } - - /** Swerve Drive Specialties - MK4 Module*/ - public static final class MK4{ - /** Swerve Drive Specialties - MK4 Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK4 Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** 12.8 : 1 */ - double angleGearRatio = (12.8 / 1.0); - - double angleKP = 1.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.CounterClockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK4 - (8.14 : 1) */ - public static final double L1 = (8.14 / 1.0); - /** SDS MK4 - (6.75 : 1) */ - public static final double L2 = (6.75 / 1.0); - /** SDS MK4 - (6.12 : 1) */ - public static final double L3 = (6.12 / 1.0); - /** SDS MK4 - (5.14 : 1) */ - public static final double L4 = (5.14 / 1.0); - } - } - - /** Swerve Drive Specialties - MK4i Module*/ - public static final class MK4i{ - /** Swerve Drive Specialties - MK4i Module (Falcon 500)*/ - public static final COTSTalonFXSwerveConstants Falcon500(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (150 / 7) : 1 */ - double angleGearRatio = ((150.0 / 7.0) / 1.0); - - double angleKP = 20.0; - double angleKI = 0.0; - double angleKD = 10.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - /** Swerve Drive Specialties - MK4i Module (Kraken X60)*/ - public static final COTSTalonFXSwerveConstants KrakenX60(double driveGearRatio){ - double wheelDiameter = Units.inchesToMeters(4.0); - - /** (150 / 7) : 1 */ - double angleGearRatio = ((150.0 / 7.0) / 1.0); - - double angleKP = 100.0; - double angleKI = 0.0; - double angleKD = 0.0; - - InvertedValue driveMotorInvert = InvertedValue.CounterClockwise_Positive; - InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; - SensorDirectionValue cancoderInvert = SensorDirectionValue.CounterClockwise_Positive; - return new COTSTalonFXSwerveConstants(wheelDiameter, angleGearRatio, driveGearRatio, angleKP, angleKI, angleKD, driveMotorInvert, angleMotorInvert, cancoderInvert); - } - - public static final class driveRatios{ - /** SDS MK4i - (8.14 : 1) */ - public static final double L1 = (8.14 / 1.0); - /** SDS MK4i - (6.75 : 1) */ - public static final double L2 = (6.75 / 1.0); - /** SDS MK4i - (6.12 : 1) */ - public static final double L3 = (6.12 / 1.0); - } - } - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveLib/CTREConfigs.java b/src/main/java/frc/robot/subsystems/SwerveLib/CTREConfigs.java index 568194f..3b01db7 100644 --- a/src/main/java/frc/robot/subsystems/SwerveLib/CTREConfigs.java +++ b/src/main/java/frc/robot/subsystems/SwerveLib/CTREConfigs.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.SwerveLib; import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; @@ -9,117 +8,13 @@ public final class CTREConfigs { public CANcoderConfiguration swerveCanCoderConfig; - public TalonFXConfiguration swerveAngleFXConfig; - public TalonFXConfiguration swerveDriveFXConfig; public CTREConfigs() { - swerveAngleFXConfig = new TalonFXConfiguration(); - swerveDriveFXConfig = new TalonFXConfiguration(); swerveCanCoderConfig = new CANcoderConfiguration(); /* Swerve CANCoder Configuration */ swerveCanCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1; //swerveCanCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; swerveCanCoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; - - - /* Drive Configs */ - swerveDriveFXConfig.MotorOutput.Inverted = Constants.SwerveConstants.driveInvert; - swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.SwerveConstants.driveNeutralMode; - - /* Limiting */ - swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = Constants.SwerveConstants.driveEnableCurrentLimit; - swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.SwerveConstants.driveContinuousCurrentLimit; - swerveDriveFXConfig.Feedback.SensorToMechanismRatio = Constants.SwerveConstants.driveGearRatio; - swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = Constants.SwerveConstants.driveCurrentThreshold; - swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = Constants.SwerveConstants.driveCurrentThresholdTime; - - /* PID */ - swerveDriveFXConfig.Slot0.kP = Constants.SwerveConstants.driveKP; - swerveDriveFXConfig.Slot0.kI = Constants.SwerveConstants.driveKI; - swerveDriveFXConfig.Slot0.kD = Constants.SwerveConstants.driveKD; - - /* Angle Configs */ - swerveAngleFXConfig.MotorOutput.Inverted = Constants.SwerveConstants.angleInvert; - swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.SwerveConstants.angleNeutralMode; - - /* Gear Ratio and Wrapping Config */ - swerveAngleFXConfig.Feedback.SensorToMechanismRatio = Constants.SwerveConstants.angleGearRatio; - swerveAngleFXConfig.ClosedLoopGeneral.ContinuousWrap = true; - - /* Current Limiting */ - swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = Constants.SwerveConstants.angleEnableCurrentLimit; - swerveAngleFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.SwerveConstants.angleContinuousCurrentLimit; - swerveAngleFXConfig.CurrentLimits.SupplyCurrentThreshold = Constants.SwerveConstants.angleCurrentThreshold; - swerveAngleFXConfig.CurrentLimits.SupplyTimeThreshold = Constants.SwerveConstants.angleCurrentThresholdTime; - - /* PID Config */ - swerveAngleFXConfig.Slot0.kP = Constants.SwerveConstants.angleKP; - swerveAngleFXConfig.Slot0.kI = Constants.SwerveConstants.angleKI; - swerveAngleFXConfig.Slot0.kD = Constants.SwerveConstants.angleKD; - - swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = Constants.SwerveConstants.openLoopRamp; - swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.SwerveConstants.openLoopRamp; - - swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = Constants.SwerveConstants.closedLoopRamp; - swerveDriveFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.SwerveConstants.closedLoopRamp; } -} - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveLib/Conversions.java b/src/main/java/frc/robot/subsystems/SwerveLib/Conversions.java deleted file mode 100644 index 1e647df..0000000 --- a/src/main/java/frc/robot/subsystems/SwerveLib/Conversions.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.subsystems.SwerveLib; - - -public class Conversions { - - /** - * @param wheelRPS Wheel Velocity: (in Rotations per Second) - * @param circumference Wheel Circumference: (in Meters) - * @return Wheel Velocity: (in Meters per Second) - */ - public static double RPSToMPS(double wheelRPS, double circumference){ - double wheelMPS = wheelRPS * circumference; - return wheelMPS; - } - - /** - * @param wheelMPS Wheel Velocity: (in Meters per Second) - * @param circumference Wheel Circumference: (in Meters) - * @return Wheel Velocity: (in Rotations per Second) - */ - public static double MPSToRPS(double wheelMPS, double circumference){ - double wheelRPS = wheelMPS / circumference; - return wheelRPS; - } - - /** - * @param wheelRotations Wheel Position: (in Rotations) - * @param circumference Wheel Circumference: (in Meters) - * @return Wheel Distance: (in Meters) - */ - public static double rotationsToMeters(double wheelRotations, double circumference){ - double wheelMeters = wheelRotations * circumference; - return wheelMeters; - } - - /** - * @param wheelMeters Wheel Distance: (in Meters) - * @param circumference Wheel Circumference: (in Meters) - * @return Wheel Position: (in Rotations) - */ - public static double metersToRotations(double wheelMeters, double circumference){ - double wheelRotations = wheelMeters / circumference; - return wheelRotations; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveLib/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveLib/SwerveModule.java new file mode 100644 index 0000000..6484dd0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/SwerveLib/SwerveModule.java @@ -0,0 +1,203 @@ + +package frc.robot.subsystems.SwerveLib; + +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkPIDController; +import com.revrobotics.CANSparkBase.ControlType; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants; +import frc.robot.Robot; +import frc.robot.subsystems.SwerveLib.CANSparkMaxUtil; +import frc.robot.subsystems.SwerveLib.OnboardModuleState; +import frc.robot.subsystems.SwerveLib.SwerveModuleConstants; +import frc.robot.subsystems.SwerveLib.CANSparkMaxUtil.Usage; + +public class SwerveModule { + public int moduleNumber; + private Rotation2d lastAngle; + private Rotation2d desiredAngle; + + private CANSparkMax angleMotor; + private CANSparkMax driveMotor; + + private RelativeEncoder driveEncoder; + private RelativeEncoder integratedAngleEncoder; + private CANcoder angleEncoder; + + private final SparkPIDController driveController; + private final SparkPIDController angleController; + + private final PIDController regController = + new PIDController(Constants.SwerveConstants.angleKP, Constants.SwerveConstants.angleKI, Constants.SwerveConstants.angleKD); + + private final SimpleMotorFeedforward feedforward = + new SimpleMotorFeedforward( + Constants.SwerveConstants.driveKS, Constants.SwerveConstants.driveKV, Constants.SwerveConstants.driveKA); + + private final PIDController turnFeedback = + new PIDController(0.1, 0.0, 0.0, 0.02); + + public SwerveModule(int moduleNumber, SwerveModuleConstants moduleConstants) { + this.moduleNumber = moduleNumber; + this.desiredAngle = moduleConstants.desiredAngle; + + /* Angle Encoder Config */ + angleEncoder = new CANcoder(moduleConstants.cancoderID); + configAngleEncoder(); + + /* Angle Motor Config */ + angleMotor = new CANSparkMax(moduleConstants.angleMotorID, MotorType.kBrushless); + integratedAngleEncoder = angleMotor.getEncoder(); + angleController = angleMotor.getPIDController(); + configAngleMotor(); + + /* Drive Motor Config */ + driveMotor = new CANSparkMax(moduleConstants.driveMotorID, MotorType.kBrushless); + driveEncoder = driveMotor.getEncoder(); + driveController = driveMotor.getPIDController(); + configDriveMotor(); + + lastAngle = getState().angle; + + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + } + + /* Desired state for each swerve module. takes in speed and angle. If its openLoop, that means it is in teleop */ + public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) { + // Custom optimize command, since default WPILib optimize assumes continuous controller which + // REV and CTRE are not + desiredState = OnboardModuleState.optimize(desiredState, getState().angle); + + setAngle(desiredState); + setSpeed(desiredState, isOpenLoop); + } + + /* Reset wheel orientation to forward */ + public void resetToAbsolute() { + double absolutePosition = getCanCoder().getDegrees() - desiredAngle.getDegrees(); + //integratedAngleEncoder.setPosition(integratedAngleEncoder.getPosition() - absolutePosition); + integratedAngleEncoder.setPosition(absolutePosition); + } + + /* Settings for Angle Encoder */ + private void configAngleEncoder() { + angleEncoder.getConfigurator().apply(Robot.ctreConfigs.swerveCanCoderConfig, 0.1); + } + + /* Settings for Angle Motor */ + private void configAngleMotor() { + angleMotor.restoreFactoryDefaults(); + angleMotor.setCANTimeout(5000); + CANSparkMaxUtil.setCANSparkMaxBusUsage(angleMotor, Usage.kPositionOnly); + angleMotor.setSmartCurrentLimit(Constants.SwerveConstants.angleContinuousCurrentLimit); + angleMotor.setInverted(Constants.SwerveConstants.angleInvert); + angleMotor.setIdleMode(Constants.SwerveConstants.angleNeutralMode); + integratedAngleEncoder.setPositionConversionFactor(Constants.SwerveConstants.angleConversionFactor); + angleController.setP(Constants.SwerveConstants.angleKP); + angleController.setI(Constants.SwerveConstants.angleKI); + angleController.setD(Constants.SwerveConstants.angleKD); + angleController.setFF(Constants.SwerveConstants.angleKFF); + angleMotor.enableVoltageCompensation(Constants.SwerveConstants.voltageComp); + angleMotor.burnFlash(); + resetToAbsolute(); + } + + /* Settings for Drive Motor */ + private void configDriveMotor() { + driveMotor.restoreFactoryDefaults(); + driveMotor.setCANTimeout(5000); + CANSparkMaxUtil.setCANSparkMaxBusUsage(driveMotor, Usage.kAll); + driveMotor.setSmartCurrentLimit(Constants.SwerveConstants.driveContinuousCurrentLimit); + driveMotor.setInverted(Constants.SwerveConstants.driveInvert); + driveMotor.setIdleMode(Constants.SwerveConstants.driveNeutralMode); + driveMotor.setOpenLoopRampRate(.25); + driveEncoder.setVelocityConversionFactor(Constants.SwerveConstants.driveConversionVelocityFactor); + driveEncoder.setPositionConversionFactor(Constants.SwerveConstants.driveConversionPositionFactor); + driveController.setP(Constants.SwerveConstants.angleKP); + driveController.setI(Constants.SwerveConstants.angleKI); + driveController.setD(Constants.SwerveConstants.angleKD); + driveController.setFF(Constants.SwerveConstants.angleKFF); + driveMotor.enableVoltageCompensation(Constants.SwerveConstants.voltageComp); + driveMotor.burnFlash(); + driveEncoder.setPosition(0.0); + } + + /* Gets the current position of the swerve module. This is an estimate */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition(driveMotor.getEncoder().getPosition(), getAngle()); + } + + /* Sets the speed of the swerve module. If it's openLoop, then it takes in a percentage, otherwise, it calculates and runs a PID */ + private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) { + if (isOpenLoop) { + double percentOutput = desiredState.speedMetersPerSecond / Constants.SwerveConstants.maxSpeed; + driveMotor.set(percentOutput); + } else { + driveController.setReference( + desiredState.speedMetersPerSecond, + ControlType.kVelocity, + 0, + feedforward.calculate(desiredState.speedMetersPerSecond)); + } + } + + /* Sets the angle of the swerve module. */ + private void setAngle(SwerveModuleState desiredState) { + // Prevent rotating module if speed is less then 1%. Prevents jittering. + Rotation2d angle = + (Math.abs(desiredState.speedMetersPerSecond) <= (Constants.SwerveConstants.maxSpeed * 0.01)) + ? lastAngle + : desiredState.angle; + + angleController.setReference(angle.getDegrees(), ControlType.kPosition); + lastAngle = angle; + + // double turnAngleError = Math.abs(angle.getDegrees() - integratedAngleEncoder.getPosition()); + + // double pidOut = regController.calculate(integratedAngleEncoder.getPosition(), angle.getDegrees()); + // // if robot is not moving, stop the turn motor oscillating + // if (turnAngleError < Constants.Swerve.stickDeadband + // && Math.abs(desiredState.speedMetersPerSecond) <= (Constants.Swerve.maxSpeed * 0.01)) + // pidOut = 0; + + // angleMotor.setVoltage(pidOut * RobotController.getBatteryVoltage()); + + // angleMotor.set(pidOut); + } + + + private Rotation2d getAngle() { + return Rotation2d.fromDegrees(integratedAngleEncoder.getPosition()); + } + + public Rotation2d getCanCoder() { + return Rotation2d.fromRotations(angleEncoder.getAbsolutePosition().getValueAsDouble()); + } + + public SwerveModuleState getState() { + return new SwerveModuleState(driveEncoder.getVelocity(), getAngle()); + } + + public double getVoltage() { + return driveMotor.getBusVoltage(); + } + + /** Returns the drive velocity in m/sec. */ + public double getCharacterizationVelocity() { + return driveEncoder.getVelocity(); + } + + public void runCharacterization(double volts) { + driveMotor.setVoltage(volts); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/TrajectoryConfiguration.java b/src/main/java/frc/robot/subsystems/TrajectoryConfiguration.java index c7c7f4e..6969332 100644 --- a/src/main/java/frc/robot/subsystems/TrajectoryConfiguration.java +++ b/src/main/java/frc/robot/subsystems/TrajectoryConfiguration.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathHolonomic; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -33,7 +34,7 @@ public TrajectoryConfiguration() { new PIDConstants(5, 0.0, 0.0), // Translation PID constants new PIDConstants(5, 0.0, 0.0), // Rotation PID constants 4.5, // Max module speed, in m/s - Units.inchesToMeters(13.02), // Drive base radius in meters. Distance from robot center to furthest module. + Units.inchesToMeters(19.09), // Drive base radius in meters. Distance from robot center to furthest module. new ReplanningConfig() // Default path replanning config. See the API for the options here ), () -> { @@ -109,6 +110,7 @@ public Command followPathManual(PathPlannerPath path) { ); } + public static TrajectoryConfiguration getInstance(){ if (trajectoryConfig == null){ trajectoryConfig = new TrajectoryConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 4522392..9b92087 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -7,6 +7,7 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.proto.Photon; import org.photonvision.PhotonUtils; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; @@ -14,9 +15,6 @@ import java.io.IOException; import java.util.ArrayList; import java.util.Optional; - -import javax.xml.crypto.dsig.Transform; - import org.photonvision.EstimatedRobotPose; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; @@ -39,9 +37,9 @@ public class Vision extends SubsystemBase { private static Transform3d robotToCamAprilFront; private static Transform3d robotToCamAprilBack; private static Transform3d robotToCamObject; - private Drivetrain m_drivetrain; - public PhotonPoseEstimator m_PoseEstimator1; - public PhotonPoseEstimator m_PoseEstimator2; + private Drivetrain driveSubsystem; + public PhotonPoseEstimator poseEstimatorFront; + public PhotonPoseEstimator poseEstimatorBack; static Vision visionInstance = null; @@ -56,7 +54,7 @@ public class Vision extends SubsystemBase { * @throws IOException **/ - public Vision(PhotonCamera cameraO, PhotonCamera cameraA) { + public Vision(PhotonCamera cameraObject, PhotonCamera cameraApriltagBack) { // tag 1 final Translation3d translation1 = new Translation3d(15.079472, 0.245872, 1.355852); final Quaternion q1 = new Quaternion(0.5, 0, 0, 0.8660254037844386); @@ -172,60 +170,81 @@ public Vision(PhotonCamera cameraO, PhotonCamera cameraA) { atList.add(tag16); robotInTagPose = new Pose2d(); - this.cameraApriltagFront = cameraA; - this.cameraApriltagBack = cameraO; - this.cameraObject = cameraO; + //this.cameraApriltagFront = cameraApriltagFront; + this.cameraApriltagBack = cameraApriltagBack; + this.cameraObject = cameraApriltagFront; resetRobotPose(); aprilTagFieldLayout = new AprilTagFieldLayout(atList, 16.451 , 8.211 ); - robotToCamAprilFront = new Transform3d(new Translation3d(0.0, Units.inchesToMeters(2.5), Units.inchesToMeters(31)), new Rotation3d()); //Cam mounted facing forward, half a meter forward of center, half a meter up from center. - - robotToCamAprilBack = new Transform3d(new Translation3d(0.0, Units.inchesToMeters(-1.5), Units.inchesToMeters(31)), new Rotation3d(0,15,180)); + robotToCamAprilBack = new Transform3d(new Translation3d(0.0,0.07,0.787), new Rotation3d(0,Units.degreesToRadians(35),Math.PI)); - robotToCamObject = new Transform3d(new Translation3d(0,-Units.inchesToMeters(1.5), Units.inchesToMeters(31)), new Rotation3d()); //0.20,-0.04 + robotToCamObject = new Transform3d(new Translation3d(0.0,-0.07,0.77), new Rotation3d(0, Units.degreesToRadians(-12), 0)); //0.20,-0.04 - m_PoseEstimator1 = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, this.cameraApriltagFront, robotToCamAprilFront); - m_PoseEstimator2 = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, this.cameraApriltagBack, robotToCamAprilBack); + //poseEstimatorFront = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, this.cameraApriltagFront, robotToCamAprilFront); + poseEstimatorBack = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, this.cameraApriltagBack, robotToCamAprilBack); - m_PoseEstimator1.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); - m_PoseEstimator2.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + driveSubsystem = Drivetrain.getInstance(); - m_drivetrain = Drivetrain.getInstance(); - - //m_PoseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + //m_PoseEstimatorFront.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } public static Vision getVisionInstance() { if (visionInstance == null) { - visionInstance = new Vision(new PhotonCamera(Constants.VisionConstants.cameraNameObject), new PhotonCamera(Constants.VisionConstants.cameraNameAprilTag)); + visionInstance = new Vision(new PhotonCamera(Constants.VisionConstants.cameraNameAprilTagFront), + new PhotonCamera(Constants.VisionConstants.cameraNameAprilTagBack)); } return visionInstance; } - public PhotonCamera getApriltagCamera(int id) { - if (id == 1 ) - return cameraApriltagFront; - else { - return cameraApriltagBack; + // public PhotonPoseEstimator getVisionPoseFront(){ + // return poseEstimatorFront; + // } + + public PhotonPoseEstimator getVisionPoseBack(){ + return poseEstimatorBack; + + } + + public PhotonCamera getApriltagCamera(){ + return cameraApriltagBack; //BACK CAMERA + } + + public Transform3d getRobotToCam(){ + return robotToCamAprilBack; + } + + public boolean hasCalibrationBack(){ + if (cameraApriltagBack.getDistCoeffs().equals(Optional.empty())){ + return false; } + return true; } - public Transform3d getRobotToCam(int id){ - if (id == 1){ - return robotToCamAprilFront; + public boolean hasTag(){ + if (cameraApriltagBack.getLatestResult().hasTargets()){ + if (cameraApriltagBack.getLatestResult().getBestTarget().getFiducialId() >= 0){ + return true; + } } - else { - return robotToCamAprilBack; + return false; + } + + public int tagID(){ + if (cameraApriltagBack.getLatestResult().hasTargets()){ + return cameraApriltagBack.getLatestResult().getBestTarget().getFiducialId(); } + return -1; } + + // getting the vision pose from the april tags public Pose2d getTagPose() { - PhotonPipelineResult result = cameraApriltagFront.getLatestResult(); + PhotonPipelineResult result = cameraApriltagBack.getLatestResult(); if (result.hasTargets()) { PhotonTrackedTarget bestTarget = result.getBestTarget(); @@ -261,18 +280,26 @@ public Pose3d return_camera_pose_tag(int id, PhotonPipelineResult results) { // photonvision pose estimator public Optional return_photon_pose(Pose2d latestPose) { - m_PoseEstimator1.setReferencePose(latestPose); - return m_PoseEstimator1.update(); + poseEstimatorFront.setReferencePose(latestPose); + return poseEstimatorFront.update(); } + //Object detection methods public double getTargetPitch(){ + + if (cameraObject.getLatestResult().hasTargets()){ PhotonPipelineResult result = cameraObject.getLatestResult(); return result.getBestTarget().getPitch(); + } + return -1; } public double getTargetYaw(){ + if (cameraObject.getLatestResult().hasTargets()){ PhotonPipelineResult result = cameraObject.getLatestResult(); return result.getBestTarget().getYaw(); + } + return -1; } public boolean hasTarget(){ @@ -282,6 +309,10 @@ public boolean hasTarget(){ return false; } + // public void switchPipeline(int id){ + // camera.setPipelineIndex(id); + // } + public double getNoteRange() { return PhotonUtils.calculateDistanceToTargetMeters( robotToCamObject.getZ(), @@ -290,44 +321,41 @@ public double getNoteRange() { Units.degreesToRadians(getTargetPitch())); } - - - public Transform2d getNoteSpace(){ // + public Transform3d getNoteSpace(){ // // - double range = - //note, the algorithm photonvision uses is the exact same as the limelight one, commented out below - PhotonUtils.calculateDistanceToTargetMeters( - robotToCamObject.getZ(), - Units.inchesToMeters(0), - 0, - Units.degreesToRadians(getTargetPitch())); - //return new Pose2d(PhotonUtils.estimateCameraToTargetTranslation(range, new Rotation2d(Units.degreesToRadians(getTargetYaw()))), new Rotation2d(Units.degreesToRadians(getTargetYaw()))); - Translation2d translation = PhotonUtils.estimateCameraToTargetTranslation(range, new Rotation2d(Units.degreesToRadians(-getTargetYaw()))); - return new Transform2d(translation, new Rotation2d()); //translation.getAngle().plus(m_drivetrain.getNavxAngle() + Transform3d camPose = cameraObject.getLatestResult().getBestTarget().getBestCameraToTarget(); + System.out.println("x " + camPose.getX() + "y " + camPose.getY()); + return camPose; + // double range = + // //note, the algorithm photonvision uses is the exact same as the limelight one, commented out below + // PhotonUtils.calculateDistanceToTargetMeters( + // robotToCamObject.getZ(), + // Units.inchesToMeters(0), + // 0, + // Units.degreesToRadians(getTargetPitch())); + // //return new Pose2d(PhotonUtils.estimateCameraToTargetTranslation(range, new Rotation2d(Units.degreesToRadians(getTargetYaw()))), new Rotation2d(Units.degreesToRadians(getTargetYaw()))); + // Translation2d translation = PhotonUtils.estimateCameraToTargetTranslation(range, new Rotation2d(Units.degreesToRadians(getTargetYaw()))); + // return new Transform2d(translation, new Rotation2d()); //translation.getAngle().plus(m_drivetrain.getNavxAngle() } - - - public PhotonPoseEstimator getVisionPose(){ - return m_PoseEstimator1; - } - - public PhotonPoseEstimator getVisionPose2(){ - return m_PoseEstimator2; - } + @Override public void periodic() { - if (cameraApriltagFront.getDistCoeffs().equals(Optional.empty())){ - System.out.println("NO CALIBRATION"); - } + // if (cameraApriltagBack.getDistCoeffs().equals(Optional.empty())){ + // System.out.println("NO CALIBRATION"); + // } // if (hasTarget()){ // SmartDashboard.putNumber("note x", getNoteSpace().getX()); // SmartDashboard.putNumber("note y", getNoteSpace().getY()); // } - SmartDashboard.putBoolean("Sees tag", cameraObject.getLatestResult().hasTargets()); + // SmartDashboard.putBoolean("Sees tag", cameraObject.getLatestResult().hasTargets()); + + // if (cameraObject != null && cameraObject.getLatestResult().hasTargets()){ + // SmartDashboard.putBoolean("has Object", cameraObject.getLatestResult().hasTargets()); + // } } diff --git a/src/main/java/frc/robot/util/PathPlannerUtil.java b/src/main/java/frc/robot/util/PathPlannerUtil.java new file mode 100644 index 0000000..223a639 --- /dev/null +++ b/src/main/java/frc/robot/util/PathPlannerUtil.java @@ -0,0 +1,51 @@ +package frc.robot.util; + +import java.io.File; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.Collections; +import java.util.List; +import java.util.stream.Stream; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPlannerTrajectory; + +import edu.wpi.first.wpilibj.Filesystem; + +//With credit to FRC 422 + +public class PathPlannerUtil { + public static List getExistingPaths() { + var path = Path.of(Filesystem.getDeployDirectory().getAbsolutePath(), "pathplanner"); + try (Stream stream = Files.walk(path)) { + // return Collections.emptyList(); + return stream.filter(x -> getFileExtension(x.toFile()) + .equals(".auto")) + .map(x -> getFileStem(x.toFile())) + .toList(); + } catch (IOException e) { + return Collections.emptyList(); + } + } + + + private static String getFileStem(File file) { + try { + String name = file.getName(); + return name.substring(0, name.lastIndexOf(".")); + } catch (Exception e) { + return ""; + } + } + + private static String getFileExtension(File file) { + try { + String name = file.getName(); + return name.substring(name.lastIndexOf(".")); + } catch (Exception e) { + return ""; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/ShuffleboardTabs.java b/src/main/java/frc/robot/util/ShuffleboardTabs.java new file mode 100644 index 0000000..932cec6 --- /dev/null +++ b/src/main/java/frc/robot/util/ShuffleboardTabs.java @@ -0,0 +1,200 @@ +// 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.util; + +import edu.wpi.first.math.estimator.PoseEstimator; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import frc.robot.subsystems.Climb; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.Vision; +import frc.robot.subsystems.SwerveLib.SwerveModule; + +/** Add your docs here. */ +public class ShuffleboardTabs { + private ShuffleboardTab driverTab; + private ShuffleboardTab drivetrainTab; + private ShuffleboardTab intakeTab; + private ShuffleboardTab poseEstimatorTab; + private ShuffleboardTab shooterTab; + private ShuffleboardTab visionTab; + private ShuffleboardTab climbTab; + + /* Driver Entries */ + + /* Debug Entries */ + private GenericEntry mod1_velocity; + private GenericEntry mod2_velocity; + private GenericEntry mod3_velocity; + private GenericEntry mod4_velocity; + + private GenericEntry mod1_angle; + private GenericEntry mod2_angle; + private GenericEntry mod3_angle; + private GenericEntry mod4_angle; + + + private GenericEntry cancoder1_angle; + private GenericEntry cancoder2_angle; + private GenericEntry cancoder3_angle; + private GenericEntry cancoder4_angle; + + private GenericEntry navxAngle; + + private GenericEntry limitForward; + private GenericEntry limitBackward; + private GenericEntry irSensor; + + private GenericEntry poseEstimatorX; + private GenericEntry poseEstimatorY; + private GenericEntry poseEstimatorAngle; + private GenericEntry poseEstimatorRadians; + + private GenericEntry shooterLeftCurrent; + private GenericEntry shooterRightCurrent; + + private GenericEntry hasCalibrationFront; + private GenericEntry hasCalibrationBack; + private GenericEntry hasTag; + private GenericEntry tagInSight; + + private GenericEntry limitLeftClimbUp; + private GenericEntry limitLeftClimbDown; + private GenericEntry limitRightClimbUp; + private GenericEntry limitRightClimbDown; + + /* Subsystems */ + private Drivetrain driveSubsystem; + private Intake intakeSubsystem; + private Shooter shooterSubsystem; + private Vision visionSubsystem; + private Climb climbSubsystem; + private frc.robot.subsystems.PoseEstimator poseEstimatorSubsystem; + private SwerveModuleState[] states; + private SwerveModule[] mods; + + + public void initButton(){ + /* Init subsystems */ + driveSubsystem = Drivetrain.getInstance(); + intakeSubsystem = Intake.getInstance(); + shooterSubsystem = Shooter.getInstance(); + visionSubsystem = Vision.getVisionInstance(); + climbSubsystem = Climb.getInstance(); + poseEstimatorSubsystem = frc.robot.subsystems.PoseEstimator.getPoseEstimatorInstance(); + states = driveSubsystem.getStates(); + mods = driveSubsystem.getModules(); + + /* Init tabs */ + driverTab = Shuffleboard.getTab("Driver Tab"); + drivetrainTab = Shuffleboard.getTab("Drivetrain Tab"); + intakeTab = Shuffleboard.getTab("Intake Tab"); + shooterTab = Shuffleboard.getTab("Shooter Tab"); + visionTab = Shuffleboard.getTab("Vision Tab"); + climbTab = Shuffleboard.getTab("Climb Tab"); + + poseEstimatorTab = Shuffleboard.getTab("Pose Estimator Tab"); + + /* Init entries */ + + mod1_velocity = drivetrainTab.add("Mod 1 Velocity", 0.0).getEntry(); + mod2_velocity = drivetrainTab.add("Mod 2 Velocity", 0.0).getEntry(); + mod3_velocity = drivetrainTab.add("Mod 3 Velocity", 0.0).getEntry(); + mod4_velocity = drivetrainTab.add("Mod 4 Velocity", 0.0).getEntry(); + + mod1_angle = drivetrainTab.add("Mod 1 Angle", 0.0).getEntry(); + mod2_angle = drivetrainTab.add("Mod 2 Angle", 0.0).getEntry(); + mod3_angle = drivetrainTab.add("Mod 3 Angle", 0.0).getEntry(); + mod4_angle = drivetrainTab.add("Mod 4 Angle", 0.0).getEntry(); + + cancoder1_angle = drivetrainTab.add("Cancoder 1 Angle", 0.0).getEntry(); + cancoder2_angle = drivetrainTab.add("Cancoder 2 Angle", 0.0).getEntry(); + cancoder3_angle = drivetrainTab.add("Cancoder 3 Angle", 0.0).getEntry(); + cancoder4_angle = drivetrainTab.add("Cancoder 4 Angle", 0.0).getEntry(); + + navxAngle = drivetrainTab.add("Navx Angle", 0.0).getEntry(); + + limitForward = intakeTab.add("Limit Forward?", false).getEntry(); + limitBackward = intakeTab.add("Limit Back?", false).getEntry(); + irSensor = intakeTab.add("IR Sensor", 0.0).getEntry(); + + poseEstimatorX = poseEstimatorTab.add("Pose Estimator X", 0.0).getEntry(); + poseEstimatorY = poseEstimatorTab.add("Pose Estimator Y", 0.0).getEntry(); + poseEstimatorAngle = poseEstimatorTab.add("Pose Estimator Angle",0.0).getEntry(); + poseEstimatorRadians = poseEstimatorTab.add("Pose Estimator Radians",0.0).getEntry(); + + shooterLeftCurrent = shooterTab.add("Shooter Left Current", 0.0).getEntry(); + shooterRightCurrent = shooterTab.add("Shooter Right Current", 0.0).getEntry(); + + limitLeftClimbUp = climbTab.add("Climb Left Up", false).getEntry(); + limitLeftClimbDown = climbTab.add("Climb Left Down", false).getEntry(); + + limitRightClimbUp = climbTab.add("Climb Right Up", false).getEntry(); + limitRightClimbDown = climbTab.add("Climb Right Down", false).getEntry(); + + + // hasCalibrationFront = visionTab.add("Front Camera Calibrated?", false).getEntry(); + // hasCalibrationBack = visionTab.add("Back Camera Calibrated?", false).getEntry(); + // hasTag = visionTab.add("Tag in sight?", false).getEntry(); + // tagInSight = visionTab.add("Tag ID (currently in sight)", -1).getEntry(); + + + } + + public void updateButtons(){ + mod1_velocity.setDouble(states[0].speedMetersPerSecond); + mod2_velocity.setDouble(states[1].speedMetersPerSecond); + mod3_velocity.setDouble(states[2].speedMetersPerSecond); + mod4_velocity.setDouble(states[3].speedMetersPerSecond); + + mod1_angle.setDouble(states[0].angle.getDegrees()); + mod2_angle.setDouble(states[1].angle.getDegrees()); + mod3_angle.setDouble(states[2].angle.getDegrees()); + mod4_angle.setDouble(states[3].angle.getDegrees()); + + cancoder1_angle.setDouble(mods[0].getCanCoder().getDegrees()); + cancoder2_angle.setDouble(mods[1].getCanCoder().getDegrees()); + cancoder3_angle.setDouble(mods[2].getCanCoder().getDegrees()); + cancoder4_angle.setDouble(mods[3].getCanCoder().getDegrees()); + + navxAngle.setDouble(driveSubsystem.getNavxAngle().getDegrees()); + + limitForward.setBoolean(intakeSubsystem.getIntakeLimitStateForward()); + limitBackward.setBoolean(intakeSubsystem.getIntakeLimitStateReverse()); + irSensor.setDouble(intakeSubsystem.getIRSensor()); + + poseEstimatorX.setDouble(poseEstimatorSubsystem.getCurrentPose().getX()); + poseEstimatorY.setDouble(poseEstimatorSubsystem.getCurrentPose().getY()); + poseEstimatorAngle.setDouble(poseEstimatorSubsystem.getCurrentPose().getRotation().getDegrees()); + poseEstimatorRadians.setDouble(poseEstimatorSubsystem.getCurrentPose().getRotation().getRadians()); + + shooterLeftCurrent.setDouble(shooterSubsystem.getCurrent()); + shooterRightCurrent.setDouble(shooterSubsystem.getCurrent()); + + // hasCalibrationFront.setBoolean(visionSubsystem.hasCalibrationFront()); + // hasCalibrationBack.setBoolean(visionSubsystem.hasCalibrationBack()); + // hasTag.setBoolean(visionSubsystem.hasTag()); + // tagInSight.setInteger(visionSubsystem.getTagID()); + + limitLeftClimbUp.setBoolean(climbSubsystem.getLeftLimitStateUp()); + limitLeftClimbDown.setBoolean(climbSubsystem.getLeftLimitStateDown()); + limitRightClimbUp.setBoolean(climbSubsystem.getRightLimitStateUp()); + limitRightClimbDown.setBoolean(climbSubsystem.getRightLimitStateUp()); + } + + + + + + + +} + + + diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index ff15fab..3ec4c12 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.3", + "version": "2024.2.6", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.3" + "version": "2024.2.6" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.3", + "version": "2024.2.6", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix5.json b/vendordeps/Phoenix5.json index 88a68dd..ff7359e 100644 --- a/vendordeps/Phoenix5.json +++ b/vendordeps/Phoenix5.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix5.json", "name": "CTRE-Phoenix (v5)", - "version": "5.33.0", + "version": "5.33.1", "frcYear": 2024, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -20,19 +20,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.33.0" + "version": "5.33.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.33.0" + "version": "5.33.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.0", + "version": "5.33.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +45,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.0", + "version": "5.33.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -60,7 +60,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -75,7 +75,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -90,7 +90,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -105,7 +105,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -120,7 +120,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -135,7 +135,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.33.0", + "version": "5.33.1", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 86329e3..60eacf8 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.2", + "version": "2024.2.3", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.2" + "version": "2024.2.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.2", + "version": "2024.2.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.2", + "version": "2024.2.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.2", + "version": "2024.2.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8c56503..e024f06 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.8", + "version": "v2024.2.10", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.8", + "version": "v2024.2.10", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.8", + "version": "v2024.2.10", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.8" + "version": "v2024.2.10" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.8" + "version": "v2024.2.10" } ] } \ No newline at end of file