diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index acd4977..e5c98a5 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -6,10 +6,9 @@ "New Folder" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 1.5, + "defaultMaxAccel": 1.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, - "maxModuleSpeed": 4.5, - "choreoProjectPath": null + "maxModuleSpeed": 4.5 } \ No newline at end of file diff --git a/build.gradle b/build.gradle index 1ba507c..ff5a9b0 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.2.1" + id "edu.wpi.first.GradleRIO" version "2024.3.1" id 'com.diffplug.spotless' version '6.20.0' id "com.peterabeles.gversion" version "1.10" } diff --git a/shuffleboard.json b/shuffleboard.json new file mode 100644 index 0000000..3716c61 --- /dev/null +++ b/shuffleboard.json @@ -0,0 +1,465 @@ +{ + "tabPane": [ + { + "title": "SmartDashboard", + "autoPopulate": true, + "autoPopulatePrefix": "SmartDashboard/", + "widgetPane": { + "gridSize": 256.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Branch", + "_title": "Branch", + "_glyph": 148, + "_showGlyph": false + } + }, + "2,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Commit", + "_title": "Commit", + "_glyph": 148, + "_showGlyph": false + } + }, + "3,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left Encoder Speed (M/s)", + "_title": "Left Encoder Speed (M/s)", + "_glyph": 148, + "_showGlyph": false + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right Encoder Speed (M/s)", + "_title": "Right Encoder Speed (M/s)", + "_glyph": 148, + "_showGlyph": false + } + }, + "1,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance L", + "_title": "Distance L", + "_glyph": 148, + "_showGlyph": false + } + }, + "2,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Distance R", + "_title": "Distance R", + "_glyph": 148, + "_showGlyph": false + } + }, + "3,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Current Robot Location X axis", + "_title": "Current Robot Location X axis", + "_glyph": 148, + "_showGlyph": false + } + }, + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Grid Layout", + "_title": "Grid Layout", + "_glyph": 149, + "_showGlyph": false, + "Layout/Number of columns": 3, + "Layout/Number of rows": 3, + "Layout/Label position": "BOTTOM", + "_children": { + "0,0": { + "_type": "Subsystem Layout", + "_title": "Subsystem Layout", + "_glyph": 149, + "_showGlyph": false, + "Layout/Label position": "BOTTOM", + "_children": [ + { + "_type": "Split Button Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "Auto Mode", + "_glyph": 148, + "_showGlyph": false + } + ] + } + } + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Ring Distance", + "_title": "Ring Distance", + "_glyph": 148, + "_showGlyph": false + } + }, + "4,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Average Distance Traveled", + "_title": "Average Distance Traveled", + "_glyph": 148, + "_showGlyph": false + } + } + } + } + }, + { + "title": "LiveWindow", + "autoPopulate": true, + "autoPopulatePrefix": "LiveWindow/", + "widgetPane": { + "gridSize": 256.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": {} + } + }, + { + "title": "Main Dashboard", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "1,1": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Field", + "_title": "Field", + "_glyph": 148, + "_showGlyph": false, + "Game/Game": "k2024Crescendo", + "Visuals/Robot Icon Size": 50.0, + "Visuals/Show Outside Circles": false + } + }, + "3,0": { + "size": [ + 3, + 2 + ], + "content": { + "_type": "Differential Drivebase", + "_title": "Differential Drivebase", + "_glyph": 148, + "_showGlyph": false, + "Wheels/Number of wheels": 4, + "Wheels/Wheel diameter": 40.0, + "Visuals/Show velocity vectors": true + } + }, + "0,0": { + "size": [ + 3, + 1 + ], + "content": { + "_type": "Basic FMS Info", + "_title": "Basic FMS Info", + "_glyph": 148, + "_showGlyph": false + } + }, + "6,0": { + "size": [ + 3, + 3 + ], + "content": { + "_type": "Camera Stream", + "_title": "Camera Stream", + "_glyph": 148, + "_showGlyph": false, + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + }, + "9,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "/SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + }, + "9,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "/SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + }, + "0,3": { + "size": [ + 3, + 1 + ], + "content": { + "_type": "Split Button Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "/SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + }, + "4,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/At Goal", + "_title": "/SmartDashboard/At Goal", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + } + } + } + }, + { + "title": "Driver Dashboard", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "0,0": { + "size": [ + 4, + 3 + ], + "content": { + "_type": "Field", + "_title": "Field", + "_glyph": 148, + "_showGlyph": false, + "Game/Game": "k2024Crescendo", + "Visuals/Robot Icon Size": 50.0, + "Visuals/Show Outside Circles": false + } + }, + "4,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Arm Mode", + "_title": "/SmartDashboard/Arm Mode", + "_glyph": 148, + "_showGlyph": false + } + }, + "4,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/At Goal", + "_title": "/SmartDashboard/At Goal", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Manual Arm Mode Enabled", + "_title": "/SmartDashboard/Manual Arm Mode Enabled", + "_glyph": 148, + "_showGlyph": false, + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Simple Dial", + "_source0": "network_table:///SmartDashboard/Current Arm Angle (Degrees) (Absolute)", + "_title": "Arm Angle", + "_glyph": 148, + "_showGlyph": true, + "Range/Min": 20.0, + "Range/Max": 110.0, + "Visuals/Show value": true + } + }, + "3,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Current Goal Angle", + "_title": "/SmartDashboard/Current Goal Angle", + "_glyph": 148, + "_showGlyph": false + } + }, + "2,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Current Arm Angle (Degrees) (Relative)", + "_title": "/SmartDashboard/Current Arm Angle (Degrees) (Relative)", + "_glyph": 148, + "_showGlyph": false + } + }, + "5,3": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/SendableChooser[0]", + "_title": "SmartDashboard/SendableChooser[0]", + "_glyph": 148, + "_showGlyph": false + } + }, + "5,0": { + "size": [ + 4, + 3 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://USB Camera 0", + "_title": "USB Camera 0", + "_glyph": 148, + "_showGlyph": false, + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": false, + "Controls/Rotation": "QUARTER_CW", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + } + } + } + } + ], + "windowGeometry": { + "x": 0.800000011920929, + "y": 0.800000011920929, + "width": 1500.0, + "height": 814.4000244140625 + } +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DoNothing.auto b/src/main/deploy/pathplanner/autos/DoNothing.auto new file mode 100644 index 0000000..07d4e33 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DoNothing.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SFR.auto b/src/main/deploy/pathplanner/autos/SFR.auto new file mode 100644 index 0000000..fbe87c2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SFR.auto @@ -0,0 +1,88 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.75, + "y": 4.35 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "BrakeCommand" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SFR1" + } + }, + { + "type": "path", + "data": { + "pathName": "SFR2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto new file mode 100644 index 0000000..910c880 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerCenter.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6727105761903848, + "y": 4.635503404956002 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto new file mode 100644 index 0000000..e43ebcc --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerFar.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "ShootSpeakerPath" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerLeft.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerLeft.auto new file mode 100644 index 0000000..75e9d22 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerLeft.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7167669684840067, + "y": 6.745441850370149 + }, + "rotation": -122.66091272167378 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ShootSpeakerRight.auto b/src/main/deploy/pathplanner/autos/ShootSpeakerRight.auto new file mode 100644 index 0000000..5654769 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ShootSpeakerRight.auto @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.787031987758696, + "y": 4.356431195030721 + }, + "rotation": 120.96375653207355 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimSpeakerCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ArmCommand" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "ShooterCommand" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimLowerCommand" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLine" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SFR1.path b/src/main/deploy/pathplanner/paths/SFR1.path new file mode 100644 index 0000000..e23f40d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SFR1.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.75, + "y": 4.35 + }, + "prevControl": null, + "nextControl": { + "x": 1.8567437134823315, + "y": 2.969949786055847 + }, + "isLocked": false, + "linkedName": "SpeakerStart" + }, + { + "anchor": { + "x": 2.3500908540278633, + "y": 1.7464488775131404 + }, + "prevControl": { + "x": 0.9374285533592253, + "y": 1.7464488775131404 + }, + "nextControl": { + "x": 4.138203120548443, + "y": 1.7464488775131404 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.957431157356543, + "y": 1.3650562174103558 + }, + "prevControl": { + "x": 6.957431157356543, + "y": 1.3650562174103558 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MidStart" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -4.382714348011046, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 120.02871427926773, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/SFR2.path b/src/main/deploy/pathplanner/paths/SFR2.path new file mode 100644 index 0000000..b104aa3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SFR2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 7.957431157356543, + "y": 1.3650562174103558 + }, + "prevControl": null, + "nextControl": { + "x": 7.957431157356543, + "y": 2.2351584729206 + }, + "isLocked": false, + "linkedName": "MidStart" + }, + { + "anchor": { + "x": 7.957431157356543, + "y": 7.222602137511967 + }, + "prevControl": { + "x": 7.957431157356543, + "y": 6.272602137511967 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MidEnd" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.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/ShootSpeakerPath.path b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path new file mode 100644 index 0000000..6c0dff1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ShootSpeakerPath.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4, + "y": 1.7313445053430379 + }, + "prevControl": null, + "nextControl": { + "x": 2.439730735385641, + "y": 1.722415665210475 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4, + "y": 5.5 + }, + "prevControl": { + "x": 2.4, + "y": 5.5 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.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/SpeakerToLine.path b/src/main/deploy/pathplanner/paths/SpeakerToLine.path new file mode 100644 index 0000000..87901a8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/SpeakerToLine.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.118024005101129, + "y": 4.635503404956002 + }, + "prevControl": null, + "nextControl": { + "x": 1.5654159830259637, + "y": 3.041080915880077 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5, + "y": 2.199257239161131 + }, + "prevControl": { + "x": 1.3231422106608646, + "y": 3.5454931848085423 + }, + "nextControl": { + "x": 3.676857789339134, + "y": 0.85302129351372 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.957431157356543, + "y": 1.3650562174103558 + }, + "prevControl": { + "x": 6.957431157356543, + "y": 1.3650562174103558 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -4.382714348011046, + "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/TeleopAmpPath.path b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path new file mode 100644 index 0000000..dcfd3d5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TeleopAmpPath.path @@ -0,0 +1,65 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.298695211705768, + "y": 4.3985144783186625 + }, + "prevControl": null, + "nextControl": { + "x": 7.438598433871163, + "y": 5.888245796954481 + }, + "isLocked": false, + "linkedName": "MidPoint" + }, + { + "anchor": { + "x": 6.222672214394183, + "y": 6.540894375087692 + }, + "prevControl": { + "x": 8.13247757990561, + "y": 5.745679746402094 + }, + "nextControl": { + "x": 4.3128668488827575, + "y": 7.336109003773291 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8051986219412255, + "y": 7.422493182165216 + }, + "prevControl": { + "x": 1.8051986219412255, + "y": 5.90180254959067 + }, + "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": -3.1134387480333885, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path similarity index 63% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/TeleopSourcePath.path index a3fe7a4..14d7952 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/TeleopSourcePath.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 8.231349922415486, + "y": 2.441390285624172 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 9.231349922415486, + "y": 1.441390285624172 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 15.398883453656053, + "y": 1.1470264049073142 }, "prevControl": { - "x": 4.0, - "y": 6.0 - }, - "nextControl": { - "x": 6.0, - "y": 4.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.0, - "y": 1.0 - }, - "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 14.995556428276537, + "y": 1.1470264049073142 }, "nextControl": null, "isLocked": false, @@ -54,7 +38,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.0, + "velocity": 0, "rotation": 0, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path b/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path index 09f662d..c33b7a8 100644 --- a/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path +++ b/src/main/deploy/pathplanner/paths/TeleopSpeakerPath.path @@ -3,25 +3,41 @@ "waypoints": [ { "anchor": { - "x": 5.0, - "y": 5.0 + "x": 8.298695211705768, + "y": 4.3985144783186625 }, "prevControl": null, "nextControl": { - "x": 6.0, - "y": 4.0 + "x": 9.712908774078864, + "y": 4.3985144783186625 + }, + "isLocked": false, + "linkedName": "MidPoint" + }, + { + "anchor": { + "x": 5.303155179058627, + "y": 4.806135432132297 + }, + "prevControl": { + "x": 7.312495893755382, + "y": 4.221098166502594 + }, + "nextControl": { + "x": 3.293814464361872, + "y": 5.391172697762 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 0.9994362713898646, + "y": 5.545540883229574 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 1.681964380095043, + "y": 5.469704426706775 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 318f2c5..cadf0ae 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -22,7 +22,8 @@ public static final class CANConstants { /// Arm Motors public static final int MOTORARMMAINID = 21; /// Shooter Motors - public static final int MOTORSHOOTERID = 31; + public static final int MOTORSHOOTERLEFTID = 31; + public static final int MOTORSHOOTERRIGHTID = 32; /// Lifter Motors public static final int MOTORLIFTERLEFTID = 41; public static final int MOTORLIFTERRIGHTID = 42; @@ -41,12 +42,37 @@ public static final class CANConstants { // Now In RobotContainer as Native Triggers. // Joystick buttons + public static final int TRIGGER = 1; + public static final int ARMAMPBUTTON = 3; + public static final int ARMSPEAKERBUTTON = 4; + public static final int ARMDEFAULTBUTTON = 2; + public static final int ARMLOADBUTTON = 11; + public static final int ARMTRAPBUTTON = 9; + public static final int ENABLEAXISBUTTON = 10; + public static final int ARMDISABLEENCODER = 8; public static final int AIMBUTTON = 12; - public static final int FIREBUTTON = 0; // Analog Ports /// Ultrasonic Sensors and ports. public static final int ULTRASONICSHOOTERPORT = 0; // Digital Ports + public static final int ARMLIMITSWITCHFRONT = 0; + public static final int ARMLIMITSWITCHBACK = 1; + + // Shooter Angles + public static final double ARMENCODEROFFSET = -2.45; + public static final double ARMSTARTINGANGLE = 22.5 + ARMENCODEROFFSET; // WHY MaTH HURT + public static final double ARMMINRELATVESTART = 0.0; + public static final double ARMLOADANGLE = 35 - ARMSTARTINGANGLE; + public static final double ARMSPEAKERANGLE = 59 - ARMSTARTINGANGLE; // to go to 75 you just put 75 + public static final double ARMAMPANGLE = 110 - ARMSTARTINGANGLE; + public static final double ARMTRAPANGLE = 63 - ARMSTARTINGANGLE; + public static final double ARMMAXRELATIVE = 120 - ARMSTARTINGANGLE; + // Shooter Speeds (M/s) + public static final double SHOOTERSOURCE = -6.0; + public static final double SHOOTERAMP = 3; + public static final double SHOOTERSPEAKER = 20.0; + public static final double SHOOTERTRAP = 8.0; + public static final double SHOOTERDEFAULT = 8.0; // somewhere around 8 is the cap - ma } diff --git a/src/main/java/frc/robot/DriveConstants.java b/src/main/java/frc/robot/DriveConstants.java index 947f9f4..cc5b575 100644 --- a/src/main/java/frc/robot/DriveConstants.java +++ b/src/main/java/frc/robot/DriveConstants.java @@ -31,21 +31,21 @@ public final class DriveConstants { // The Robot Characterization Toolsuite provides a convenient tool for obtaining these // values for your robot. // Feed Forward Constants - public static final double ksDriveVolts = 0.16213; - public static final double kvDriveVoltSecondsPerMeter = 2.2194; - public static final double kaDriveVoltSecondsSquaredPerMeter = 0.33599; + public static final double ksDriveVolts = 0.19676; + public static final double kvDriveVoltSecondsPerMeter = 2.2623; + public static final double kaDriveVoltSecondsSquaredPerMeter = 0.43785; // Max speed Constants - public static final double kMaxOutputDrive = 1.0; - public static final double kMinOutputDrive = -1.0; + public static final double kMaxOutputDrive = 0.8; + public static final double kMinOutputDrive = -0.8; // Feed Back / PID Constants - public static final double kPDriveVel = 0.00034388; + public static final double kPDriveVel = 0.00088622; public static final double kIDriveVel = 0.0; public static final double kDDriveVel = 0.0; public static final double kIzDriveVel = 0.0; // error before integral takes effect - public static final double kPDrivePos = 5.7745; + public static final double kPDrivePos = 4.6269; public static final double kIDrivePos = 0.0; - public static final double kDDrivePos = 0.55289; + public static final double kDDrivePos = 0.49649; public static final double kIzDrivePos = 0.0; // error before integral takes effect // Helper class that converts a chassis velocity (dx and dtheta components) to left and right // wheel velocities for a differential drive. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9ad2067..b7dc170 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,15 +26,20 @@ import frc.robot.commands.ArmCommand; import frc.robot.commands.BalanceCommand; import frc.robot.commands.DefaultDrive; -import frc.robot.commands.LifterDownCommand; -import frc.robot.commands.LifterUpCommand; +import frc.robot.commands.LeftLifterCommand; +import frc.robot.commands.RightLifterCommand; import frc.robot.commands.ShooterCommand; import frc.robot.commands.StraightCommand; import frc.robot.commands.UltrasonicShooterCommand; +import frc.robot.commands.auto.AimAmpCommand; +import frc.robot.commands.auto.AimSpeakerCommand; +import frc.robot.commands.auto.LowerArmCommand; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; -import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.DriverCameraSubsystem; +import frc.robot.subsystems.LeftLifterSubsystem; +import frc.robot.subsystems.RightLifterSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.UltrasonicSubsystem; @@ -62,9 +67,11 @@ public class RobotContainer { private final DriveSubsystem m_driveSubsystem = new DriveSubsystem(); private final CameraSubsystem m_cameraSubsystem = new CameraSubsystem(m_driveSubsystem); - private final ArmSubsystem m_armSubsystem = new ArmSubsystem(); + private final ArmSubsystem m_armSubsystem = new ArmSubsystem(m_shooterState); private final ShooterSubsystem m_shooterSubsytem = new ShooterSubsystem(); - private final LifterSubsystem m_lifterSubsystem = new LifterSubsystem(); + private final LeftLifterSubsystem m_leftLifterSubsystem = new LeftLifterSubsystem(); + private final RightLifterSubsystem m_rightLifterSubsystem = new RightLifterSubsystem(); + private final DriverCameraSubsystem m_DriverCameraSubsystem = new DriverCameraSubsystem(); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); @@ -78,20 +85,44 @@ public class RobotContainer { new UltrasonicShooterCommand(m_ultrasonicShooterSubsystem, m_shooterState); private final ArmCommand m_armCommand = new ArmCommand(m_armSubsystem, m_shooterState, this::GetFlightStickY); - private final ShooterCommand m_shooterCommand = new ShooterCommand(m_shooterSubsytem); - private final LifterUpCommand m_lifterUpCommand = new LifterUpCommand(m_lifterSubsystem); - private final LifterDownCommand m_lifterDownCommand = new LifterDownCommand(m_lifterSubsystem); + private final ShooterCommand m_shooterCommand = + new ShooterCommand(m_shooterSubsytem, m_shooterState); + private final LeftLifterCommand m_LeftLifterCommand = + new LeftLifterCommand(m_leftLifterSubsystem); + private final RightLifterCommand m_RightLifterCommand = + new RightLifterCommand(m_rightLifterSubsystem); + // these commands are used by autonomous only + private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, m_shooterState); + private final LowerArmCommand m_AimLowerCommand = + new LowerArmCommand(m_armSubsystem, m_shooterState); + private final AimSpeakerCommand m_AimSpeakerCommand = + new AimSpeakerCommand(m_armSubsystem, m_shooterState); + + // this command is used by on the fly path planning private Command m_driveToSpeaker; + private Command m_driveToAmp; + private Command m_driveToSource; // Init Buttons - private Trigger m_balanceButton; + // private Trigger m_balanceButton; private Trigger m_straightButton; private Trigger m_toggleBrakeButton; - private Trigger m_lifterUpButton; - private Trigger m_lifterDownButton; - private JoystickButton m_aimButton; - private JoystickButton m_fireButton; + private Trigger m_lifterRightButton; + private Trigger m_lifterLeftButton; private Trigger m_driveToSpeakerButton; + private Trigger m_driveToSourceButton; + private Trigger m_driveToAmpButton; + private Trigger m_lifterDirectionButton; + // joystick buttons + private JoystickButton m_aimButton; + private JoystickButton m_armRaiseToSpeakerButton; + private JoystickButton m_armRaiseToAmpButton; + private JoystickButton m_armRaiseToTrapButton; + private JoystickButton m_armWheelLoadButton; + private JoystickButton m_enableAxisButton; + private JoystickButton m_shooterTrigger; + private JoystickButton m_armRaiseToDefault; + private JoystickButton m_armDisableEncoderButton; // Init For Autonomous // private RamseteAutoBuilder autoBuilder; private SendableChooser autoDashboardChooser = new SendableChooser(); @@ -107,8 +138,8 @@ public RobotContainer() { setupTriggers(); // Bind the commands to the triggers if (enableAutoProfiling) { - bindDriveSysIDCommands(); - // bindArmSysIDCommands(); + // bindDriveSysIDCommands(); + bindArmSysIDCommands(); // bindShooterSysIDCommands(); } else { bindCommands(); @@ -131,27 +162,56 @@ public RobotContainer() { private void setupTriggers() { // Controller buttons m_toggleBrakeButton = m_controller1.x(); - m_lifterUpButton = m_controller1.a(); - m_lifterDownButton = m_controller1.b(); - m_balanceButton = m_controller1.rightBumper(); - m_straightButton = m_controller1.rightTrigger(); + m_straightButton = m_controller1.rightBumper(); + m_lifterRightButton = m_controller1.rightTrigger(); + m_lifterLeftButton = m_controller1.leftTrigger(); m_driveToSpeakerButton = m_controller1.y(); + m_driveToSourceButton = m_controller1.b(); + m_lifterDirectionButton = m_controller1.a(); // Joystick buttons m_aimButton = new JoystickButton(m_flightStick, Constants.AIMBUTTON); - m_fireButton = new JoystickButton(m_flightStick, Constants.FIREBUTTON); + // arm raise buttons + m_armWheelLoadButton = new JoystickButton(m_flightStick, Constants.ARMLOADBUTTON); + m_armDisableEncoderButton = new JoystickButton(m_flightStick, Constants.ARMDISABLEENCODER); + m_armRaiseToSpeakerButton = new JoystickButton(m_flightStick, Constants.ARMSPEAKERBUTTON); + m_armRaiseToAmpButton = new JoystickButton(m_flightStick, Constants.ARMAMPBUTTON); + m_armRaiseToTrapButton = new JoystickButton(m_flightStick, Constants.ARMTRAPBUTTON); + m_enableAxisButton = new JoystickButton(m_flightStick, Constants.ENABLEAXISBUTTON); + m_armRaiseToDefault = new JoystickButton(m_flightStick, Constants.ARMDEFAULTBUTTON); + + // load and shoot buttons + m_shooterTrigger = new JoystickButton(m_flightStick, Constants.TRIGGER); } private void bindCommands() { // commands - m_balanceButton.whileTrue(m_balanceCommand); + // m_balanceButton.whileTrue(m_balanceCommand); m_straightButton.whileTrue(m_straightCommand); m_aimButton.whileTrue(m_aimCommand); - m_fireButton.whileTrue(m_shooterCommand); m_driveToSpeakerButton.whileTrue(m_driveToSpeaker); - m_lifterUpButton.whileTrue(m_lifterUpCommand); - m_lifterDownButton.whileTrue(m_lifterDownCommand); + // m_driveToAmpButton.whileTrue(m_driveToAmp); // TODO: Need to bind button + m_driveToSourceButton.whileTrue(m_driveToSource); + m_lifterRightButton.whileTrue(m_RightLifterCommand); + m_lifterLeftButton.whileTrue(m_LeftLifterCommand); + m_lifterDirectionButton.whileTrue( + new InstantCommand(() -> m_leftLifterSubsystem.changeDirection()) + .andThen(new InstantCommand(() -> m_rightLifterSubsystem.changeDirection()))); m_toggleBrakeButton.whileTrue(new InstantCommand(() -> m_driveSubsystem.SwitchBrakemode())); + // shooter + arm commands + m_shooterTrigger.whileTrue(m_shooterCommand); + m_armDisableEncoderButton.whileTrue(new InstantCommand(() -> m_armSubsystem.disableOffset())); + m_armRaiseToSpeakerButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER))); + m_armRaiseToAmpButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.AMP))); + m_armWheelLoadButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.SOURCE))); + m_armRaiseToTrapButton.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.TRAP))); + m_armRaiseToDefault.whileTrue( + new InstantCommand(() -> m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT))); + m_enableAxisButton.whileTrue(new InstantCommand(() -> m_shooterState.toggleAxis())); } private void bindDriveSysIDCommands() { @@ -168,6 +228,7 @@ private void bindArmSysIDCommands() { m_controller1.x().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward)); m_controller1.y().whileTrue(m_armSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse)); m_controller1.leftTrigger().whileTrue(new InstantCommand(() -> DataLogManager.stop())); + m_armSubsystem.disablePID(); } private void bindShooterSysIDCommands() { @@ -184,9 +245,12 @@ private void bindShooterSysIDCommands() { private void initializeAutonomous() { // Network Table Routine Options - autoDashboardChooser.setDefaultOption("Auto With Balancing", "FullAuto"); - autoDashboardChooser.setDefaultOption("DriveForward", "DriveForward"); - autoDashboardChooser.addOption("End at cones", "EndAtCones"); + autoDashboardChooser.setDefaultOption("SFR", "SFR"); + autoDashboardChooser.addOption("ShootSpeakerCenter", "ShootSpeakerCenter"); + autoDashboardChooser.addOption("ShootSpeakerLeft", "ShootSpeakerLeft"); + autoDashboardChooser.addOption("ShootSpeakerRight", "ShootSpeakerRight"); + autoDashboardChooser.addOption("ShootSpeakerFar", "ShootSpeakerFar"); + autoDashboardChooser.addOption("DriveForward", "DriveForward"); autoDashboardChooser.addOption("Do Nothing", "DoNothing"); SmartDashboard.putData(autoDashboardChooser); @@ -197,6 +261,11 @@ private void initializeAutonomous() { NamedCommands.registerCommand("BalanceRobot", m_balanceCommand); NamedCommands.registerCommand( "BrakeCommand", new InstantCommand(() -> m_driveSubsystem.SetBrakemode())); + NamedCommands.registerCommand("ShooterCommand", m_shooterCommand); + NamedCommands.registerCommand("AimSpeakerCommand", m_AimSpeakerCommand); + NamedCommands.registerCommand("ArmCommand", m_armCommand); + NamedCommands.registerCommand("AimAmpCommand", m_AimAmpCommand); + NamedCommands.registerCommand("AimLowerCommand", m_AimLowerCommand); // autoBuilder = // new RamseteAutoBuilder( @@ -225,11 +294,15 @@ private void initializeAutonomous() { private void configureTeleopPaths() { // Limits for all Paths PathConstraints constraints = - new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); + new PathConstraints(3.0, 2.0, Units.degreesToRadians(540), Units.degreesToRadians(720)); PathPlannerPath speakerPath = PathPlannerPath.fromPathFile("TeleopSpeakerPath"); + PathPlannerPath ampPath = PathPlannerPath.fromPathFile("TeleopAmpPath"); + PathPlannerPath sourcePath = PathPlannerPath.fromPathFile("TeleopSourcePath"); m_driveToSpeaker = AutoBuilder.pathfindThenFollowPath(speakerPath, constraints); + m_driveToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, constraints); + m_driveToSource = AutoBuilder.pathfindThenFollowPath(sourcePath, constraints); } public double getControllerRightY() { diff --git a/src/main/java/frc/robot/ShooterState.java b/src/main/java/frc/robot/ShooterState.java index fd4358c..3f184d7 100644 --- a/src/main/java/frc/robot/ShooterState.java +++ b/src/main/java/frc/robot/ShooterState.java @@ -1,25 +1,81 @@ package frc.robot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + /* * ShooterState.java * Tracks the status of a ring loaded in the shooter */ + public class ShooterState { + public boolean isLoaded = false; public boolean isLowered = false; + public enum ShooterMode { + DEFAULT, + SOURCE, + AMP, + SPEAKER, + TRAP, + STOP + }; + + public ShooterMode mode = ShooterMode.DEFAULT; + public boolean axisEnabled = false; + public boolean shooting = false; + public ShooterState() {} public void setLoaded() { isLoaded = true; } + public void setMode(ShooterMode newMode) { + mode = newMode; + } + + public void toggleAxis() { + axisEnabled = !axisEnabled; + } + public void setFired() { isLoaded = false; isLowered = false; + mode = ShooterMode.SOURCE; } public void setLowered() { isLowered = true; + mode = ShooterMode.DEFAULT; + } + + public double getShooterSpeed() { + switch (mode) { + case SOURCE: // TODO + return Constants.SHOOTERSOURCE; + case AMP: + return Constants.SHOOTERAMP; + case SPEAKER: + return Constants.SHOOTERSPEAKER; + case TRAP: + return Constants.SHOOTERTRAP; + default: + return Constants.SHOOTERDEFAULT; + } + } + + /** + * Updates the values on the SmartDashboard related to the shooter state. This method puts the + * values of various shooter state variables onto the SmartDashboard. The variables include + * whether the manual arm mode is enabled, the current arm mode, whether the shooter is loaded, + * whether the arm is lowered, and whether the arm is shooting. + */ + public void updateDash() { + SmartDashboard.putBoolean("Manual Arm Mode Enabled", axisEnabled); + SmartDashboard.putString("Arm Mode", mode.name()); + SmartDashboard.putBoolean("Loaded", isLoaded); + SmartDashboard.putBoolean("Lowered", isLowered); + SmartDashboard.putBoolean("Arm Shooting", shooting); } } diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 5e2e4db..82a7d81 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -41,14 +41,46 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE)) { - m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); + if (!m_shooterState.shooting) { + if (m_shooterState.axisEnabled) { + m_ArmSubsystem.disableOffset(); + if ((!HelperFunctions.inDeadzone(m_yAxis.getAsDouble(), Constants.CONTROLLERDEADZONE))) { + m_ArmSubsystem.MoveArmRelative(m_yAxis.getAsDouble() * kMaxRadiansPerInput); + } + } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { + m_ArmSubsystem.enableOffset(); + m_ArmSubsystem.lowerArm(); + m_shooterState.setLowered(); + } else if (!m_shooterState.isLoaded & !m_shooterState.isLowered) { + m_ArmSubsystem.enableOffset(); + m_ArmSubsystem.lowerArm(); + } else { + m_ArmSubsystem.enableOffset(); + followState(); + } + } + } - } else if (m_shooterState.isLoaded & !m_shooterState.isLowered) { - m_ArmSubsystem.lowerArm(); - m_shooterState.setLowered(); - } else { - m_ArmSubsystem.stop(); + private void followState() { + switch (m_shooterState.mode) { + case SOURCE: + m_ArmSubsystem.moveArmToLoad(); + break; + case AMP: + m_ArmSubsystem.moveArmToAmp(); + break; + case SPEAKER: + m_ArmSubsystem.moveArmToSpeaker(); + break; + case TRAP: + m_ArmSubsystem.moveArmToTrap(); + break; + case DEFAULT: + m_ArmSubsystem.lowerArm(); + break; + case STOP: + m_ArmSubsystem.stop(); + break; } } diff --git a/src/main/java/frc/robot/commands/LifterDownCommand.java b/src/main/java/frc/robot/commands/LeftLifterCommand.java similarity index 73% rename from src/main/java/frc/robot/commands/LifterDownCommand.java rename to src/main/java/frc/robot/commands/LeftLifterCommand.java index 0ccaf25..666fa44 100644 --- a/src/main/java/frc/robot/commands/LifterDownCommand.java +++ b/src/main/java/frc/robot/commands/LeftLifterCommand.java @@ -5,22 +5,19 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.LeftLifterSubsystem; /** Lifter Command using the Lifter Subsystem. */ -public class LifterDownCommand extends Command { +public class LeftLifterCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final LifterSubsystem m_lifterSubsystem; - - private final double speed = Constants.LIFTERSPEED; + private final LeftLifterSubsystem m_lifterSubsystem; /** * Creates a new LifterDownCommand. * * @param subsystem The subsystem used by this command. */ - public LifterDownCommand(LifterSubsystem l_subsystem) { + public LeftLifterCommand(LeftLifterSubsystem l_subsystem) { m_lifterSubsystem = l_subsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(l_subsystem); @@ -33,14 +30,13 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_lifterSubsystem.leftDown(speed); - m_lifterSubsystem.rightDown(speed); + m_lifterSubsystem.activateLeft(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_lifterSubsystem.stop(); + m_lifterSubsystem.stopLeft(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/LifterUpCommand.java b/src/main/java/frc/robot/commands/RightLifterCommand.java similarity index 67% rename from src/main/java/frc/robot/commands/LifterUpCommand.java rename to src/main/java/frc/robot/commands/RightLifterCommand.java index b7c35a0..c733159 100644 --- a/src/main/java/frc/robot/commands/LifterUpCommand.java +++ b/src/main/java/frc/robot/commands/RightLifterCommand.java @@ -5,22 +5,19 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.LifterSubsystem; +import frc.robot.subsystems.RightLifterSubsystem; /** Lifter Command using the Lifter Subsystem. */ -public class LifterUpCommand extends Command { +public class RightLifterCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final LifterSubsystem m_lifterSubsystem; - - private final double speed = Constants.LIFTERSPEED; + private final RightLifterSubsystem m_lifterSubsystem; /** - * Creates a new LifterUpCommand. + * Creates a new LifterDownCommand. * - * @param l_subsystem The subsystem used by this command. + * @param subsystem The subsystem used by this command. */ - public LifterUpCommand(LifterSubsystem l_subsystem) { + public RightLifterCommand(RightLifterSubsystem l_subsystem) { m_lifterSubsystem = l_subsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(l_subsystem); @@ -33,14 +30,13 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_lifterSubsystem.leftUp(speed); - m_lifterSubsystem.rightUp(speed); + m_lifterSubsystem.activateRight(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - m_lifterSubsystem.stop(); + m_lifterSubsystem.stopRight(); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index 2d14be6..cee33d0 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -5,6 +5,7 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; import frc.robot.subsystems.ShooterSubsystem; /** A Shooter Command that uses an example subsystem. */ @@ -12,13 +13,16 @@ public class ShooterCommand extends Command { @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) private final ShooterSubsystem m_shooterSubsystem; + private final ShooterState m_shooterState; + /** * Creates a new ShooterCommand. * * @param subsystem The subsystem used by this command. */ - public ShooterCommand(ShooterSubsystem s_subsystem) { + public ShooterCommand(ShooterSubsystem s_subsystem, ShooterState shooterState) { m_shooterSubsystem = s_subsystem; + m_shooterState = shooterState; // Use addRequirements() here to declare subsystem dependencies. addRequirements(s_subsystem); } @@ -30,14 +34,16 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // TODO: Finish Implementing ShooterCommand - System.out.println("ShooterCommand Activated"); - m_shooterSubsystem.SpinShooterFull(); + m_shooterSubsystem.SpinShooter(m_shooterState.getShooterSpeed()); + m_shooterState.shooting = true; } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + m_shooterState.shooting = false; + m_shooterSubsystem.StopShooter(); + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/auto/AimAmpCommand.java b/src/main/java/frc/robot/commands/auto/AimAmpCommand.java new file mode 100644 index 0000000..e13633b --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AimAmpCommand.java @@ -0,0 +1,58 @@ +// 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.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; +import frc.robot.subsystems.ArmSubsystem; + +/** An example command that uses an example subsystem. */ +public class AimAmpCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ArmSubsystem m_ArmSubsystem; + + private final ShooterState m_shooterState; + + /** + * Creates a new ShootAmpCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AimAmpCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { + m_ArmSubsystem = a_subsystem; + m_shooterState = shooterState; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_ArmSubsystem); + } + + // 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() { + // this bypasses the shooter state and just moves the arm to the speaker position, as the arm + // subsystem gets paused during the auto shoot command + m_shooterState.setMode(ShooterState.ShooterMode.AMP); + m_ArmSubsystem.moveArmToAmp(); + } + + // 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() { + // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at + // correct angle + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java b/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java new file mode 100644 index 0000000..98f79de --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AimSpeakerCommand.java @@ -0,0 +1,58 @@ +// 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.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; +import frc.robot.subsystems.ArmSubsystem; + +/** An example command that uses an example subsystem. */ +public class AimSpeakerCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ArmSubsystem m_ArmSubsystem; + + private final ShooterState m_shooterState; + + /** + * Creates a new ShootSpeakerCommand. + * + * @param subsystem The subsystem used by this command. + */ + public AimSpeakerCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { + m_ArmSubsystem = a_subsystem; + m_shooterState = shooterState; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_ArmSubsystem); + } + + // 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() { + // this bypasses the shooter state and just moves the arm to the speaker position, as the arm + // subsystem gets paused during the auto shoot command + m_shooterState.setMode(ShooterState.ShooterMode.SPEAKER); + m_ArmSubsystem.moveArmToSpeaker(); + } + + // 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() { + // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at + // correct angle + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.atGoal() && m_ArmSubsystem.isOffsetSet()) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/auto/LowerArmCommand.java b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java new file mode 100644 index 0000000..caaa386 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/LowerArmCommand.java @@ -0,0 +1,58 @@ +// 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.auto; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.ShooterState; +import frc.robot.subsystems.ArmSubsystem; + +/** An example command that uses an example subsystem. */ +public class LowerArmCommand extends Command { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ArmSubsystem m_ArmSubsystem; + + private final ShooterState m_shooterState; + + /** + * Creates a new ShootSpeakerCommand. + * + * @param subsystem The subsystem used by this command. + */ + public LowerArmCommand(ArmSubsystem a_subsystem, ShooterState shooterState) { + m_ArmSubsystem = a_subsystem; + m_shooterState = shooterState; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_ArmSubsystem); + } + + // 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() { + // this bypasses the shooter state and just moves the arm to the speaker position, as the arm + // subsystem gets paused during the auto shoot command + m_shooterState.setMode(ShooterState.ShooterMode.DEFAULT); + m_ArmSubsystem.lowerArm(); + } + + // 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() { + // if the arm is stopped, at goal, and the offset is set, then the command is finished, aka at + // correct angle + if (m_ArmSubsystem.ArmStopped() && m_ArmSubsystem.getFrontLimit()) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 34ef757..e09571a 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -6,45 +6,64 @@ import static edu.wpi.first.units.Units.Volts; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; import frc.robot.Constants.CANConstants; +import frc.robot.ShooterState; +import frc.robot.utils.HelperFunctions; public class ArmSubsystem extends SubsystemBase { + private final ShooterState m_shooterState; private final CANSparkMax m_armMotorMain; private final SparkPIDController m_armMainPIDController; private final RelativeEncoder m_MainEncoder; + private final AbsoluteEncoder m_AbsoluteEncoder; private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; - private final double kminArmAngle = - Units.degreesToRadians( - 0); // this is needed for the feedforward control, basically min angle relative to flat on - // floor. private static double kDt = 0.02; // 20ms (update rate for wpilib) - private final double ksArmVolts = 0.0; - private final double kgArmGravityGain = 0.0; - private final double kvArmVoltSecondsPerMeter = 0.0; - private final double kaArmVoltSecondsSquaredPerMeter = 0.0; - private final double kLoweredArmPositionRadians = Units.degreesToRadians(45); - private final double karmMaxVelocity = 2; // m/s - private final double karmMaxAcceleration = 1; // m/s^2 + private final double ksArmVolts = 0.31531; + private final double kgArmGravityGain = 0.19588; + private final double kvArmVoltSecondsPerMeter = 2.0638; + private final double kaArmVoltSecondsSquaredPerMeter = 0.37994; + private final double kMinArmAngleRadians = Units.degreesToRadians(Constants.ARMMINRELATVESTART); + private final double kMaxArmAngleRadians = Units.degreesToRadians(Constants.ARMMAXRELATIVE); + private final double kArmLoadAngleRadians = + Units.degreesToRadians(Constants.ARMLOADANGLE); // angle to be when recieving ring + private final double kArmSpeakerAngleRadians = + Units.degreesToRadians(Constants.ARMSPEAKERANGLE); // angle to be when shooting into speaker + private final double kArmAmpAngleRadians = + Units.degreesToRadians(Constants.ARMAMPANGLE); // angle to be when shooting into amp + private final double kArmTrapAngleRadians = + Units.degreesToRadians(Constants.ARMTRAPANGLE); // angle to be when shooting into trap + + private final double karmMaxVelocity = 1.0; // rad/s + private final double karmMaxAcceleration = 0.5; // rad/s^2 + private boolean kPIDEnabled = true; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html - private final double kGearRatio = 1; // TBD + private final double kGearRatio = 120; // TBD, 48:1 / kGearRatio:1 // basically converted from rotations to radians by multiplying by 2 pi, then adjusting for the // gear ratio by dividing by the gear ratio. // remember that 2pi radians in 360 degrees. private final double kRadiansConversionRatio = (Math.PI * 2) / kGearRatio; + private final double kVelocityConversionRatio = kRadiansConversionRatio / 60; + private final double kAbsoluteRadiansConversionRatio = (Math.PI * 2); + private final double kAbsoluteVelocityConversionRatio = kAbsoluteRadiansConversionRatio / 60; private final ArmFeedforward m_armFeedforward = new ArmFeedforward( ksArmVolts, kgArmGravityGain, kvArmVoltSecondsPerMeter, kaArmVoltSecondsSquaredPerMeter); @@ -52,32 +71,64 @@ public class ArmSubsystem extends SubsystemBase { new TrapezoidProfile(new TrapezoidProfile.Constraints(karmMaxVelocity, karmMaxAcceleration)); private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); - private boolean m_stopped = true; + + // track if goal was changed + private boolean m_newGoal = true; + + // enable or disable offset + private boolean m_offsetEnabled = true; + + // last goal pre offset + private double m_lastGoal = 0.0; // setup SysID for auto profiling private final SysIdRoutine m_sysIdRoutine; + // setup front limit switch for rest + private final DigitalInput m_frontLimit; + + // last measured state of limit switch + private boolean m_frontLimitState = false; + + // makes limit switch input less finnicky + private final Debouncer m_frontLimitDebouncer = new Debouncer(0.2, Debouncer.DebounceType.kBoth); + + // offset to match the absolute encoder with the main encoder by adding offset to goal + private double m_curOffset = 0.0; + /** Creates a new ArmSubsystem. */ - public ArmSubsystem() { + public ArmSubsystem(ShooterState shooterState) { + m_shooterState = shooterState; // create the arm motors m_armMotorMain = new CANSparkMax(CANConstants.MOTORARMMAINID, CANSparkMax.MotorType.kBrushless); + // front limit switch + m_frontLimit = new DigitalInput(Constants.ARMLIMITSWITCHFRONT); + // set the idle mode to brake m_armMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_armMotorMain.setInverted(true); // connect to built in PID controller m_armMainPIDController = m_armMotorMain.getPIDController(); // allow us to read the encoder m_MainEncoder = m_armMotorMain.getEncoder(); + m_AbsoluteEncoder = m_armMotorMain.getAbsoluteEncoder(); + m_AbsoluteEncoder.setInverted(false); + m_AbsoluteEncoder.setPositionConversionFactor(kAbsoluteRadiansConversionRatio); + m_AbsoluteEncoder.setVelocityConversionFactor(kAbsoluteVelocityConversionRatio); + // m_AbsoluteEncoder.setZeroOffset(Constants.ARMENCODEROFFSET); // setup the encoders m_MainEncoder.setPositionConversionFactor(kRadiansConversionRatio); + m_MainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); + matchEncoders(); // PID coefficients - kP = 0.1; - kI = 1e-4; - kD = 1; + kP = 2.3142; + kI = 0; + kD = 0.23128; kIz = 0; - kMaxOutput = 1; - kMinOutput = -1; + kMaxOutput = 0.4; + kMinOutput = -0.4; // set PID coefficients m_armMainPIDController.setP(kP); @@ -85,6 +136,10 @@ public ArmSubsystem() { m_armMainPIDController.setD(kD); m_armMainPIDController.setIZone(kIz); m_armMainPIDController.setOutputRange(kMinOutput, kMaxOutput); + m_armMainPIDController.setFeedbackDevice(m_MainEncoder); + // m_armMotorMain.setSoftLimit(SoftLimitDirection.kReverse, kMinArmAngleRadians)); + // m_armMotorMain.setSoftLimit(SoftLimitDirection.kForward, kMaxArmAngleRadians); + m_armMotorMain.burnFlash(); // setup SysID for auto profiling m_sysIdRoutine = @@ -124,48 +179,160 @@ public void MoveArmRelative(double radians) { public void lowerArm() { // move to set lowered arm position - MoveArmToPosition(kLoweredArmPositionRadians); + MoveArmToPosition(kMinArmAngleRadians); + } + + public void rasieArm() { + // move to set raised arm position + MoveArmToPosition(kMaxArmAngleRadians); + } + + public void moveArmToLoad() { + MoveArmToPosition(kArmLoadAngleRadians); + } + + public void moveArmToAmp() { + MoveArmToPosition(kArmAmpAngleRadians); + } + + public void moveArmToSpeaker() { + MoveArmToPosition(kArmSpeakerAngleRadians); + } + + public void moveArmToTrap() { + MoveArmToPosition(kArmTrapAngleRadians); } /* * Move arm to global position (by updating goal) */ public void MoveArmToPosition(double radians) { - m_stopped = false; // update the motion profile with new goal // add minimum starting angle to the target angle to get the real angle - double total_radians = radians + kminArmAngle; - m_goal = new TrapezoidProfile.State(total_radians, 0); // set the goal + double final_radians = Math.max(radians, kMinArmAngleRadians); + final_radians = Math.min(final_radians, kMaxArmAngleRadians); + if (final_radians != m_lastGoal) { + m_newGoal = true; + m_lastGoal = final_radians; + resetOffset(); + } + m_goal = new TrapezoidProfile.State(final_radians + m_curOffset, 0); // set the goal } /* * attempt to hold arm at current location */ public void stop() { - if (!m_stopped) { + if (!atGoal()) { // update the PID controller with current encoder position MoveArmToPosition(getAverageEncoderPosition()); - m_stopped = true; } } - public void zeroEncoders() { - m_MainEncoder.setPosition(0); + public void disablePID() { + kPIDEnabled = false; + } + + /** Matches the position of the main encoder with the absolute encoder. */ + public void matchEncoders() { + m_MainEncoder.setPosition(m_AbsoluteEncoder.getPosition()); + } + + private void SetOffsetWithEncoder() { + m_curOffset = getError(); + } + + public void resetOffset() { + m_curOffset = 0.0; + } + + /** + * Calculates the error between the position reported by the absolute encoder and the main + * encoder. + * + * @return The error between the two encoder positions. + */ + public double getError() { + return m_MainEncoder.getPosition() - m_AbsoluteEncoder.getPosition(); + } + + /** + * Checks if the arm is at the goal position. + * + * @return true if the arm is at the goal position, false otherwise. + */ + public boolean atGoal() { + return HelperFunctions.inDeadzone( + m_goal.position - m_MainEncoder.getPosition(), Units.degreesToRadians(1)); + } + + /** + * Checks if the arm is stopped. + * + * @return true if the arm is stopped, false otherwise. + */ + public boolean ArmStopped() { + return HelperFunctions.inDeadzone(m_goal.velocity, 0.0001); + } + + public boolean isOffsetSet() { + return m_curOffset != 0.0; + } + + public boolean getFrontLimit() { + return !m_frontLimitDebouncer.calculate(m_frontLimit.get()); + } + + public void enableOffset() { + m_offsetEnabled = true; + } + + public void disableOffset() { + m_offsetEnabled = true; } @Override public void periodic() { // This method will be called once per scheduler run - - // update the setpoint - m_setpoint = m_armTrapezoidProfile.calculate(kDt, m_setpoint, m_goal); - - // Call the controller and feedforward with the target position and velocity - m_armMainPIDController.setReference( - m_setpoint.position, - CANSparkBase.ControlType.kPosition, - 0, - m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); + if (kPIDEnabled) { + // update the setpoint + m_setpoint = m_armTrapezoidProfile.calculate(kDt, m_setpoint, m_goal); + // Call the controller and feedforward with the target position and velocity + m_armMainPIDController.setReference( + m_setpoint.position, + CANSparkBase.ControlType.kPosition, + 0, + m_armFeedforward.calculate(m_setpoint.position, m_setpoint.velocity)); + if (atGoal() && ArmStopped() && m_newGoal && m_offsetEnabled) { + double cur_error = getError(); + if (!HelperFunctions.inDeadzone(cur_error, Units.degreesToRadians(3))) { + SetOffsetWithEncoder(); + m_newGoal = + false; // reset the new goal flag, so that we dont try resyncing encoders again + m_goal = new TrapezoidProfile.State(m_lastGoal + m_curOffset, 0); // set the goal + } + } + } + if (m_frontLimitState != getFrontLimit()) { + m_frontLimitState = getFrontLimit(); + if (m_frontLimitState) { + matchEncoders(); + } + } + m_shooterState.updateDash(); + SmartDashboard.putNumber( + "Current Arm Angle (Degrees) (Relative)", + Units.radiansToDegrees(m_MainEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putNumber( + "Current Arm Angle (Degrees) (Absolute)", + Units.radiansToDegrees(m_AbsoluteEncoder.getPosition()) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putBoolean("Front Limit Switch Pressed", !m_frontLimit.get()); + SmartDashboard.putNumber("Current angle Offset", Units.radiansToDegrees(m_curOffset)); + SmartDashboard.putNumber( + "Requested Angle", Units.radiansToDegrees(m_lastGoal) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putNumber( + "Current Goal Angle", Units.radiansToDegrees(m_goal.position) + Constants.ARMSTARTINGANGLE); + SmartDashboard.putBoolean("At Goal", atGoal()); } @Override diff --git a/src/main/java/frc/robot/subsystems/CameraSubsystem.java b/src/main/java/frc/robot/subsystems/CameraSubsystem.java index f518a12..5c143b6 100644 --- a/src/main/java/frc/robot/subsystems/CameraSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CameraSubsystem.java @@ -22,19 +22,19 @@ public class CameraSubsystem extends SubsystemBase { private final DriveSubsystem m_driveSubsystem; public final AprilTagFieldLayout aprilTagFieldLayout; - private final String frontCameraName = "OV5647"; + private final String frontCameraName = "cam"; private final PhotonCamera frontCamera; public PhotonPipelineResult frontCameraResult; // Physical location of camera relative to center - private final double CameraLocationXMeters = 0.5; - private final double CameraLocationYMeters = 0.0; - private final double CameraLocationZMeters = 0.5; + private final double CameraLocationXMeters = Units.inchesToMeters(6); + private final double CameraLocationYMeters = Units.inchesToMeters(9.3); + private final double CameraLocationZMeters = Units.inchesToMeters(10.5); // angle of camera / orientation // Cam mounted facing forward, half a meter forward of center, half a meter up from center. - private final double CameraRollRadians = Units.degreesToRadians(0.5); + private final double CameraRollRadians = Units.degreesToRadians(90); private final double CameraPitchRadians = Units.degreesToRadians(0.0); - private final double CameraYawRadians = Units.degreesToRadians(0.5); + private final double CameraYawRadians = Units.degreesToRadians(0); private final Transform3d frontCameraLocation = new Transform3d( diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 69506c9..323b70d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; import frc.robot.Constants.CANConstants; import frc.robot.DriveConstants; @@ -76,9 +77,9 @@ public class DriveSubsystem extends SubsystemBase { static final double turn_P = 0.1; static final double turn_I = 0.00; static final double turn_D = 0.00; - static final double MaxTurnRateDegPerS = 20; - static final double MaxTurnAccelerationDegPerSSquared = 50; - static final double TurnToleranceDeg = 3; // max diff in degrees + static final double MaxTurnRateDegPerS = 5; + static final double MaxTurnAccelerationDegPerSSquared = 5; + static final double TurnToleranceDeg = 10; // max diff in degrees static final double TurnRateToleranceDegPerS = 10; // degrees per second // false when inactive, true when active / a target is set. private boolean turnControllerEnabled = false; @@ -370,7 +371,7 @@ public void driveStraight( } double leftStickValue = joystickMagnitude + angleRate; double rightStickValue = joystickMagnitude - angleRate; - this.tankDrive(leftStickValue, rightStickValue); + this.tankDrive((leftStickValue * Constants.MAX_SPEED), (rightStickValue * Constants.MAX_SPEED)); } /* diff --git a/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java new file mode 100644 index 0000000..63f4eac --- /dev/null +++ b/src/main/java/frc/robot/subsystems/DriverCameraSubsystem.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems; + +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class DriverCameraSubsystem extends SubsystemBase { + private final UsbCamera m_DriverCamera1; + private final int k_FrontCameraID = 0; + private NetworkTableEntry cameraSelection; + + // Initalizes DriverCameraSubsystem + public DriverCameraSubsystem() { + // connect to network tables + cameraSelection = NetworkTableInstance.getDefault().getTable("").getEntry("CameraSelection"); + + // Connect to Cameras + m_DriverCamera1 = CameraServer.startAutomaticCapture(k_FrontCameraID); + + // set the camera selection to the front camera + cameraSelection.setString(m_DriverCamera1.getName()); + // start telemetry + // m_DriverCamera1.set + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Driver Camera FPS", m_DriverCamera1.getActualFPS()); + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java b/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java new file mode 100644 index 0000000..30a5594 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LeftLifterSubsystem.java @@ -0,0 +1,50 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.CANConstants; + +public class LeftLifterSubsystem extends SubsystemBase { + private final CANSparkMax m_left; // Motor for Left + private double kCurrentSpeed = Constants.LIFTERSPEED; + + /** Creates a new LifterSubsystem. */ + public LeftLifterSubsystem() { + m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless); + m_left.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_left.setInverted(false); + m_left.setSmartCurrentLimit(60); + m_left.burnFlash(); + } + + public void leftMove(double speed) { + m_left.set(speed); + } + + public void activateLeft() { + leftMove(kCurrentSpeed); + } + + public void changeDirection() { + kCurrentSpeed = -kCurrentSpeed; + } + + public void stopLeft() { + m_left.set(0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} diff --git a/src/main/java/frc/robot/subsystems/LifterSubsystem.java b/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java similarity index 62% rename from src/main/java/frc/robot/subsystems/LifterSubsystem.java rename to src/main/java/frc/robot/subsystems/RightLifterSubsystem.java index 938d386..d677abc 100644 --- a/src/main/java/frc/robot/subsystems/LifterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/RightLifterSubsystem.java @@ -6,36 +6,34 @@ import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import frc.robot.Constants.CANConstants; -public class LifterSubsystem extends SubsystemBase { - private final CANSparkMax m_left; // Motor for Left +public class RightLifterSubsystem extends SubsystemBase { private final CANSparkMax m_right; // Motor for right + private double kCurrentSpeed = Constants.LIFTERSPEED; /** Creates a new LifterSubsystem. */ - public LifterSubsystem() { - m_left = new CANSparkMax(CANConstants.MOTORLIFTERLEFTID, CANSparkMax.MotorType.kBrushless); + public RightLifterSubsystem() { m_right = new CANSparkMax(CANConstants.MOTORLIFTERRIGHTID, CANSparkMax.MotorType.kBrushless); + m_right.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_right.setSmartCurrentLimit(60); + m_right.burnFlash(); } - public void leftUp(double speed) { - m_left.set(speed); - } - - public void leftDown(double speed) { - m_left.set(-speed); + public void rightMove(double speed) { + m_right.set(speed); } - public void rightUp(double speed) { - m_right.set(speed); + public void activateRight() { + rightMove(kCurrentSpeed); } - public void rightDown(double speed) { - m_right.set(-speed); + public void changeDirection() { + kCurrentSpeed = -kCurrentSpeed; } - public void stop() { - m_left.set(0); + public void stopRight() { m_right.set(0); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5cdff47..bd6d5ad 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -21,22 +22,24 @@ public class ShooterSubsystem extends SubsystemBase { private final CANSparkMax m_ShooterMotorMain; + private final CANSparkMax m_ShooterMotorSecondary; private final SparkPIDController m_ShooterMainPIDController; private RelativeEncoder m_ShooterMainEncoder; - private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput, kMaxSpeed; + private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; // general drive constants // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 // https://sciencing.com/convert-rpm-linear-speed-8232280.html - private final double kWheelDiameter = Units.inchesToMeters(6); // meters - private final double kGearRatio = 1; // TBD + private final double kWheelDiameter = Units.inchesToMeters(4); // meters + private final double kGearRatio = 4; // TBD // basically converted from rotations to to radians to then meters using the wheel diameter. // the diameter is already *2 so we don't need to multiply by 2 again. - private final double kVelocityConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio / 60; + private final double kPositionConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio; + private final double kVelocityConversionRatio = kPositionConversionRatio / 60; // setup feedforward - private final double ksShooterVolts = 0.0; - private final double kvDriveVoltSecondsPerMeter = 0.0; - private final double kaDriveVoltSecondsSquaredPerMeter = 0.0; + private final double ksShooterVolts = 0.2063; + private final double kvDriveVoltSecondsPerMeter = 1.5611; + private final double kaDriveVoltSecondsSquaredPerMeter = 0.1396; SimpleMotorFeedforward m_shooterFeedForward = new SimpleMotorFeedforward( @@ -45,29 +48,37 @@ public class ShooterSubsystem extends SubsystemBase { // setup SysID for auto profiling private final SysIdRoutine m_sysIdRoutine; + // current limit + private final int k_CurrentLimit = 80; + /** Creates a new ShooterSubsystem. */ public ShooterSubsystem() { // create the shooter motors m_ShooterMotorMain = - new CANSparkMax(CANConstants.MOTORSHOOTERID, CANSparkMax.MotorType.kBrushless); + new CANSparkMax(CANConstants.MOTORSHOOTERLEFTID, CANSparkMax.MotorType.kBrushless); + m_ShooterMotorSecondary = + new CANSparkMax(CANConstants.MOTORSHOOTERRIGHTID, CANSparkMax.MotorType.kBrushless); // set the idle mode to coast - m_ShooterMotorMain.setIdleMode(CANSparkMax.IdleMode.kCoast); + m_ShooterMotorMain.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_ShooterMotorSecondary.setIdleMode(CANSparkMax.IdleMode.kBrake); + m_ShooterMotorSecondary.follow(m_ShooterMotorMain, true); + m_ShooterMotorMain.setSmartCurrentLimit(k_CurrentLimit); + m_ShooterMotorSecondary.setSmartCurrentLimit(k_CurrentLimit); // connect to built in PID controller m_ShooterMainPIDController = m_ShooterMotorMain.getPIDController(); // allow us to read the encoder m_ShooterMainEncoder = m_ShooterMotorMain.getEncoder(); + m_ShooterMainEncoder.setPositionConversionFactor(kPositionConversionRatio); m_ShooterMainEncoder.setVelocityConversionFactor(kVelocityConversionRatio); // PID coefficients - kP = 6e-5; + kP = 0.00013373; kI = 0; kD = 0; kIz = 0; kMaxOutput = 1; kMinOutput = -1; - kMaxSpeed = 5; - // set PID coefficients m_ShooterMainPIDController.setP(kP); m_ShooterMainPIDController.setI(kI); @@ -82,6 +93,8 @@ public ShooterSubsystem() { (voltage) -> this.setVoltage(voltage), null, // No log consumer, since data is recorded by URCL this)); + m_ShooterMotorMain.burnFlash(); + m_ShooterMotorSecondary.burnFlash(); } public void setVoltage(Measure voltage) { @@ -104,6 +117,10 @@ public void SpinShooter(double speed) { speed, CANSparkBase.ControlType.kVelocity, 0, m_shooterFeedForward.calculate(speed)); } + public void SpinAtFull() { + m_ShooterMotorMain.set(1); + } + /* * Stop the shooter */ @@ -111,13 +128,6 @@ public void StopShooter() { SpinShooter(0); } - /* - * Spin Shooter at max Speed - */ - public void SpinShooterFull() { - SpinShooter(kMaxSpeed); - } - /* * Check if shooter is at a given Speed */ @@ -126,16 +136,10 @@ public Boolean isAtSpeedTolerance(double speed) { && m_ShooterMainEncoder.getVelocity() < speed + 0.1); } - /* - * Check if shooter is at max Speed - */ - public Boolean isAtMaxSpeed() { - return isAtSpeedTolerance(kMaxSpeed); - } - @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Shooter Motor Speed m/s", m_ShooterMainEncoder.getVelocity()); } @Override diff --git a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java index e27792c..ef7566f 100644 --- a/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java +++ b/src/main/java/frc/robot/subsystems/UltrasonicSubsystem.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; /** This Subsystem is what translates the output of the ultrasonic sensor to standard units. */ @@ -34,6 +35,7 @@ public void periodic() { // SmartDashboard.putNumber("Ultrasonic Sensor Distance", getRangeInches(ultrasonic1)); // Calculate what percentage of 5 Volts we are actually at voltageScaleFactor = 5 / RobotController.getVoltage5V(); + SmartDashboard.putNumber("Ring Distance", DistanceIn()); } @Override diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index cae1363..787450f 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.6", + "version": "2024.2.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2024.2.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2024.2.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e..a829581 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.1", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/URCL.json b/vendordeps/URCL.json index d2c0e48..998c261 100644 --- a/vendordeps/URCL.json +++ b/vendordeps/URCL.json @@ -1,25 +1,25 @@ { "fileName": "URCL.json", "name": "URCL", - "version": "2024.0.1", + "version": "2024.1.0", "frcYear": "2024", "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c", "mavenUrls": [ - "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.0.1" + "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0" ], "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json", "javaDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-java", - "version": "2024.0.1" + "version": "2024.1.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2024.0.1", + "version": "2024.1.0", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-cpp", - "version": "2024.0.1", + "version": "2024.1.0", "libName": "URCL", "headerClassifier": "headers", "sharedLibrary": false, @@ -49,7 +49,7 @@ { "groupId": "org.littletonrobotics.urcl", "artifactId": "URCL-driver", - "version": "2024.0.1", + "version": "2024.1.0", "libName": "URCLDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index d4b0b67..26a28d5 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.3", + "version": "v2024.2.7", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.3", + "version": "v2024.2.7", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.3", + "version": "v2024.2.7", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.3" + "version": "v2024.2.7" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.3" + "version": "v2024.2.7" } ] } \ No newline at end of file