diff --git a/.vscode/settings.json b/.vscode/settings.json index 565b0808..25918d97 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,6 @@ }, ], "java.test.defaultConfig": "WPIlibUnitTests", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable", + "java.debug.settings.onBuildFailureProceed": true } diff --git a/elastic-layout.json b/elastic-layout.json new file mode 100644 index 00000000..182f36ed --- /dev/null +++ b/elastic-layout.json @@ -0,0 +1,932 @@ +{ + "version": 1.0, + "grid_size": 70, + "tabs": [ + { + "name": "Autonomous", + "grid_layout": { + "layouts": [ + { + "title": "Enabled Subsystems", + "x": 1120.0, + "y": 0.0, + "width": 280.0, + "height": 420.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Intake", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Intake Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Climber", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Climber-Hopper Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Handoff", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Handoff Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Hood", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Hood Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Spindexer", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Spindexer Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Swerve", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Shooter", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Shooter Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Turret", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Back Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Left Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Right Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Right Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + ], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 840.0, + "height": 420.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt (No Fuel)", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Match Time", + "x": 0.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Match Time", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Handoff At Tol?", + "x": 980.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Handoff/At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Handoff State", + "x": 840.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Handoff/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Pivot At Tol?", + "x": 980.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Pivot State", + "x": 840.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood State", + "x": 840.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Shooter State", + "x": 840.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Turret State", + "x": 840.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood At Tol?", + "x": 980.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shooter At Tol?", + "x": 980.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Turret At Tol?", + "x": 980.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shift", + "x": 0.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Field State", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Is Active Shift?", + "x": 280.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Time Left In Shift", + "x": 280.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, + { + "title": "Superstructure At Tol?", + "x": 840.0, + "y": 350.0, + "width": 280.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Everything At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Autonomous", + "x": 560.0, + "y": 420.0, + "width": 210.0, + "height": 140.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Autonomous", + "period": 0.06, + "sort_options": false + } + } + ] + } + }, + { + "name": "Teleoperated", + "grid_layout": { + "layouts": [ + { + "title": "Enabled Subsystems", + "x": 1120.0, + "y": 0.0, + "width": 280.0, + "height": 420.0, + "type": "List Layout", + "properties": { + "label_position": "TOP" + }, + "children": [ + { + "title": "Intake", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Intake Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Climber", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Climber-Hopper Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Handoff", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Handoff Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Hood", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Hood Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Spindexer", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Spindexer Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Swerve", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Swerve Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Shooter", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Shooter Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Turret", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Turret Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Back Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Back Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Left Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Left Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Right Limelight", + "x": 0.0, + "y": 0.0, + "width": 166.0, + "height": 166.0, + "type": "Toggle Switch", + "properties": { + "topic": "SmartDashboard/Enabled Subsystems/Right Limelight Is Enabled", + "period": 0.06, + "data_type": "boolean" + } + } + ] + } + ], + "containers": [ + { + "title": "Field", + "x": 0.0, + "y": 0.0, + "width": 840.0, + "height": 420.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Rebuilt (No Fuel)", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": 0.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Match Time", + "x": 0.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/Match Time", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "Handoff At Tol?", + "x": 980.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Handoff/At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Handoff State", + "x": 840.0, + "y": 0.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Handoff/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Pivot At Tol?", + "x": 980.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Pivot State", + "x": 840.0, + "y": 70.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake/Pivot State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood State", + "x": 840.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Shooter State", + "x": 840.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Turret State", + "x": 840.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Hood At Tol?", + "x": 980.0, + "y": 140.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Hood At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shooter At Tol?", + "x": 980.0, + "y": 210.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Shooter At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Turret At Tol?", + "x": 980.0, + "y": 280.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Superstructure/Turret At Tolerance?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shift", + "x": 0.0, + "y": 560.0, + "width": 280.0, + "height": 140.0, + "type": "Large Text Display", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Field State", + "period": 0.06, + "data_type": "string" + } + }, + { + "title": "Is Active Shift?", + "x": 280.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Is Active Shift?", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Time Left In Shift", + "x": 280.0, + "y": 420.0, + "width": 280.0, + "height": 140.0, + "type": "Match Time", + "properties": { + "topic": "/SmartDashboard/FMSUtil/Time Left In Shift", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 20, + "yellow_start_time": 30 + } + }, + { + "title": "Won Auto?", + "x": 420.0, + "y": 560.0, + "width": 140.0, + "height": 140.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/FMSUtil/No Auto Winner Data", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Handoff Reverse", + "x": 1120.0, + "y": 420.0, + "width": 280.0, + "height": 280.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Handoff Reverse", + "period": 0.06, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Intake Reverse", + "x": 560.0, + "y": 420.0, + "width": 280.0, + "height": 280.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Intake Reverse", + "period": 0.06, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Spindexer Reverse", + "x": 840.0, + "y": 420.0, + "width": 280.0, + "height": 280.0, + "type": "Command", + "properties": { + "topic": "/SmartDashboard/Robot/Spindexer Reverse", + "period": 0.06, + "show_type": false, + "maximize_button_space": true + } + }, + { + "title": "Spin At Tol?", + "x": 980.0, + "y": 350.0, + "width": 140.0, + "height": 70.0, + "type": "Boolean Box", + "properties": { + "topic": "/SmartDashboard/Spindexer/At Tolerance", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Spin State", + "x": 840.0, + "y": 350.0, + "width": 140.0, + "height": 70.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Spindexer/State", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + } + } + ] +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3cb..ff81e307 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ @@ -18,12 +23,20 @@ } ], "axisCount": 3, - "buttonCount": 4, + "buttonCount": 12, "buttonKeys": [ 90, 88, 67, - 86 + 86, + 66, + 78, + 77, + 71, + 72, + 74, + 75, + 76 ], "povConfig": [ { @@ -41,19 +54,15 @@ }, { "axisConfig": [ + {}, { - "decKey": 74, - "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 + "decKey": 73 } ], "axisCount": 2, "buttonCount": 4, "buttonKeys": [ - 77, + -1, 44, 46, 47 diff --git a/simgui.json b/simgui.json index d01bfcf5..84499020 100644 --- a/simgui.json +++ b/simgui.json @@ -1,19 +1,35 @@ { - "HALProvider": { - "Other Devices": { - "window": { - "visible": false - } - } - }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", "/SmartDashboard/Autonomous": "String Chooser", "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/Visualizers/ClimberHopper": "Mechanism2d" + "/SmartDashboard/Handoff Reverse": "Command", + "/SmartDashboard/Intake Reverse": "Command", + "/SmartDashboard/PathPlanner": "Alerts", + "/SmartDashboard/Robot/Handoff Reverse": "Command", + "/SmartDashboard/Robot/Intake Reverse": "Command", + "/SmartDashboard/Robot/Ryan Testing Seed Hood Encoder (NEW)": "Command", + "/SmartDashboard/Robot/Seed Hood Relative Encoder At Upper Hardstop": "Command", + "/SmartDashboard/Robot/Seed Turret": "Command", + "/SmartDashboard/Robot/Set Megatag 1": "Command", + "/SmartDashboard/Robot/Set Megatag 2": "Command", + "/SmartDashboard/Robot/Spindexer Reverse": "Command", + "/SmartDashboard/Robot/Zero Hood Encoder": "Command", + "/SmartDashboard/Robot/Zero Pivot Encoder at Lower Limit (Deployed)": "Command", + "/SmartDashboard/Robot/Zero Pivot Encoder at Upper Limit (Stowed)": "Command", + "/SmartDashboard/Robot/Zero Turret Encoders": "Command", + "/SmartDashboard/Scheduler": "Scheduler", + "/SmartDashboard/Spindexer Reverse": "Command", + "/SmartDashboard/Visualizers/Hood": "Mechanism2d", + "/SmartDashboard/Visualizers/Turret": "Mechanism2d" }, "windows": { + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + }, "/SmartDashboard/Field": { "bottom": 1914, "height": 8.069275856018066, @@ -25,7 +41,7 @@ "visible": true } }, - "/SmartDashboard/Visualizers/ClimberHopper": { + "/SmartDashboard/Scheduler": { "window": { "visible": true } @@ -33,15 +49,15 @@ } }, "NetworkTables": { - "Persistent Values": { - "open": false - }, - "Retained Values": { - "open": false - }, "transitory": { "SmartDashboard": { - "ClimberHopper": { + "FieldPositions": { + "open": true + }, + "Handoff": { + "open": true + }, + "States": { "open": true }, "open": true diff --git a/src/main/deploy/pathplanner/autos/Box Test.auto b/src/main/deploy/pathplanner/autos/Box Test.auto new file mode 100644 index 00000000..34e6aefd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Box Test.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Box 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Box 2" + } + }, + { + "type": "path", + "data": { + "pathName": "Box 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Box 4" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot Auto.auto b/src/main/deploy/pathplanner/autos/Depot Auto.auto new file mode 100644 index 00000000..03a0af76 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Depot Auto.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Bump To Depot" + } + }, + { + "type": "path", + "data": { + "pathName": "Depot To Tower Left" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto b/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto new file mode 100644 index 00000000..689d4d90 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 1 Cycle.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle Climb.auto b/src/main/deploy/pathplanner/autos/Left 2 Cycle Climb.auto new file mode 100644 index 00000000..e437961e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Cycle Climb.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score Score Tower" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto new file mode 100644 index 00000000..b666c1c2 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Left 2 Cycle.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Left Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Left NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Left Score To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto b/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto new file mode 100644 index 00000000..0592ec02 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 1 Cycle (P).auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle Climb.auto b/src/main/deploy/pathplanner/autos/Right 2 Cycle Climb.auto new file mode 100644 index 00000000..aef61bda --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Cycle Climb.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score Score Tower" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto b/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto new file mode 100644 index 00000000..1a967da4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Right 2 Cycle.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right Trench To NZ" + } + }, + { + "type": "path", + "data": { + "pathName": "Right NZ To Score" + } + }, + { + "type": "path", + "data": { + "pathName": "Right Score To Score" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Straight Line Test.auto b/src/main/deploy/pathplanner/autos/Straight Line Test.auto new file mode 100644 index 00000000..92bbd21a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Straight Line Test.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight Line" + } + } + ] + } + }, + "resetOdom": true, + "folder": "Tests", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 00000000..ac5f5216 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 1.path b/src/main/deploy/pathplanner/paths/Box 1.path new file mode 100644 index 00000000..d8930a67 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0000000000000018, + "y": 2.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 2.0 + }, + "prevControl": { + "x": 2.0, + "y": 2.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 2.path b/src/main/deploy/pathplanner/paths/Box 2.path new file mode 100644 index 00000000..8ac93dd7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.0, + "y": 2.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.003409325748791, + "y": 1.720136381756153 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0, + "y": 1.0 + }, + "prevControl": { + "x": 3.0045204355743644, + "y": 1.6922222258449668 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 3.path b/src/main/deploy/pathplanner/paths/Box 3.path new file mode 100644 index 00000000..3bfc33d1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.0, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 1.77619604200323, + "y": 0.9977414835927593 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 1.0 + }, + "prevControl": { + "x": 3.0717077172960874, + "y": 0.9943971527224802 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Box 4.path b/src/main/deploy/pathplanner/paths/Box 4.path new file mode 100644 index 00000000..e38a13d5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Box 4.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 1.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.0299091229524393, + "y": 1.2482044406617574 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 2.0 + }, + "prevControl": { + "x": 2.0255951320775263, + "y": 1.751313673045875 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot To Tower Left.path b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path new file mode 100644 index 00000000..a86d903e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot To Tower Left.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7278054567022547, + "y": 5.9339976275207595 + }, + "prevControl": null, + "nextControl": { + "x": 3.869489916963227, + "y": 6.149181494661922 + }, + "isLocked": false, + "linkedName": "Depot Collect" + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 3.643546856465006, + "y": 4.83655990510083 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.06336939721792927, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Misc", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Bump To Depot.path b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path new file mode 100644 index 00000000..7d2e13cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Bump To Depot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.589750889679717, + "y": 5.9339976275207595 + }, + "prevControl": null, + "nextControl": { + "x": 3.8521567589767303, + "y": 5.92251805970181 + }, + "isLocked": false, + "linkedName": "Left Bump Start" + }, + { + "anchor": { + "x": 0.7278054567022547, + "y": 5.9339976275207595 + }, + "prevControl": { + "x": 1.6779675215686134, + "y": 5.934033465612401 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot Collect" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Misc", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Score.path b/src/main/deploy/pathplanner/paths/Left NZ To Score.path new file mode 100644 index 00000000..b7b1758a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Score.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.231184022824536, + "y": 4.6366476462196875 + }, + "prevControl": null, + "nextControl": { + "x": 7.6360057061340925, + "y": 7.703109843081313 + }, + "isLocked": false, + "linkedName": "NZ Left Center" + }, + { + "anchor": { + "x": 3.600510083036774, + "y": 7.580154211150653 + }, + "prevControl": { + "x": 7.7653922967189715, + "y": 7.6125392296718974 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7829408020369174, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path new file mode 100644 index 00000000..0b6065d3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left NZ To Tower Left.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.231184022824536, + "y": 4.6366476462196875 + }, + "prevControl": null, + "nextControl": { + "x": 6.1331413181982, + "y": 7.713776946338313 + }, + "isLocked": false, + "linkedName": "NZ Left Center" + }, + { + "anchor": { + "x": 4.633392645314355, + "y": 7.655468564650059 + }, + "prevControl": { + "x": 5.92449584816133, + "y": 7.655468564650059 + }, + "nextControl": { + "x": 2.6733286409106265, + "y": 7.655468564650059 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 2.2018149466192183, + "y": 7.268137603795967 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.2581143740340126, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score Score Tower.path b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path new file mode 100644 index 00000000..3765688a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score Score Tower.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.600510083036774, + "y": 7.580154211150653 + }, + "prevControl": null, + "nextControl": { + "x": 5.956773428237163, + "y": 7.580154211155934 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.924495848161329, + "y": 6.3858837485172 + }, + "prevControl": { + "x": 5.981334656466404, + "y": 7.488556629635666 + }, + "nextControl": { + "x": 5.870699881376039, + "y": 5.3422419928825615 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.483973902728352, + "y": 4.4707473309608545 + }, + "prevControl": { + "x": 5.84048778435412, + "y": 4.1030409776021175 + }, + "nextControl": { + "x": 7.312431791226488, + "y": 4.944151838676692 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.73143534994069, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 7.011174377219542, + "y": 7.41876631079478 + }, + "nextControl": { + "x": 4.795823194932401, + "y": 7.333560496090013 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.558078291814947, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.354258600237249, + "y": 7.472562277580071 + }, + "nextControl": { + "x": 3.6566879283896982, + "y": 7.3349214122951345 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 1.7714472123368927, + "y": 7.031435349940688 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.4623824451410663, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.2238010657193716, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 4.440497335701599, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Score To Score.path b/src/main/deploy/pathplanner/paths/Left Score To Score.path new file mode 100644 index 00000000..273e9436 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Score To Score.path @@ -0,0 +1,125 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.600510083036774, + "y": 7.580154211150653 + }, + "prevControl": null, + "nextControl": { + "x": 6.645361803084224, + "y": 7.558635824436536 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 5.924495848161329, + "y": 6.3858837485172 + }, + "prevControl": { + "x": 5.9890510083036785, + "y": 7.601672597864768 + }, + "nextControl": { + "x": 5.869085726034151, + "y": 5.342326448455356 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.483973902728352, + "y": 4.4707473309608545 + }, + "prevControl": { + "x": 5.956773428232504, + "y": 4.2555634638196915 + }, + "nextControl": { + "x": 7.571772343343488, + "y": 4.914746694477237 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.83902728351593, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 7.216188798669032, + "y": 7.40370263955874 + }, + "nextControl": { + "x": 4.789500713266761, + "y": 7.431398002853067 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.7544079885877313, + "y": 7.408007117437722 + }, + "prevControl": { + "x": 5.41055634807418, + "y": 7.366704707560627 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left Center NZ (2)" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.4623824451410663, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.2238010657193716, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.2132399745385114, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path new file mode 100644 index 00000000..88729809 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Trench Score To Tower Left.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.600510083036774, + "y": 7.580154211150653 + }, + "prevControl": null, + "nextControl": { + "x": 1.2227283511269276, + "y": 7.6447093712930005 + }, + "isLocked": false, + "linkedName": "Left Trench Score" + }, + { + "anchor": { + "x": 1.2227283511269282, + "y": 4.922633451957295 + }, + "prevControl": { + "x": 1.5777817319098464, + "y": 6.04158956109134 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Left" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2742946708463985, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Left Trench To NZ.path b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path new file mode 100644 index 00000000..5bf2b1b4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Left Trench To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.998600237247925, + "y": 7.526358244365362 + }, + "prevControl": null, + "nextControl": { + "x": 9.162767475035661, + "y": 7.651355206847361 + }, + "isLocked": false, + "linkedName": "Left Trench Start" + }, + { + "anchor": { + "x": 8.231184022824536, + "y": 4.6366476462196875 + }, + "prevControl": { + "x": 8.20530670470756, + "y": 6.706833095577747 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "NZ Left Center" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.596175478065245, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Score.path b/src/main/deploy/pathplanner/paths/Right NZ To Score.path new file mode 100644 index 00000000..4daa9b7c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Score.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.282938659058486, + "y": 3.4333523537803137 + }, + "prevControl": null, + "nextControl": { + "x": 7.90771754636234, + "y": 0.3280741797432247 + }, + "isLocked": false, + "linkedName": "Right Center NZ" + }, + { + "anchor": { + "x": 3.042781740370898, + "y": 0.5221540656205423 + }, + "prevControl": { + "x": 7.46780313837375, + "y": 0.5221540656205423 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Trench Score" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.782940802036919, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path new file mode 100644 index 00000000..3611ebc6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right NZ To Tower Right.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.28075919335706, + "y": 3.6638078291814944 + }, + "prevControl": null, + "nextControl": { + "x": 6.053606168446025, + "y": 0.48984578884934704 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.3644128113879015, + "y": 0.4252906287069993 + }, + "prevControl": { + "x": 6.397900355871888, + "y": 0.4468090154211153 + }, + "nextControl": { + "x": 3.1632509361049994, + "y": 0.4125799210320481 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9645077105575333, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 2.6644602609727146, + "y": 0.6835112692763936 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.5156739811912283, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score Score Tower.path b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path new file mode 100644 index 00000000..08c50d19 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score Score Tower.path @@ -0,0 +1,131 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.42896797153025, + "y": 0.5006049822064051 + }, + "prevControl": null, + "nextControl": { + "x": 6.656120996441281, + "y": 0.4145314353499395 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 5.9890510083036785, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 5.809794588396062, + "y": 0.6785071367427231 + }, + "nextControl": { + "x": 6.053606168446026, + "y": 4.4169513641755636 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.946619217081851, + "y": 3.6315302491103196 + }, + "prevControl": { + "x": 6.83902728351127, + "y": 4.1479715302491105 + }, + "nextControl": { + "x": 7.053981008712923, + "y": 3.116193649281173 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.763712930011864, + "y": 0.6512336892052202 + }, + "prevControl": { + "x": 7.013067438601093, + "y": 0.6691872138236445 + }, + "nextControl": { + "x": 5.418813760379598, + "y": 0.5544009489916971 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.6118742586002375, + "y": 0.6512336892052202 + }, + "prevControl": { + "x": 5.85994068801898, + "y": 0.6942704626334513 + }, + "nextControl": { + "x": 3.3189082801011907, + "y": 0.6066486554638754 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.9645077105575333, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 1.4917081850533813, + "y": 0.9417319098457893 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9449378330372995, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 4.360568383658999, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Score To Score.path b/src/main/deploy/pathplanner/paths/Right Score To Score.path new file mode 100644 index 00000000..f8bb4623 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Score To Score.path @@ -0,0 +1,125 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.042781740370898, + "y": 0.5221540656205423 + }, + "prevControl": null, + "nextControl": { + "x": 6.859686162624821, + "y": 0.4703994293865924 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 5.9890510083036785, + "y": 3.4271055753262156 + }, + "prevControl": { + "x": 6.186875891583452, + "y": 0.2504422253922973 + }, + "nextControl": { + "x": 5.924849720295674, + "y": 4.458047058785347 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.946619217081851, + "y": 3.6315302491103196 + }, + "prevControl": { + "x": 6.903582443653619, + "y": 4.104934756820878 + }, + "nextControl": { + "x": 6.994277354658343, + "y": 3.107290735768909 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.1295255041518395, + "y": 1.415136417556346 + }, + "prevControl": { + "x": 7.118173588922177, + "y": 1.6648785526089207 + }, + "nextControl": { + "x": 7.172562277580072, + "y": 0.4683274021352315 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5991440798858774, + "y": 0.71623395149786 + }, + "prevControl": { + "x": 5.718705171226329, + "y": 0.71623395149786 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Center NZ (2)" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9449378330372995, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.2, + "rotationDegrees": -90.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.467854869509843, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "To Score", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path new file mode 100644 index 00000000..d55f75b1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Trench Score To Tower Right.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.042781740370898, + "y": 0.5221540656205423 + }, + "prevControl": null, + "nextControl": { + "x": -0.008019763708495553, + "y": 0.7343747446748101 + }, + "isLocked": false, + "linkedName": "Right Trench Score" + }, + { + "anchor": { + "x": 0.9645077105575333, + "y": 2.5448517200474496 + }, + "prevControl": { + "x": 1.373357058125742, + "y": 1.3183036773428227 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Tower Right" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.41536050156739895, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "To Tower", + "idealStartingState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Right Trench To NZ.path b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path new file mode 100644 index 00000000..288b8ca0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Right Trench To NZ.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.42896797153025, + "y": 0.5006049822064051 + }, + "prevControl": null, + "nextControl": { + "x": 9.188644793152639, + "y": 0.48333808844507875 + }, + "isLocked": false, + "linkedName": "Right Trench Start" + }, + { + "anchor": { + "x": 8.282938659058486, + "y": 3.4333523537803137 + }, + "prevControl": { + "x": 8.153552068473608, + "y": 1.182025677603424 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Right Center NZ" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6276715410573667, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 7.5, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 90.0 + }, + "reversed": false, + "folder": "To NZ", + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path new file mode 100644 index 00000000..f77ecd7b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.48276393831554, + "y": 7.0 + }, + "prevControl": { + "x": 3.48276393831554, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 300.0, + "maxAngularAcceleration": 900.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "Tests", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 00000000..23f131a1 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,44 @@ +{ + "robotWidth": 0.981, + "robotLength": 0.762, + "holonomicMode": true, + "pathFolders": [ + "Misc", + "Tests", + "To NZ", + "To Score", + "To Tower" + ], + "autoFolders": [ + "Tests" + ], + "defaultMaxVel": 2.0, + "defaultMaxAccel": 5.0, + "defaultMaxAngVel": 300.0, + "defaultMaxAngAccel": 900.0, + "defaultNominalVoltage": 12.0, + "robotMass": 64.86, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.051, + "driveGearing": 5.35714285714, + "maxDriveSpeed": 4.93, + "driveMotorType": "krakenX60FOC", + "driveCurrentLimit": 120.0, + "wheelCOF": 1.542, + "flModuleX": 0.229, + "flModuleY": 0.33, + "frModuleX": 0.229, + "frModuleY": -0.33, + "blModuleX": -0.229, + "blModuleY": 0.33, + "brModuleX": -0.229, + "brModuleY": -0.33, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [ + "{\"name\":\"Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.475,\"y\":0.0},\"size\":{\"width\":0.98,\"length\":0.19},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}", + "{\"name\":\"Turret\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.15,\"y\":0.275},\"size\":{\"width\":0.3,\"length\":0.35},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Climber\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":-0.1,\"y\":-0.45},\"size\":{\"width\":0.1,\"length\":0.2},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":true}}" + ] +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java index 4fa0dcc5..ad54144d 100644 --- a/src/main/java/com/stuypulse/robot/Robot.java +++ b/src/main/java/com/stuypulse/robot/Robot.java @@ -5,13 +5,14 @@ /***************************************************************/ package com.stuypulse.robot; +import com.stuypulse.robot.commands.vision.SetIMUMode; import com.stuypulse.robot.commands.vision.SetMegaTagMode; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.vision.LimelightVision; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -24,7 +25,6 @@ public class Robot extends TimedRobot { private RobotContainer robot; private Command auto; private static Alliance alliance; - private PowerDistribution powerDistribution; public static boolean isBlue() { return alliance == Alliance.Blue; @@ -40,14 +40,17 @@ public void robotInit() { DataLogManager.start(); SignalLogger.start(); - - powerDistribution = new PowerDistribution(); + CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.RESET_IMU_INDEX)); } @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - SmartDashboard.putNumber("Robot/Voltage of Robot", powerDistribution.getVoltage()); + if (!Robot.isReal()) { + SmartDashboard.putData(CommandScheduler.getInstance()); + } + + SmartDashboard.putNumber("Robot/Match Time", DriverStation.getMatchTime()); if (DriverStation.getAlliance().isPresent()) { alliance = DriverStation.getAlliance().get(); @@ -64,7 +67,9 @@ public void disabledInit() { } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.RESET_IMU_INDEX)); + } /***********************/ /*** AUTONOMOUS MODE ***/ @@ -72,7 +77,9 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); + CommandScheduler.getInstance().schedule(new SetIMUMode(Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX)); auto = robot.getAutonomousCommand(); @@ -85,7 +92,9 @@ public void autonomousInit() { public void autonomousPeriodic() {} @Override - public void autonomousExit() {} + public void autonomousExit( + + ) {} /*******************/ /*** TELEOP MODE ***/ @@ -93,15 +102,19 @@ public void autonomousExit() {} @Override public void teleopInit() { + CommandScheduler.getInstance().schedule(new SetMegaTagMode(LimelightVision.MegaTagMode.MEGATAG2)); - + CommandScheduler.getInstance().schedule(new SetIMUMode(0)); + if (auto != null) { auto.cancel(); } + } @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override public void teleopExit() {} diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index b7124041..e2147423 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -9,75 +9,88 @@ import com.stuypulse.stuylib.input.gamepads.AutoGamepad; import com.stuypulse.stuylib.network.SmartBoolean; -import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; -import com.stuypulse.robot.commands.climberhopper.ClimberDown; -import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand; -import com.stuypulse.robot.commands.climberhopper.ClimberUp; +import com.stuypulse.robot.commands.auton.regular.DepotAuton; +import com.stuypulse.robot.commands.auton.regular.LeftTwoCycle; +import com.stuypulse.robot.commands.auton.regular.RightTwoCycle; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterFerry; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterInterpolation; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterLeftCorner; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterReverse; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterRightCorner; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; -import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; +import com.stuypulse.robot.commands.hood.SeedHoodRelativeEncoderAtUpperHardstop; import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeOuttake; +import com.stuypulse.robot.commands.intake.IntakeRunRollers; +import com.stuypulse.robot.commands.intake.IntakeSetState; import com.stuypulse.robot.commands.intake.IntakeStopRollers; import com.stuypulse.robot.commands.intake.IntakeStow; +import com.stuypulse.robot.commands.intake.ZeroPivotDeployed; +import com.stuypulse.robot.commands.intake.ZeroPivotStowed; +import com.stuypulse.robot.commands.spindexer.SpindexerReverse; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; -import com.stuypulse.robot.commands.swerve.SwerveClimbAlign; +import com.stuypulse.robot.commands.superstructure.SuperstructureFOTM; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.commands.superstructure.SuperstructureLeftCorner; +import com.stuypulse.robot.commands.superstructure.SuperstructureRightCorner; +import com.stuypulse.robot.commands.superstructure.SuperstructureSOTM; +import com.stuypulse.robot.commands.superstructure.SuperstructureStow; import com.stuypulse.robot.commands.swerve.SwerveDriveDrive; +import com.stuypulse.robot.commands.swerve.SwerveDriveFOTM; +import com.stuypulse.robot.commands.swerve.SwerveDriveSOTM; import com.stuypulse.robot.commands.swerve.SwerveResetHeading; import com.stuypulse.robot.commands.swerve.SwerveXMode; -import com.stuypulse.robot.commands.turret.TurretFerry; -import com.stuypulse.robot.commands.turret.TurretIdle; -import com.stuypulse.robot.commands.turret.TurretLeftCorner; -import com.stuypulse.robot.commands.turret.TurretRightCorner; -import com.stuypulse.robot.commands.turret.TurretSeed; -import com.stuypulse.robot.commands.turret.TurretShoot; +import com.stuypulse.robot.commands.turret.SeedTurret; +import com.stuypulse.robot.commands.turret.ZeroTurret; +import com.stuypulse.robot.commands.vision.ResetLimelightIMU; +import com.stuypulse.robot.commands.vision.SetIMUMode; +import com.stuypulse.robot.commands.vision.SetMegaTagMode; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import com.stuypulse.robot.subsystems.handoff.Handoff; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; -import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter; +import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import com.stuypulse.robot.subsystems.spindexer.Spindexer; +import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.subsystems.turret.Turret; import com.stuypulse.robot.subsystems.vision.LimelightVision; +import com.stuypulse.robot.subsystems.vision.LimelightVision.MegaTagMode; +import com.stuypulse.robot.util.PathUtil.AutonConfig; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; public class RobotContainer { public interface EnabledSubsystems { SmartBoolean SWERVE = new SmartBoolean("Enabled Subsystems/Swerve Is Enabled", true); - SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", false); - SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", false); - SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", false); - SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", false); - SmartBoolean CLIMBER_HOPPER = new SmartBoolean("Enabled Subsystems/Climber-Hopper Is Enabled", false); - SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", false); - SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", false); - SmartBoolean LIMELIGHT = new SmartBoolean("Enabled Subsystems/Limelight Is Enabled", false); + SmartBoolean TURRET = new SmartBoolean("Enabled Subsystems/Turret Is Enabled", true); + SmartBoolean HANDOFF = new SmartBoolean("Enabled Subsystems/Handoff Is Enabled", true); + SmartBoolean INTAKE = new SmartBoolean("Enabled Subsystems/Intake Is Enabled", true); + SmartBoolean SPINDEXER = new SmartBoolean("Enabled Subsystems/Spindexer Is Enabled", true); + SmartBoolean HOOD = new SmartBoolean("Enabled Subsystems/Hood Is Enabled", true); + SmartBoolean SHOOTER = new SmartBoolean("Enabled Subsystems/Shooter Is Enabled", true); + + SmartBoolean BACK_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Back Limelight Is Enabled", true); + SmartBoolean LEFT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Left Limelight Is Enabled", true); + SmartBoolean RIGHT_LIMELIGHT = new SmartBoolean("Enabled Subsystems/Right Limelight Is Enabled", true); } // Gamepads public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER); // Subsystem - private final ClimberHopper climberHopper = ClimberHopper.getInstance(); private final Handoff handoff = Handoff.getInstance(); private final Intake intake = Intake.getInstance(); private final Spindexer spindexer = Spindexer.getInstance(); @@ -86,7 +99,7 @@ public interface EnabledSubsystems { private final LimelightVision vision = LimelightVision.getInstance(); private final Turret turret = Turret.getInstance(); - private final HoodedShooter hoodedShooter = HoodedShooter.getInstance(); + private final Superstructure superstructure = Superstructure.getInstance(); private final Shooter shooter = Shooter.getInstance(); private final Hood hood = Hood.getInstance(); @@ -95,11 +108,34 @@ public interface EnabledSubsystems { // Robot container public RobotContainer() { + swerve.configureAutoBuilder(); configureDefaultCommands(); configureButtonBindings(); configureAutons(); + configureSysids(); SmartDashboard.putData("Field", Field.FIELD2D); + SmartDashboard.putData("Robot/Zero Pivot Encoder at Lower Limit (Deployed)", new ZeroPivotDeployed().ignoringDisable(true)); + SmartDashboard.putData("Robot/Zero Pivot Encoder at Upper Limit (Stowed)", new ZeroPivotStowed().ignoringDisable(true)); + SmartDashboard.putData("Robot/Zero Turret Encoders", new ZeroTurret().ignoringDisable(true)); + SmartDashboard.putData("Robot/Seed Turret", new SeedTurret().ignoringDisable(true)); + SmartDashboard.putData("Robot/Seed Hood Relative Encoder At Upper Hardstop", new SeedHoodRelativeEncoderAtUpperHardstop().ignoringDisable(true)); + SmartDashboard.putData("Robot/Set Megatag 1", new SetMegaTagMode(MegaTagMode.MEGATAG1).ignoringDisable(true)); + SmartDashboard.putData("Robot/Set Megatag 2", new SetMegaTagMode(MegaTagMode.MEGATAG2).ignoringDisable(true)); + + SmartDashboard.putData("Robot/Handoff Reverse", + new ConditionalCommand( + new HandoffReverse().andThen(new WaitCommand(0.25)).andThen(new HandoffRun()), + new HandoffReverse().andThen(new WaitCommand(0.25).andThen(new HandoffStop())), + () -> handoff.getState() == HandoffState.FORWARD)); + + SmartDashboard.putData("Robot/Intake Reverse", new IntakeSetState(RollerState.OUTTAKE)); + + SmartDashboard.putData("Robot/Spindexer Reverse", + new ConditionalCommand( + new SpindexerReverse().andThen(new WaitCommand(1)).andThen(new SpindexerRun()), + new SpindexerReverse().andThen(new WaitCommand(1).andThen(new SpindexerStop())), + () -> spindexer.getState() == SpindexerState.FORWARD)); } /****************/ @@ -108,7 +144,6 @@ public RobotContainer() { private void configureDefaultCommands() { swerve.setDefaultCommand(new SwerveDriveDrive(driver)); - climberHopper.setDefaultCommand(new ClimberHopperDefaultCommand()); } /***************/ @@ -116,154 +151,127 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { - // Intake Up and Off + // Scoring Routine + driver.getTopButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeRunRollers()) + .whileTrue(new SuperstructureInterpolation() + .alongWith(new WaitUntilCommand(() -> superstructure.getState() == SuperstructureState.INTERPOLATION && superstructure.atTolerance())) + .andThen(new HandoffRun()) + .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance())) + .andThen(new SpindexerRun())) + .onFalse(new SpindexerStop() + .alongWith(new SuperstructureStow()) + .alongWith(new HandoffStop())); + + // Intake Stow driver.getLeftTriggerButton() .onTrue(new IntakeStow()); - // Intake Down and On + // Intake Deploy driver.getRightTriggerButton() - .onTrue(new IntakeDeploy()); - + .onTrue(new IntakeDeploy()) + .onTrue(new SuperstructureStow() + .alongWith(new SpindexerStop()) //TODO: test this logic + .alongWith(new HandoffStop())); // TURNS OFF SOTM + // Reset Heading driver.getDPadUp() - .onTrue(new SwerveResetHeading()); - - // Scoring Routine using Interpolation Settings - driver.getTopButton() - .whileTrue(new HoodedShooterInterpolation() - .alongWith(new TurretShoot()) - .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) - .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) - .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) - .onFalse(new SpindexerStop() - .alongWith(new HoodedShooterStow()) - .alongWith(new HandoffStop())); - - // Ferry Routine using Interpolation Settings - driver.getBottomButton() - .onTrue(new HoodedShooterFerry() - .alongWith(new TurretFerry()) - .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) - .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) - .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance()))) - ) - .onFalse(new SpindexerStop() - .alongWith(new HoodedShooterStow()) - .alongWith(new HandoffStop())); - -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ -//-------------------------------------------------------------------------------------------------------------------------\\ - /* - // Climb Align - driver.getTopButton() - .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp())); + .onTrue(new SwerveResetHeading()) + .onTrue(new ResetLimelightIMU()) + .onFalse(new SetIMUMode(0)); - // Left Corner Shoot - driver.getLeftButton() - .whileTrue( - new SwerveXMode().alongWith( - new HoodedShooterLeftCorner().alongWith( - new TurretLeftCorner()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - - // Right Corner Shoot - driver.getRightButton() - .whileTrue( - new SwerveXMode().alongWith( - new HoodedShooterRightCorner().alongWith( - new TurretRightCorner()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - - // Hub Shoot - driver.getBottomButton() - .whileTrue( - new SwerveXMode().alongWith( - new HoodedShooterShoot().alongWith( - new TurretShoot()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - - // Intake Up and Off - driver.getLeftTriggerButton() - .onTrue(new IntakeStow()); - - // Intake Down and On - driver.getRightTriggerButton() - .onTrue(new IntakeDeploy()); - - // Climb Down Placeholder + // Stop Rollers driver.getLeftBumper() - .onTrue(new BuzzController(driver).alongWith(new ClimberDown())); + .onTrue(new IntakeDeploy().andThen(new IntakeStopRollers())); - // Climb Up Placeholder driver.getRightBumper() - .onTrue(new BuzzController(driver)) - .whileTrue(new ClimberUp()); - - // Reset Heading - driver.getDPadUp() - .onTrue(new SwerveResetHeading()); - - // Ferry In Place - driver.getDPadLeft() - .whileTrue( - new SwerveXMode().alongWith( - new HoodedShooterFerry().alongWith( - new TurretFerry()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - - // Score In Place - driver.getDPadRight() - .whileTrue( - new SwerveXMode().alongWith( - new HoodedShooterShoot().alongWith( - new TurretShoot()).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isHoodAtTolerance())).alongWith( - new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance())).alongWith( - new WaitUntilCommand(() -> turret.atTargetAngle())).andThen( - new SpindexerRun().alongWith(new HandoffRun())))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop())) - ); - */ + .whileTrue(new IntakeOuttake()) + .onFalse(new IntakeRunRollers()); + + // Ferrying In Place + // driver.getDPadRight() + // .whileTrue(new SwerveXMode()) + // .onTrue(new IntakeRunRollers()) + // .whileTrue(new SuperstructureInterpolation() // CURRENTLY, THIS PROVES THE FERRYING STATE IS BLOCKING SPINDEXER AND HANDOFF SOMEWHERE IN THE CODE + // .alongWith(new WaitUntilCommand(() -> superstructure.getState() == SuperstructureState.INTERPOLATION && superstructure.atTolerance())) + // .andThen(new HandoffRun()) + // .alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance())) + // .andThen(new SpindexerRun())) + // .onFalse(new SpindexerStop() + // .alongWith(new SuperstructureStow()) + // .alongWith(new HandoffStop())); + + // SOTM + driver.getRightMenuButton() + .onTrue(new IntakeRunRollers()) + .onTrue(new ConditionalCommand( + new ParallelCommandGroup( + new SuperstructureStow(), + new SpindexerStop(), + new HandoffStop() + ), + new ParallelCommandGroup( + new SuperstructureSOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .andThen(new SpindexerRun()), + new SwerveDriveSOTM(driver) + ), + () -> superstructure.getState() == SuperstructureState.SOTM + )); + + // FOTM + driver.getLeftMenuButton() + .onTrue(new IntakeRunRollers()) + .onTrue(new ConditionalCommand( + new ParallelCommandGroup( + new SuperstructureStow(), + new SpindexerStop(), + new HandoffStop() + ), + new ParallelCommandGroup( + new SuperstructureFOTM().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()) + .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) + .andThen(new SpindexerRun()), + new SwerveDriveFOTM(driver) + ), + () -> superstructure.getState() == SuperstructureState.FOTM + )); + + driver.getDPadDown() + .whileTrue(new SwerveXMode()); + +//--------------------------------------------------------------------------------------------------------------------------\\ + + // Manual Left Corner Scoring + driver.getLeftButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeRunRollers()) + .whileTrue(new SuperstructureLeftCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance()) + .andThen(new SpindexerRun()))) + .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // Manual Right Corner Scoring + driver.getRightButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeRunRollers()) + .whileTrue(new SuperstructureRightCorner().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance()) + .andThen(new SpindexerRun()))) + .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + + // Manual KB Distance Scoring + driver.getBottomButton() + .whileTrue(new SwerveXMode()) + .onTrue(new IntakeRunRollers()) + .whileTrue(new SuperstructureKB().alongWith(new WaitUntilCommand(() -> superstructure.atTolerance())) + .andThen(new HandoffRun()).alongWith(new WaitUntilCommand(() -> handoff.getState() == HandoffState.FORWARD && handoff.atTolerance()) + .andThen(new SpindexerRun()))) + .onFalse(new SuperstructureStow().alongWith(new SpindexerStop()).alongWith(new HandoffStop())); + } /**************/ @@ -271,17 +279,52 @@ private void configureButtonBindings() { /**************/ public void configureAutons() { + autonChooser.setDefaultOption("Do Nothing", new DoNothingAuton()); + // DEPOT + AutonConfig DEPOT_AUTON = new AutonConfig("Depot Auton", DepotAuton::new, + "Left Bump To Depot", "Depot To Tower Left"); + DEPOT_AUTON.register(autonChooser); + + // ONE CYCLES + // AutonConfig LEFT_ONE_CYCLE = new AutonConfig("Left One Cycle", LeftOneCycle::new, + // "Left Trench To NZ", "Left NZ To Score"); + // LEFT_ONE_CYCLE.register(autonChooser); + + // AutonConfig RIGHT_ONE_CYCLE = new AutonConfig("Right One Cycle", RightOneCycle::new, + // "Right Trench To NZ", "Right NZ To Score"); + // RIGHT_ONE_CYCLE.register(autonChooser); + + // TWO CYCLES + AutonConfig LEFT_TWO_CYCLE = new AutonConfig("Left Two Cycle", LeftTwoCycle::new, + "Left Trench To NZ", "Left NZ To Score", "Left Score To Score"); + LEFT_TWO_CYCLE.register(autonChooser); + + AutonConfig RIGHT_TWO_CYCLE = new AutonConfig("Right Two Cycle", RightTwoCycle::new, + "Right Trench To NZ", "Right NZ To Score", "Right Score To Score"); + RIGHT_TWO_CYCLE.register(autonChooser); + SmartDashboard.putData("Autonomous", autonChooser); + } public void configureSysids() { - // autonChooser.addOption("SysID Module Translation Dynamic Forward", swerve.sysIdDynamic(Direction.kForward)); + // autonChooser.addOption("SysID Module Translation Dynamic Forwards", swerve.sysIdDynamic(Direction.kForward)); // autonChooser.addOption("SysID Module Translation Dynamic Backwards", swerve.sysIdDynamic(Direction.kReverse)); // autonChooser.addOption("SysID Module Translation Quasi Forwards", swerve.sysIdQuasistatic(Direction.kForward)); - // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + // autonChooser.addOption("SysID Module Translation Quasi Backwards", swerve.sysIdQuasistatic(Direction.kReverse)); + + // autonChooser.addOption("SysID Rotation Translation Dynamic Forwards", swerve.sysidRotationDynamic(Direction.kForward)); + // autonChooser.addOption("SysID Rotation Translation Dynamic Backwards", swerve.sysidRotationDynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Rotation Translation Quasi Forwards", swerve.sysidRotationQuasiStatic(Direction.kForward)); + // autonChooser.addOption("SysID Rotation Translation Quasi Backwards", swerve.sysidRotationQuasiStatic(Direction.kReverse)); + + // autonChooser.addOption("SysID Turret Dynamic Forwards", turret.getSysIdRoutine().dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Turret Dynamic Reverse", turret.getSysIdRoutine().dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Turret Quasistatic Forwards", turret.getSysIdRoutine().quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Turret Quasistatic Reverse", turret.getSysIdRoutine().quasistatic(Direction.kReverse)); // autonChooser.addOption("SysID Module Rotation Dynamic Forwards", swerve.sysIdRotDynamic(Direction.kForward)); // autonChooser.addOption("SysID Module Rotation Dynamic Backwards", swerve.sysIdRotDynamic(Direction.kReverse)); @@ -289,35 +332,37 @@ public void configureSysids() { // autonChooser.addOption("SysID Module Rotation Quasi Backwards", swerve.sysIdRotQuasi(Direction.kReverse)); // SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine(); - // autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Shooter Dynamic Forwards", shooterSysId.dynamic(Direction.kForward)); // autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse)); // autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward)); // autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse)); // SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine(); - // autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Hood Dynamic Forwards", hoodSysId.dynamic(Direction.kForward)); // autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse)); // autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward)); // autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse)); // SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine(); - // autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Intake Pivot Dynamic Forwards", intakePivotSysId.dynamic(Direction.kForward)); // autonChooser.addOption("SysID Intake Pivot Dynamic Backwards", intakePivotSysId.dynamic(Direction.kReverse)); // autonChooser.addOption("SysID Intake Pivot Quasi Forwards", intakePivotSysId.quasistatic(Direction.kForward)); // autonChooser.addOption("SysID Intake Pivot Quasi Backwards", intakePivotSysId.quasistatic(Direction.kReverse)); // SysIdRoutine spindexerSysId = spindexer.getSysIdRoutine(); - // autonChooser.addOption("SysID Spindexer Dynamic Forward", spindexerSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Spindexer Dynamic Forwards", spindexerSysId.dynamic(Direction.kForward)); // autonChooser.addOption("SysID Spindexer Dynamic Backwards", spindexerSysId.dynamic(Direction.kReverse)); // autonChooser.addOption("SysID Spindexer Quasi Forwards", spindexerSysId.quasistatic(Direction.kForward)); // autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse)); - // SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); - // autonChooser.addOption("SysID Handoff Forward", handoffSysId.dynamic(Direction.kForward)); - // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.dynamic(Direction.kReverse)); - // autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); - // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); + // // Wheel Radius Characterization + // autonChooser.addOption("Wheel Characterization", new SwerveWheelRadiusCharacterization()); + // SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); + // autonChooser.addOption("SysID Handoff Dynamic Forward", handoffSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Dynamic Backwards", handoffSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Handoff Quasi Forwards", handoffSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Quasi Backwards", handoffSysId.quasistatic(Direction.kReverse)); } public Command getAutonomousCommand() { diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java new file mode 100644 index 00000000..0cbe7062 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/DepotAuton.java @@ -0,0 +1,51 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeStow; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class DepotAuton extends SequentialCommandGroup { + + public DepotAuton(PathPlannerPath... paths) { + + addCommands( + + // To Depot + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + + new IntakeStow().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]) + ), + + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java new file mode 100644 index 00000000..541e3a30 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/EightFuel.java @@ -0,0 +1,34 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.SuperstructureKB; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class EightFuel extends SequentialCommandGroup { + + public EightFuel(PathPlannerPath... paths) { + + addCommands( + + new SuperstructureKB().alongWith( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()).andThen( + new SpindexerRun().alongWith(new HandoffRun()) + ) + ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java new file mode 100644 index 00000000..806244ef --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftOneCycle.java @@ -0,0 +1,54 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class LeftOneCycle extends SequentialCommandGroup { + + public LeftOneCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + new SuperstructureInterpolation(), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java new file mode 100644 index 00000000..4d2dc494 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/LeftTwoCycle.java @@ -0,0 +1,77 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeRunRollers; +import com.stuypulse.robot.commands.intake.IntakeStopRollers; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class LeftTwoCycle extends SequentialCommandGroup { + + public LeftTwoCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.5).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new WaitCommand(0.5).andThen(new SuperstructureAutoInterpolation()) + ), + new SuperstructureInterpolation(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() + ).andThen(new WaitCommand(4.0)), + new SuperstructureAutoInterpolation(), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + new SuperstructureInterpolation(), + + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java new file mode 100644 index 00000000..0a4d5fb2 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightOneCycle.java @@ -0,0 +1,54 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class RightOneCycle extends SequentialCommandGroup { + + public RightOneCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + new IntakeDeploy().alongWith( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]) + ), + new SuperstructureInterpolation(), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new SpindexerRun().alongWith( + new HandoffRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java new file mode 100644 index 00000000..6d96ddf5 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/regular/RightTwoCycle.java @@ -0,0 +1,77 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.regular; + +import com.stuypulse.robot.commands.handoff.HandoffRun; +import com.stuypulse.robot.commands.handoff.HandoffStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeRunRollers; +import com.stuypulse.robot.commands.intake.IntakeStopRollers; +import com.stuypulse.robot.commands.spindexer.SpindexerRun; +import com.stuypulse.robot.commands.spindexer.SpindexerStop; +import com.stuypulse.robot.commands.superstructure.SuperstructureAutoInterpolation; +import com.stuypulse.robot.commands.superstructure.SuperstructureInterpolation; +import com.stuypulse.robot.subsystems.handoff.Handoff; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class RightTwoCycle extends SequentialCommandGroup { + + public RightTwoCycle(PathPlannerPath... paths) { + + addCommands( + + // NZ Trip 1 + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]).alongWith( + new WaitCommand(0.5).andThen(new IntakeDeploy()) + ), + + // Trip 1 To Score + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]).alongWith( + new WaitCommand(0.5).andThen(new SuperstructureAutoInterpolation()) + ), + new SuperstructureInterpolation(), + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()), + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() + ).andThen(new WaitCommand(4.0)), + new SuperstructureAutoInterpolation(), + + // NZ Trip 2 + new ParallelCommandGroup( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + new HandoffStop(), + new SpindexerStop() + ), + new SuperstructureInterpolation(), + + new ParallelCommandGroup( + new WaitUntilCommand(() -> Superstructure.getInstance().atTolerance()) + // new SwerveClimbAlign() + ), + new HandoffRun().alongWith(new WaitUntilCommand(() -> Handoff.getInstance().atTolerance())).andThen( + new SpindexerRun() + ) + // .until(() -> DriverStation.getMatchTime() < 2).andThen( + // new ParallelCommandGroup( + // new HandoffStop(), + // new SpindexerStop(), + // new ClimberDown() + // ) + // ) + + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java new file mode 100644 index 00000000..1ad35b24 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/test/BoxTest.java @@ -0,0 +1,27 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.auton.test; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import com.pathplanner.lib.path.PathPlannerPath; + +public class BoxTest extends SequentialCommandGroup { + + public BoxTest(PathPlannerPath... paths) { + + addCommands( + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[0]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[1]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[2]), + CommandSwerveDrivetrain.getInstance().followPathCommand(paths[3]) + ); + + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java deleted file mode 100644 index 434b1b25..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java +++ /dev/null @@ -1,78 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.climberhopper; - -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -import edu.wpi.first.wpilibj2.command.Command; - - -public class ClimberHopperDefaultCommand extends Command { - private final ClimberHopper climberHopper; - // private final CommandSwerveDrivetrain swerve; - // private Pose2d pose; - // private boolean flag = false; // To prevent repeated stalling under trench - - public ClimberHopperDefaultCommand() { - climberHopper = ClimberHopper.getInstance(); - // swerve = CommandSwerveDrivetrain.getInstance(); - // pose = swerve.getPose(); - - - addRequirements(climberHopper); - } - - @Override - public void execute() { - // === Robot Position Logic === - // Reminder from driver's perspective, positive X to the opposite alliance and positive Y points to the left. - // pose = swerve.getPose(); - - // boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY(); - - // boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY(); - - // boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance; - - // boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance; - - // boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance - - // // === Climber Position and State Logic === - // boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN; - - // boolean stalledByBalls = climberHopper.getStalling() && inDownState; - // // boolean stalledByBalls = true; - - // if (isUnderTrench) { - // if (!stalledByBalls && !flag) { - // climberHopper.setState(ClimberHopperState.HOPPER_DOWN); - // } else { - // climberHopper.setState(ClimberHopperState.HOPPER_UP); - // // TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!! - // flag = true; // prevent hopper from going back down while still under trench with too many balls - // } - // } else { // If not under trench... - // if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { - // // Set the hopper up - // climberHopper.setState(ClimberHopperState.HOPPER_UP); - // } - // } - - // if (!isUnderTrench) { - // flag = false; - // } - - // SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench); - - // !!! AFTER ABANDONING VERTICAL EXPANSION: - if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { - // Set the hopper down - climberHopper.setState(ClimberHopperState.HOPPER_DOWN); - } - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java deleted file mode 100644 index fd669f38..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java +++ /dev/null @@ -1,46 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.climberhopper; - -// import java.util.Optional; - -// import com.stuypulse.robot.Robot; -// import com.stuypulse.robot.constants.Settings; -// import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ClimberHopperReset extends Command { - // private final ClimberHopper climberHopper; - - // public ClimberHopperReset() { - // climberHopper = ClimberHopper.getInstance(); - // } - - // @Override - // public void initialize() { - // climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE)); - // } - - // @Override - // public boolean isFinished() { - // if (climberHopper.getStalling()) { - // climberHopper.resetPostionUpper(); - // return true; - // } - // return false; - // } - - // @Override - // public void end(boolean interrupted) { - // climberHopper.setVoltageOverride(Optional.empty()); - // } - - // @Override - // public void execute() { - - // } -} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java deleted file mode 100644 index 85d8d6cd..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java +++ /dev/null @@ -1,26 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.climberhopper; - -import com.stuypulse.robot.subsystems.climberhopper.*; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class ClimberHopperSetState extends InstantCommand { - private final ClimberHopper climberHopper = ClimberHopper.getInstance(); - private final ClimberHopperState state; - - public ClimberHopperSetState(ClimberHopperState state) { - this.state = state; - addRequirements(climberHopper); - } - - @Override - public void initialize() { - climberHopper.setState(state); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java deleted file mode 100644 index 03fca2a9..00000000 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.climberhopper; - -// import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; - -// public class HopperUp extends ClimberHopperSetState { -// public HopperUp() { -// super(ClimberHopperState.HOPPER_UP); -// } -// } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java index 1176cae4..663b21c5 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffReverse.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -public class HandoffReverse extends SetHandoffState { +public class HandoffReverse extends HandoffSetState { public HandoffReverse() { super(HandoffState.REVERSE); } diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java index dd166bdf..ebd37507 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffRun.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.commands.handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -public class HandoffRun extends SetHandoffState { +public class HandoffRun extends HandoffSetState { public HandoffRun() { super(HandoffState.FORWARD); } diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/SetHandoffState.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java similarity index 77% rename from src/main/java/com/stuypulse/robot/commands/handoff/SetHandoffState.java rename to src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java index d2e5787c..ce13ffd6 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/SetHandoffState.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffSetState.java @@ -8,13 +8,13 @@ import com.stuypulse.robot.subsystems.handoff.Handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class SetHandoffState extends InstantCommand{ +public class HandoffSetState extends Command { private final Handoff handoff; private HandoffState state; - public SetHandoffState(HandoffState state) { + public HandoffSetState(HandoffState state) { this.handoff = Handoff.getInstance(); this.state = state; addRequirements(handoff); @@ -24,4 +24,9 @@ public SetHandoffState(HandoffState state) { public void initialize() { handoff.setState(state); } + + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java index 964b89ae..738159c0 100644 --- a/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java +++ b/src/main/java/com/stuypulse/robot/commands/handoff/HandoffStop.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.commands.handoff; import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState; -public class HandoffStop extends SetHandoffState { +public class HandoffStop extends HandoffSetState { public HandoffStop() { super(HandoffState.STOP); } diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java b/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java new file mode 100644 index 00000000..28c32fee --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hood/HomingRoutine.java @@ -0,0 +1,22 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.hood; + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class HomingRoutine extends InstantCommand { + private Hood hood; + public HomingRoutine() { + hood = Hood.getInstance(); + } + + @Override + public void initialize() { + hood.setState(Hood.HoodState.HOMING); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java new file mode 100644 index 00000000..ecac6c79 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hood/HoodAnalog.java @@ -0,0 +1,35 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.hood; + +import com.stuypulse.stuylib.input.Gamepad; + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; + +import edu.wpi.first.wpilibj2.command.Command; + +public class HoodAnalog extends Command{ + private final Hood hood; + private final Gamepad driver; + + public HoodAnalog(Gamepad driver) { + this.driver = driver; + hood = Hood.getInstance(); + + addRequirements(hood); + } + + @Override + public void initialize() { + hood.setState(Hood.HoodState.ANALOG); + } + + @Override + public void execute() { + hood.hoodAnalogToInput(driver); + //SmartDashboard.putNumber("Superstructure/Hood/Analog", hood.hoodAnalogToOutput().getDegrees()); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java b/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java new file mode 100644 index 00000000..5a518fa0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/hood/SeedHoodRelativeEncoderAtUpperHardstop.java @@ -0,0 +1,25 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.hood; + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SeedHoodRelativeEncoderAtUpperHardstop extends InstantCommand { + private final Hood hood; + + public SeedHoodRelativeEncoderAtUpperHardstop() { + this.hood = Hood.getInstance(); + + addRequirements(hood); + } + + @Override + public void initialize() { + hood.seedHoodAtUpperHardStop(); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterLeftCorner.java deleted file mode 100644 index 9c1556b3..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterLeftCorner.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; - -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; - -public class HoodedShooterLeftCorner extends HoodedShooterSetState { - public HoodedShooterLeftCorner() { - super(HoodedShooterState.LEFT_CORNER); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterRightCorner.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterRightCorner.java deleted file mode 100644 index a189811c..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterRightCorner.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; - -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; - -public class HoodedShooterRightCorner extends HoodedShooterSetState { - public HoodedShooterRightCorner() { - super(HoodedShooterState.RIGHT_CORNER); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterSetState.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterSetState.java deleted file mode 100644 index e110761f..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterSetState.java +++ /dev/null @@ -1,28 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; - -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; - -import edu.wpi.first.wpilibj2.command.InstantCommand; - -public class HoodedShooterSetState extends InstantCommand { - private final HoodedShooter hoodedshooter; - private final HoodedShooterState state; - - public HoodedShooterSetState(HoodedShooterState state) { - hoodedshooter = HoodedShooter.getInstance(); - this.state = state; - - addRequirements(hoodedshooter); - } - - @Override - public void execute() { - hoodedshooter.setState(state); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterShoot.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterShoot.java deleted file mode 100644 index 6850da29..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterShoot.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; - -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; - -public class HoodedShooterShoot extends HoodedShooterSetState { - public HoodedShooterShoot() { - super(HoodedShooterState.SHOOT); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterStow.java b/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterStow.java deleted file mode 100644 index aad5c5c4..00000000 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterStow.java +++ /dev/null @@ -1,14 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; - -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; - -public class HoodedShooterStow extends HoodedShooterSetState{ - public HoodedShooterStow(){ - super(HoodedShooterState.STOW); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java index de0fac60..c1f70b71 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java @@ -10,6 +10,6 @@ public class IntakeDeploy extends IntakeSetState { public IntakeDeploy() { - super(PivotState.DEPLOYED, RollerState.INTAKE); + super(PivotState.DEPLOY, RollerState.INTAKE); } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java new file mode 100644 index 00000000..f1f51a83 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeHoming.java @@ -0,0 +1,16 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +public class IntakeHoming extends IntakeSetState { + + public IntakeHoming() { //TODO: ensure this works/overrides the Intake Deploy w/o conflicts + super(PivotState.HOMING); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeOuttake.java similarity index 60% rename from src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeOuttake.java index 8ad2e8d7..a72c138f 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretZero.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeOuttake.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.turret; +package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class TurretZero extends TurretSetState { - public TurretZero() { - super(TurretState.ZERO); +public class IntakeOuttake extends IntakeSetState { + public IntakeOuttake() { + super(RollerState.OUTTAKE); } -} \ No newline at end of file +} diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeRunRollers.java similarity index 57% rename from src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeRunRollers.java index c25a1dfd..544f8fdb 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerDynamic.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeRunRollers.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.spindexer; +package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class SpindexerDynamic extends SetSpindexerState { - public SpindexerDynamic() { - super(SpindexerState.DYNAMIC); +public class IntakeRunRollers extends IntakeSetState { + public IntakeRunRollers() { + super(RollerState.INTAKE); } -} \ No newline at end of file +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java index 56f2cb0c..bfd917ad 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java @@ -10,6 +10,6 @@ public class IntakeStow extends IntakeSetState { public IntakeStow() { - super(PivotState.STOWED, RollerState.STOP); + super(PivotState.STOW, RollerState.STOP); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java new file mode 100644 index 00000000..ec51b2bc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotDeployed.java @@ -0,0 +1,25 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ZeroPivotDeployed extends InstantCommand { + private Intake intake; + + public ZeroPivotDeployed() { + intake = Intake.getInstance(); + } + + @Override + public void initialize() { + intake.zeroPivotDeployed(); + intake.setPivotState(PivotState.DEPLOY); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java new file mode 100644 index 00000000..43a9093c --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/intake/ZeroPivotStowed.java @@ -0,0 +1,25 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.intake; + +import com.stuypulse.robot.subsystems.intake.Intake; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ZeroPivotStowed extends InstantCommand { + private Intake intake; + + public ZeroPivotStowed() { + intake = Intake.getInstance(); + } + + @Override + public void initialize() { + intake.zeroPivotStowed(); + intake.setPivotState(PivotState.STOW); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java index 77795d39..721e92ec 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerReverse.java @@ -6,7 +6,7 @@ package com.stuypulse.robot.commands.spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -public class SpindexerReverse extends SetSpindexerState{ +public class SpindexerReverse extends SpindexerSetState{ public SpindexerReverse() { super(SpindexerState.REVERSE); } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java index 79fc4c8c..a46f2db4 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerRun.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -public class SpindexerRun extends SetSpindexerState{ +public class SpindexerRun extends SpindexerSetState{ public SpindexerRun(){ super(SpindexerState.FORWARD); } diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SetSpindexerState.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java similarity index 78% rename from src/main/java/com/stuypulse/robot/commands/spindexer/SetSpindexerState.java rename to src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java index ec4dde5b..81474ebe 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SetSpindexerState.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerSetState.java @@ -8,13 +8,13 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer; import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Command; -public class SetSpindexerState extends InstantCommand { +public class SpindexerSetState extends Command { private final Spindexer spindexer; private SpindexerState state; - public SetSpindexerState(SpindexerState state) { + public SpindexerSetState(SpindexerState state) { this.spindexer = Spindexer.getInstance(); this.state = state; addRequirements(spindexer); @@ -24,4 +24,9 @@ public SetSpindexerState(SpindexerState state) { public void initialize() { spindexer.setState(state); } + + @Override + public boolean isFinished() { + return true; + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java index cf39063f..362be749 100644 --- a/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java +++ b/src/main/java/com/stuypulse/robot/commands/spindexer/SpindexerStop.java @@ -7,7 +7,7 @@ import com.stuypulse.robot.subsystems.spindexer.Spindexer.SpindexerState; -public class SpindexerStop extends SetSpindexerState { +public class SpindexerStop extends SpindexerSetState { public SpindexerStop() { super(SpindexerState.STOP); } diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureAutoInterpolation.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureAutoInterpolation.java new file mode 100644 index 00000000..589eaa7f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureAutoInterpolation.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureAutoInterpolation extends SuperstructureSetState { + public SuperstructureAutoInterpolation() { + super(SuperstructureState.AUTO_INTERPOLATION); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterFerry.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFOTM.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterFerry.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFOTM.java index 7bd7c239..abace6a7 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFOTM.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterFerry extends HoodedShooterSetState { - public HoodedShooterFerry(){ - super(HoodedShooterState.FERRY); +public class SuperstructureFOTM extends SuperstructureSetState { + public SuperstructureFOTM() { + super(SuperstructureState.FOTM); } -} \ No newline at end of file +} diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterReverse.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java similarity index 55% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterReverse.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java index c0b62b07..755b9642 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterReverse.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureFerry.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterReverse extends HoodedShooterSetState { - public HoodedShooterReverse() { - super(HoodedShooterState.REVERSE); +public class SuperstructureFerry extends SuperstructureSetState { + public SuperstructureFerry() { + super(SuperstructureState.FERRY); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureInterpolation.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureInterpolation.java new file mode 100644 index 00000000..d9f7dffe --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureInterpolation.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureInterpolation extends SuperstructureSetState { + public SuperstructureInterpolation() { + super(SuperstructureState.INTERPOLATION); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureKB.java similarity index 55% rename from src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureKB.java index 57036b6f..fe5ad13c 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberDown.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureKB.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.climberhopper; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class ClimberDown extends ClimberHopperSetState { - public ClimberDown() { - super(ClimberHopperState.CLIMBER_DOWN); +public class SuperstructureKB extends SuperstructureSetState { + public SuperstructureKB() { + super(SuperstructureState.KB); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java new file mode 100644 index 00000000..6fb36bde --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureLeftCorner.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureLeftCorner extends SuperstructureSetState { + public SuperstructureLeftCorner() { + super(SuperstructureState.LEFT_CORNER); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureReverse.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureReverse.java new file mode 100644 index 00000000..b64d15a4 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureReverse.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureReverse extends SuperstructureSetState { + public SuperstructureReverse() { + super(SuperstructureState.REVERSE); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureRightCorner.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureRightCorner.java index 26ad75b4..a305a7fa 100644 --- a/src/main/java/com/stuypulse/robot/commands/hoodedshooter/HoodedShooterInterpolation.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureRightCorner.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.hoodedshooter; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter.HoodedShooterState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HoodedShooterInterpolation extends HoodedShooterSetState { - public HoodedShooterInterpolation() { - super(HoodedShooterState.INTERPOLATION); +public class SuperstructureRightCorner extends SuperstructureSetState { + public SuperstructureRightCorner() { + super(SuperstructureState.RIGHT_CORNER); } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTM.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTM.java new file mode 100644 index 00000000..786c27cf --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSOTM.java @@ -0,0 +1,14 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +public class SuperstructureSOTM extends SuperstructureSetState { + public SuperstructureSOTM() { + super(SuperstructureState.SOTM); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java new file mode 100644 index 00000000..96e68c3f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureSetState.java @@ -0,0 +1,33 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; + +import edu.wpi.first.wpilibj2.command.Command; + +public class SuperstructureSetState extends Command { + private final Superstructure superstructure; + private final SuperstructureState state; + + public SuperstructureSetState(SuperstructureState state) { + superstructure = Superstructure.getInstance(); + this.state = state; + + addRequirements(superstructure); + } + + @Override + public void execute() { + superstructure.setState(state); + } + + @Override + public boolean isFinished() { + return superstructure.getState() == state; + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java index d9db87ce..6afd299e 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberUp.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureShoot.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.climberhopper; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class ClimberUp extends ClimberHopperSetState { - public ClimberUp() { - super(ClimberHopperState.CLIMBER_UP); +public class SuperstructureShoot extends SuperstructureSetState { + public SuperstructureShoot() { + super(SuperstructureState.SHOOT); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java similarity index 54% rename from src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java rename to src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java index 5eb46595..0fbac6ea 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperDown.java +++ b/src/main/java/com/stuypulse/robot/commands/superstructure/SuperstructureStow.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.climberhopper; +package com.stuypulse.robot.commands.superstructure; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; -public class HopperDown extends ClimberHopperSetState { - public HopperDown() { - super(ClimberHopperState.HOPPER_DOWN); +public class SuperstructureStow extends SuperstructureSetState { + public SuperstructureStow() { + super(SuperstructureState.STOW); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlign.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlign.java deleted file mode 100644 index 41444f68..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlign.java +++ /dev/null @@ -1,16 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.wpilibj2.command.ConditionalCommand; - -public class SwerveClimbAlign extends ConditionalCommand{ - public SwerveClimbAlign(){ - super(new SwerveClimbAlignTop(), new SwerveClimbAlignBot(), () -> Field.closerToTop()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignBot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignBot.java deleted file mode 100644 index 835fdd5c..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignBot.java +++ /dev/null @@ -1,23 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; - -public class SwerveClimbAlignBot extends SwerveDrivePIDToPose{ - public SwerveClimbAlignBot(){ - super(new Pose2d(Field.towerCenter.getX(), Field.towerCenter.getY() - Field.barDisplacement - Field.DISTANCE_TO_RUNGS, new Rotation2d(0))); - } - - @Override - public void execute(){ - super.execute(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignTop.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignTop.java deleted file mode 100644 index 3b908c21..00000000 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveClimbAlignTop.java +++ /dev/null @@ -1,25 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.commands.swerve; - -import com.stuypulse.robot.commands.swerve.pidToPose.SwerveDrivePIDToPose; -import com.stuypulse.robot.constants.Field; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; - - -public class SwerveClimbAlignTop extends SwerveDrivePIDToPose{ - public SwerveClimbAlignTop(){ - super(new Pose2d(Field.towerCenter.getX(), Field.towerCenter.getY() + Field.barDisplacement + Field.DISTANCE_TO_RUNGS, new Rotation2d(Units.degreesToRadians(180)))); - } - - @Override - public void execute(){ - super.execute(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java new file mode 100644 index 00000000..b49bfed3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveAlignTurretToHub.java @@ -0,0 +1,80 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.control.angle.AngleController; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.streams.angles.filters.AMotionProfile; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Gains.Swerve.Alignment; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; + +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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +import com.ctre.phoenix6.swerve.SwerveRequest; + +public class SwerveDriveAlignTurretToHub extends Command { + + private final CommandSwerveDrivetrain swerve; + private final Turret turret; + private final AngleController angleController; + + private Pose2d robot; + + public SwerveDriveAlignTurretToHub() { + swerve = CommandSwerveDrivetrain.getInstance(); + turret = Turret.getInstance(); + + angleController = new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) + .setSetpointFilter(new AMotionProfile(Settings.Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, Settings.Swerve.Constraints.MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED)); + + addRequirements(swerve); + } + + protected double getAngleError() { + return angleController.getError().getRotation2d().getDegrees(); + } + + public Rotation2d getTargetAngle() { + Translation2d robot = swerve.getPose().getTranslation(); + Translation2d hub = Field.getHubPose().getTranslation(); + return robot.minus(hub).getAngle().plus(Rotation2d.k180deg); + } + + @Override + public void execute() { + robot = swerve.getPose(); + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(angleController.update( + Angle.fromRotation2d(getTargetAngle()), + Angle.fromRotation2d(robot.getRotation())))); + + SmartDashboard.putNumber("Swerve/Angle Error", angleController.getError().toDegrees()); + SmartDashboard.putNumber("Swerve/Target Angle Hub Deg", TurretAngleCalculator.getPointAtTargetAngle(Field.getHubPose().getTranslation(), swerve.getTurretPose().getTranslation()).getDegrees()); + } + + @Override + public boolean isFinished() { + return angleController.isDoneDegrees(Settings.Swerve.Alignment.Tolerances.THETA_TOLERANCE_DEG); + } + + @Override + public void end(boolean interrupted) { + swerve.setControl(new SwerveRequest.SwerveDriveBrake()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java index df7fb7a9..33fd1e47 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java @@ -38,7 +38,7 @@ public SwerveDriveDrive(Gamepad driver) { .filtered( new VDeadZone(Drive.DEADBAND), x -> x.clamp(1), - x -> x.pow(Drive.POWER.get()), + x -> x.pow(Drive.POWER), x -> x.mul(Swerve.Constraints.MAX_VELOCITY_M_PER_S), new VRateLimit(Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED), new VLowPassFilter(Drive.RC) @@ -46,8 +46,8 @@ public SwerveDriveDrive(Gamepad driver) { turn = IStream.create(driver::getRightX) .filtered( - x -> SLMath.deadband(x, Turn.DEADBAND.get()), - x -> SLMath.spow(x, Turn.POWER.get()), + x -> SLMath.deadband(x, Turn.DEADBAND), + x -> SLMath.spow(x, Turn.POWER), x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, new LowPassFilter(Turn.RC) ); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java new file mode 100644 index 00000000..792d9711 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveFOTM.java @@ -0,0 +1,92 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveFOTM extends Command{ + + private final CommandSwerveDrivetrain swerve; + private final Superstructure superstructure; + + private final Gamepad driver; + + private final VStream speed; + private final IStream turn; + + + public SwerveDriveFOTM(Gamepad driver) { + swerve = CommandSwerveDrivetrain.getInstance(); + superstructure = Superstructure.getInstance(); + + speed = VStream.create(this::getDriverInputAsVelocity) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER), + x -> x.mul(Swerve.Constraints.MAX_VELOCITY_FOTM_M_PER_S), + new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_FOTM), + new VLowPassFilter(Drive.RC) + ); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND), + x -> SLMath.spow(x, Turn.POWER), + x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_FOTM_RAD_PER_S, + new LowPassFilter(Turn.RC) + ); + + this.driver = driver; + + addRequirements(swerve); + } + + private Vector2D getDriverInputAsVelocity() { + return new Vector2D(driver.getLeftStick().y, -driver.getLeftStick().x); + } + + @Override + public void execute() { + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-turn.get())); + + SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); + SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); + } + + @Override + public boolean isFinished() { + SuperstructureState state = superstructure.getState(); + if (state == SuperstructureState.FOTM) { + return false; + } else { + return true; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java new file mode 100644 index 00000000..c28bcbe9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveSOTM.java @@ -0,0 +1,92 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.stuylib.input.Gamepad; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit; + +import com.stuypulse.robot.constants.DriverConstants.Driver.Drive; +import com.stuypulse.robot.constants.DriverConstants.Driver.Turn; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveSOTM extends Command { + + private final CommandSwerveDrivetrain swerve; + private final Superstructure superstructure; + + private final Gamepad driver; + + private final VStream speed; + private final IStream turn; + + + public SwerveDriveSOTM(Gamepad driver) { + swerve = CommandSwerveDrivetrain.getInstance(); + superstructure = Superstructure.getInstance(); + + speed = VStream.create(this::getDriverInputAsVelocity) + .filtered( + new VDeadZone(Drive.DEADBAND), + x -> x.clamp(1), + x -> x.pow(Drive.POWER), + x -> x.mul(Swerve.Constraints.MAX_VELOCITY_SOTM_M_PER_S), + new VRateLimit(Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED_SOTM), + new VLowPassFilter(Drive.RC) + ); + + turn = IStream.create(driver::getRightX) + .filtered( + x -> SLMath.deadband(x, Turn.DEADBAND), + x -> SLMath.spow(x, Turn.POWER), + x -> x * Swerve.Constraints.MAX_ANGULAR_VEL_SOTM_RAD_PER_S, + new LowPassFilter(Turn.RC) + ); + + this.driver = driver; + + addRequirements(swerve); + } + + private Vector2D getDriverInputAsVelocity() { + return new Vector2D(driver.getLeftStick().y, -driver.getLeftStick().x); + } + + @Override + public void execute() { + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(speed.get().x) + .withVelocityY(speed.get().y) + .withRotationalRate(-turn.get())); + + SmartDashboard.putNumber("Swerve/Speed x", speed.get().x); + SmartDashboard.putNumber("Swerve/Speed y", speed.get().y); + } + + @Override + public boolean isFinished() { + SuperstructureState state = superstructure.getState(); + if (state == SuperstructureState.SOTM) { + return false; + } else { + return true; + } + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java new file mode 100644 index 00000000..4f480ae0 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveWheelRadiusCharacterization.java @@ -0,0 +1,127 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveWheelRadiusCharacterization extends Command { + + /* + * - Triple check the track width and length + * - Double check the gear ratio in getWheelDrivePositionsRadians() + * - Allow the robot to rotate 3-5 times when running the routine + */ + + private static final double HALF_TRACK_WIDTH_INCHES = 9.0; + private static final double HALF_TRACK_LENGTH_INCHES = 13.0; + + private static final double DRIVE_RADIUS_INCHES = Math.sqrt(HALF_TRACK_WIDTH_INCHES * HALF_TRACK_WIDTH_INCHES + HALF_TRACK_LENGTH_INCHES * HALF_TRACK_LENGTH_INCHES); + private static final double DRIVE_RADIUS_METERS = Units.inchesToMeters(DRIVE_RADIUS_INCHES); + + private static final double TARGET_ROTATIONAL_RATE = 1.5; // rad/s about center of rotation + private static final double RAMP_TIME = 0.75; // seconds to reach target + + private final Timer timer = new Timer(); + + private final CommandSwerveDrivetrain swerve; + + private double[] wheelInitialRad; + private Rotation2d lastAngle; + private double gyroDeltaRad; + private boolean initalReading; + + public SwerveWheelRadiusCharacterization() { + swerve = CommandSwerveDrivetrain.getInstance(); + addRequirements(swerve); + } + + @Override + public void initialize() { + timer.restart(); + // limiter.reset(0.0); + gyroDeltaRad = 0.0; + initalReading = false; + } + + @Override + public void execute() { + double elapsed = timer.get(); + + double commandedRate; + + if (elapsed < RAMP_TIME) { + commandedRate = TARGET_ROTATIONAL_RATE * (elapsed / RAMP_TIME); // Ramping omega proportional to RAMP_TIME + } else { + commandedRate = TARGET_ROTATIONAL_RATE; // Steady state omega + } + + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0.0) + .withVelocityY(0.0) + .withRotationalRate(commandedRate) + ); + + // Wait half a second after reaching target omega to start data collection + if (timer.get() > RAMP_TIME + 0.5) { + if (!initalReading) { + wheelInitialRad = swerve.getWheelDrivePositionsRadians(); + lastAngle = swerve.getPigeon2().getRotation2d(); + gyroDeltaRad = 0.0; + initalReading = true; + } + + Rotation2d currentAngle = swerve.getPigeon2().getRotation2d(); + gyroDeltaRad += Math.abs(currentAngle.minus(lastAngle).getRadians()); + lastAngle = currentAngle; + + double[] wheelCurrentRad = swerve.getWheelDrivePositionsRadians(); + double wheelDeltaRad = 0.0; + for (int i = 0; i < 4; i++) { + wheelDeltaRad += Math.abs(wheelCurrentRad[i] - wheelInitialRad[i]) / 4.0; + } + + double wheelRadius = (gyroDeltaRad * DRIVE_RADIUS_METERS) / wheelDeltaRad; + + SmartDashboard.putNumber("Radius Characterization/Radius (m)", wheelRadius); + SmartDashboard.putNumber("Radius Characterization/Radius (in.)", Units.metersToInches(wheelRadius)); + SmartDashboard.putNumber("Radius Characterization/Gyro Delta (rad)", gyroDeltaRad); + SmartDashboard.putNumber("Radius Characterization/Wheel Delta (rad)", wheelDeltaRad); + } + } + + @Override + public void end(boolean interrupted) { + swerve.setControl(swerve.getFieldCentricSwerveRequest() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(0)); + + double[] wheelCurrentRad = swerve.getWheelDrivePositionsRadians(); + + double wheelDeltaRad = 0.0; + for (int i = 0; i < 4; i++) { + wheelDeltaRad += (Math.abs(wheelCurrentRad[i] - wheelInitialRad[i]) / 4.0); + } + + double wheelRadiusMeters = (gyroDeltaRad * DRIVE_RADIUS_METERS) / wheelDeltaRad; + double wheelRadiusInches = Units.metersToInches(wheelRadiusMeters); + + System.out.println("********** Wheel Radius Characterization Results **********"); + System.out.printf("\tWheel Delta: %.9f radians%n", wheelDeltaRad); + System.out.printf("\tGyro Delta: %.9f radians%n", gyroDeltaRad); + System.out.printf("\tWheel Radius: %.9f meters / %.9f inches%n", wheelRadiusMeters, wheelRadiusInches); + } + + @Override + public boolean isFinished() { + return false; // driver cancels manually + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java index 9422a6b4..fdc7e67e 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveXMode.java @@ -12,7 +12,15 @@ import com.ctre.phoenix6.swerve.SwerveRequest; public class SwerveXMode extends Command { + private CommandSwerveDrivetrain swerve; + public SwerveXMode() { + swerve = CommandSwerveDrivetrain.getInstance(); + addRequirements(swerve); + } + + @Override + public void initialize() { SwerveRequest request = new SwerveRequest.SwerveDriveBrake(); CommandSwerveDrivetrain.getInstance().setControl(request); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java index b68b37f7..f2936cd6 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java @@ -70,10 +70,10 @@ public SwerveDrivePIDToPose(Supplier targetPose) { new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) - .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION))); + .setSetpointFilter(new AMotionProfile(Settings.Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, Settings.Swerve.Constraints.MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED))); - maxVelocity = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_VELOCITY; - maxAcceleration = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ACCELERATION; + maxVelocity = Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S; + maxAcceleration = Settings.Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED; isMotionProfiled = true; translationSetpoint = getNewTranslationSetpointGenerator(); @@ -142,7 +142,7 @@ private boolean isAlignedY() { } private boolean isAlignedTheta() { - return Math.abs(targetPose.get().getRotation().minus(swerve.getPose().getRotation()).getRadians()) < thetaTolerance.doubleValue(); + return Math.abs(targetPose.get().getRotation().minus(swerve.getPose().getRotation()).getDegrees()) < thetaTolerance.doubleValue(); } private boolean isAligned() { @@ -164,6 +164,8 @@ public void execute() { SmartDashboard.putNumber("Alignment/Target y", targetPose.get().getY()); SmartDashboard.putNumber("Alignment/Target angle", targetPose.get().getRotation().getDegrees()); + SmartDashboard.putNumber("Alignment/Error of Angle Controller)", controller.getError().omegaRadiansPerSecond); + SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative X (m per s)", controller.getOutput().vxMetersPerSecond); SmartDashboard.putNumber("Alignment/Target Velocity Robot Relative Y (m per s)", controller.getOutput().vyMetersPerSecond); SmartDashboard.putNumber("Alignment/Target Angular Velocity (rad per s)", controller.getOutput().omegaRadiansPerSecond); diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java b/src/main/java/com/stuypulse/robot/commands/turret/SeedTurret.java similarity index 71% rename from src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java rename to src/main/java/com/stuypulse/robot/commands/turret/SeedTurret.java index 8833a4a2..ed961ec3 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSeed.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/SeedTurret.java @@ -5,15 +5,16 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import edu.wpi.first.wpilibj2.command.InstantCommand; -public class TurretSeed extends InstantCommand { - private Turret turret; +public class SeedTurret extends InstantCommand { - public TurretSeed() { - turret = Turret.getInstance(); + private final Turret turret; + + public SeedTurret() { + this.turret = Turret.getInstance(); addRequirements(turret); } diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java index 7f63376b..3ba3bbcf 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretAnalog.java @@ -7,8 +7,8 @@ import com.stuypulse.stuylib.input.Gamepad; -import com.stuypulse.robot.subsystems.turret.Turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java index a576df14..2381eadb 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretFerry.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretFerry extends TurretSetState { public TurretFerry() { - super(TurretState.FERRYING); + super(TurretState.FERRY); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java index f0997013..37039cb2 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretIdle.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretIdle extends TurretSetState { public TurretIdle() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java index 20297313..0fb3fb67 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretLeftCorner.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretLeftCorner extends TurretSetState { public TurretLeftCorner() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java index 98b07bf3..5786fd60 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretRightCorner.java @@ -5,7 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretRightCorner extends TurretSetState { public TurretRightCorner() { diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java index 0fc2099d..479fa504 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretSetState.java @@ -5,8 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; import edu.wpi.first.wpilibj2.command.InstantCommand; diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java b/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java index 29dcf0ad..04ace480 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/turret/TurretShoot.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.turret; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; public class TurretShoot extends TurretSetState { public TurretShoot() { - super(TurretState.SHOOTING); + super(TurretState.SHOOT); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java b/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java new file mode 100644 index 00000000..cea47dca --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/turret/ZeroTurret.java @@ -0,0 +1,27 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.turret; + +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ZeroTurret extends InstantCommand { + + private final Turret turret; + + public ZeroTurret() { + this.turret = Turret.getInstance(); + + addRequirements(turret); + } + + @Override + public void initialize() { + turret.zeroEncoders(); + turret.seedTurret(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/turret/TurretHub.java b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java similarity index 60% rename from src/main/java/com/stuypulse/robot/commands/turret/TurretHub.java rename to src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java index 18a09bc2..071e6c3e 100644 --- a/src/main/java/com/stuypulse/robot/commands/turret/TurretHub.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/ResetLimelightIMU.java @@ -3,12 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.commands.turret; +package com.stuypulse.robot.commands.vision; -import com.stuypulse.robot.subsystems.turret.Turret.TurretState; +import com.stuypulse.robot.constants.Settings; -public class TurretHub extends TurretSetState { - public TurretHub() { - super(TurretState.HUB); +public class ResetLimelightIMU extends SetIMUMode { + public ResetLimelightIMU() { + super(Settings.Vision.RESET_IMU_INDEX); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java new file mode 100644 index 00000000..39172070 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUAssistValue.java @@ -0,0 +1,24 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.vision; + +import com.stuypulse.robot.subsystems.vision.LimelightVision; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SetIMUAssistValue extends InstantCommand { + private LimelightVision vision; + private double assistValue; + + public SetIMUAssistValue(double assistValue) { + vision = LimelightVision.getInstance(); + } + + @Override + public void initialize() { + vision.setIMUAssistValue(assistValue); + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java new file mode 100644 index 00000000..80ec87bd --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetIMUMode.java @@ -0,0 +1,31 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.commands.vision; + +import com.stuypulse.robot.subsystems.vision.LimelightVision; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class SetIMUMode extends InstantCommand { + private final LimelightVision vision; + private final int index; + + public SetIMUMode(int IMUIndex) { + vision = LimelightVision.getInstance(); + index = IMUIndex; + } + + @Override + public void initialize() { + super.initialize(); + vision.setIMUMode(index); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java b/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java index 4e5d8412..9cec1728 100644 --- a/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java +++ b/src/main/java/com/stuypulse/robot/commands/vision/SetMegaTagMode.java @@ -23,4 +23,10 @@ public void initialize() { super.initialize(); LimelightVision.getInstance().setMegaTagMode(megaTagMode); } + + + @Override + public boolean runsWhenDisabled() { + return true; + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java index b264f114..56400b19 100644 --- a/src/main/java/com/stuypulse/robot/constants/Cameras.java +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -7,13 +7,32 @@ import com.stuypulse.stuylib.network.SmartBoolean; +import com.stuypulse.robot.RobotContainer; + import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; /** This interface stores information about each camera. */ public interface Cameras { - public Camera[] LimelightCameras = new Camera[] { - + public Camera[] LimelightCameras = { + new Camera("limelight-right", //.381, .2325, .2069592 + new Pose3d(Units.inchesToMeters( -9.164), Units.inchesToMeters(14.962), Units.inchesToMeters(8.118), //TODO: check this offset + // new Pose3d(0.0,0.0,0.0, + new Rotation3d(Units.degreesToRadians(180), Units.degreesToRadians(31.08), Units.degreesToRadians(-80))), + RobotContainer.EnabledSubsystems.RIGHT_LIMELIGHT), + new Camera("limelight-left", + new Pose3d(Units.inchesToMeters(2.490), Units.inchesToMeters(-14.8620), Units.inchesToMeters(7.920157), + // new Pose3d(0.0,0.0,0.0, + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(15.0), Units.degreesToRadians(71.0))), //289 + RobotContainer.EnabledSubsystems.LEFT_LIMELIGHT), + new Camera("limelight-back", //11.0845u + new Pose3d(Units.inchesToMeters(-10.768823), Units.inchesToMeters(-12.717519), Units.inchesToMeters(8.331714), + new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(31.0), Units.degreesToRadians(185.0))), + RobotContainer.EnabledSubsystems.BACK_LIMELIGHT)// 31 // 26.965 + //THIS ROTATION IS NOT EXACTLY 180 DEGREES... + }; public static class Camera { diff --git a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java index a997419c..c1ef99da 100644 --- a/src/main/java/com/stuypulse/robot/constants/DriverConstants.java +++ b/src/main/java/com/stuypulse/robot/constants/DriverConstants.java @@ -5,8 +5,6 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.stuypulse.stuylib.network.SmartNumber; - public interface DriverConstants { public interface Driver { @@ -14,16 +12,14 @@ public interface Driver { double BUZZ_INTENSITY = 1.0; public interface Drive { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Drive/Deadband", 0.05); - - SmartNumber RC = new SmartNumber("Driver Settings/Drive/RC", 0.05); - SmartNumber POWER = new SmartNumber("Driver Settings/Drive/Power", 2); + double DEADBAND = 0.05; + double RC = 0.05; + int POWER = 3; } public interface Turn { - SmartNumber DEADBAND = new SmartNumber("Driver Settings/Turn/Deadband", 0.05); - - SmartNumber RC = new SmartNumber("Driver Settings/Turn/RC", 0.05); - SmartNumber POWER = new SmartNumber("Driver Settings/Turn/Power", 2); + double DEADBAND = 0.05; + double RC = 0.05; + int POWER = 3; } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index 8d45aed9..5c20aae3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -30,52 +30,74 @@ public interface Field { double WIDTH = Units.inchesToMeters(317.000); double LENGTH = Units.inchesToMeters(651.200); - public static final double trenchXTolerance = Units.inchesToMeters(50); + public static final double TRENCH_HOOD_TOLERANCE = Units.inchesToMeters(20); // Alliance relative hub center coordinates public static final Pose2d hubCenter = new Pose2d(Units.inchesToMeters(182.11), WIDTH / 2.0, new Rotation2d()); - public static final Pose3d hubCenter3d = new Pose3d(hubCenter.getX(), hubCenter.getY(), Units.inchesToMeters(72), Rotation3d.kZero); + public static final Pose2d hubFarRightCorner = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 - Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); + public static final Pose2d hubFarLeftCorner = new Pose2d(Units.inchesToMeters(205.6), WIDTH / 2.0 + Units.inchesToMeters(47 / 2.0), Rotation2d.kZero); + + public static final double HUB_RADIUS = Units.inchesToMeters(41.7 / 2); + + public static final double OPPONENT_ZONE_X = LENGTH - Units.inchesToMeters(158.6); + + public static final double hubTolerance = Units.inchesToMeters(283); public static Pose2d getHubPose() { return hubCenter; } // Alliance relative tower center coordinates - public final Pose2d towerCenter = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47), new Rotation2d()); + public final Pose2d towerFarCenter = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47), new Rotation2d()); + public final Pose2d towerFarRight = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47-23.5), new Rotation2d()); + public final Pose2d towerFarLeft = new Pose2d(Units.inchesToMeters(42.0), Units.inchesToMeters(147.47+23.5), new Rotation2d()); public final double barDisplacement = Units.inchesToMeters(11.38); public final double DISTANCE_TO_RUNGS = Units.inchesToMeters(20); // placeholder value, how far away in terms of y-cord from the rung - public static boolean closerToTop(){ - return (CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.towerCenter.getY()); + public static boolean closerToTop() { + return CommandSwerveDrivetrain.getInstance().getPose().getY() >= Field.towerFarCenter.getY(); } - // 1.0 meters from driverstation wall and field wall - public final Pose2d leftFerryZone = new Pose2d(1.0, WIDTH - 1.0, new Rotation2d()); - public final Pose2d rightFerryZone = new Pose2d(1.0 + Units.feetToMeters(6), 1.0 + Units.feetToMeters(3), new Rotation2d()); //TODO: GET ACTUAL POS + public final Pose2d leftFerryZone = new Pose2d( + Units.inchesToMeters(31.5), + WIDTH - Units.inchesToMeters(34.5), + new Rotation2d() + ); + + public final Pose2d rightFerryZone = new Pose2d( + Units.inchesToMeters(20.75), + Units.inchesToMeters(76), + new Rotation2d() + ); public static Pose2d getFerryZonePose(Translation2d robot) { - if (robot.getDistance(leftFerryZone.getTranslation()) > robot.getDistance(rightFerryZone.getTranslation())) { - return rightFerryZone; - } else { + double fieldMidY = WIDTH / 2.0; + + if (robot.getY() > fieldMidY) { return leftFerryZone; + } else { + return rightFerryZone; } } /*** TRENCH COORDINATES ***/ - public interface NearLeftTrench { + public interface AllianceLeftTrench { public static final Pose2d leftEdge = new Pose2d(Units.inchesToMeters(182.11), WIDTH, new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(Units.inchesToMeters(182.11), WIDTH - Units.inchesToMeters(50.59), new Rotation2d()); } - public interface NearRightTrench { + public interface AllianceRightTrench { public static final Pose2d leftEdge = new Pose2d(Units.inchesToMeters(182.11), Units.inchesToMeters(50.59), new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(Units.inchesToMeters(182.11), Units.inchesToMeters(0), new Rotation2d()); } - public interface FarLeftTrench { + + // OPPONENT SIDE, BUT LEFT/RIGHT RELATIVE TO YOUR ALLIANCE POV + public interface OpponentLeftTrench { public static final Pose2d leftEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), WIDTH, new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), WIDTH - Units.inchesToMeters(50.59), new Rotation2d()); } - public interface FarRightTrench { + // OPPONENT SIDE, BUT LEFT/RIGHT RELATIVE TO YOUR ALLIANCE POV + public interface OpponentRightTrench { public static final Pose2d leftEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), Units.inchesToMeters(50.59), new Rotation2d()); public static final Pose2d rightEdge = new Pose2d(LENGTH - Units.inchesToMeters(182.11), Units.inchesToMeters(0), new Rotation2d()); } diff --git a/src/main/java/com/stuypulse/robot/constants/Gains.java b/src/main/java/com/stuypulse/robot/constants/Gains.java index d172cde0..b9d3b06c 100644 --- a/src/main/java/com/stuypulse/robot/constants/Gains.java +++ b/src/main/java/com/stuypulse/robot/constants/Gains.java @@ -5,64 +5,77 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import com.stuypulse.stuylib.network.SmartNumber; + import com.pathplanner.lib.config.PIDConstants; public class Gains { - - public interface ClimberHopper { - double kP = 1.0; - double kI = 0.0; - double kD = 0.20; - - double kS = 0.0; - double kV = 0.123; - double kA = 0.0; - } - public interface HoodedShooter { + public interface Superstructure { public interface Shooter { - double kP = 0.45; - double kI = 0.0; - double kD = 0.0; + SmartNumber kP = new SmartNumber("Superstructure/Shooter/Gains/kP", 0.45); + SmartNumber kI = new SmartNumber("Superstructure/Shooter/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Superstructure/Shooter/Gains/kD", 0.0); - double kS = 0.0; - double kV = 0.123; - double kA = 0.0; + SmartNumber kS = new SmartNumber("Superstructure/Shooter/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("Superstructure/Shooter/Gains/kV", 0.123); + SmartNumber kA = new SmartNumber("Superstructure/Shooter/Gains/kA", 0.0); } public interface Hood { - double kP = 300.0; + double kP = 250.0; double kI = 0.0; - double kD = 0.0; + double kD = 2.0; - double kS = 0.0; + double kS = 0.25; double kV = 0.0; double kA = 0.0; } - + + public interface Turret { + public interface slot0 { + double kP = 200.0; + double kI = 0.0; + double kD = 0.0; + + double kS = 0.4775; + double kV = 0.0; + double kA = 0.0; + } + + public interface slot1 { + SmartNumber kP = new SmartNumber("Superstructure/Turret/Gains/kP", 20.0); + SmartNumber kI = new SmartNumber("Superstructure/Turret/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Superstructure/Turret/Gains/kD", 0.0); + + SmartNumber kS = new SmartNumber("Superstructure/Turret/Gains/kS", 0.4775); + SmartNumber kV = new SmartNumber("Superstructure/Turret/Gains/kV", 0.0); + SmartNumber kA = new SmartNumber("Superstructure/Turret/Gains/kA", 0.0); + } + } } public interface Spindexer { - double kP = 1.20; + double kP = 1.2; double kI = 0.0; - double kD = 0.0; - - double kS = 0.019444; + double kD = 10.0; + + double kS = 0.25; + double kV = 1.2; double kA = 0.010876; - double kV = 0.38546; } public interface Intake { public interface Pivot { - double kP = 1.0; - double kI = 0.0; - double kD = 0.0; - - double kS = 0.0; - double kV = 0.0; - double kA = 0.0; - - double kG = 0.0; + SmartNumber kP = new SmartNumber("Intake/Pivot/Gains/kP", 100.0); + SmartNumber kI = new SmartNumber("Intake/Pivot/Gains/kI", 0.0); + SmartNumber kD = new SmartNumber("Intake/Pivot/Gains/kD", 10.0); + + SmartNumber kS = new SmartNumber("Intake/Pivot/Gains/kS", 0.0); + SmartNumber kV = new SmartNumber("Intake/Pivot/Gains/kV", 0.12); + SmartNumber kA = new SmartNumber("Intake/Pivot/Gains/kA", 0.0); + + double kG = 0.5; } } @@ -70,31 +83,21 @@ public interface Handoff { double kP = 0.00015508; double kI = 0.0; double kD = 0.0; - - double kS = 0.1728; - double kA = 0.0028428; - double kV = 0.11725; - } - - public interface Turret { - double kP = 1300.0; - double kI = 0.0; - double kD = 140.0; - double kS = 0.179; - double kV = 0.0; - double kA = 0.0; + double kS = 0.1728; + double kV = 0.12; + double kA = 0.00284; } public interface Swerve { public interface Drive { - double kP = 0.1; + double kP = 0.10224; double kI = 0.0; double kD = 0.0; - double kS = 0.0; - double kV = 0.124; - double kA = 0.0; + double kS = 0.19896; + double kV = 0.12528; + double kA = 0.011662; } public interface Turn { @@ -108,24 +111,8 @@ public interface Turn { } public interface Alignment { - public interface Rotation { - double kp = 112.3; - double ki = 0.0; - double kd = 2.3758; - double ks = 0.31395; - double kv = 0.10969; - double ka = 0.026589; - } - - double kP = 0.0; - double kI = 0.0; - double kD = 0.0; - double akP = 0.0; - double akI = 0.0; - double akD = 0.0; - - PIDConstants XY = new PIDConstants(3.0, 0.0, 0.2); - PIDConstants THETA = new PIDConstants(3.0, 0.0, 0.2); + PIDConstants XY = new PIDConstants(2.2, 0, 0.0); + PIDConstants THETA = new PIDConstants(3, 0, 0.0); } } } diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index 61bf55e2..941fab59 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -5,7 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import com.stuypulse.stuylib.network.SmartNumber; + import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; @@ -16,11 +19,14 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.Slot2Configs; +import com.ctre.phoenix6.configs.SlotConfigs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.configs.VoltageConfigs; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.GainSchedBehaviorValue; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -29,139 +35,6 @@ public interface Motors { - public interface ClimberHopper { - public final TalonFXConfig MOTOR = new TalonFXConfig() - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake) - .withCurrentLimitAmps(50.0) - .withSupplyCurrentLimitAmps(50.0) - .withRampRate(Settings.ClimberHopper.RAMP_RATE); - - public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP) - .withReverseSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); - } - - public interface Handoff { - public final TalonFXConfig HANDOFF = new TalonFXConfig() - .withCurrentLimitAmps(80.0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) - .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) - .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); - } - - public interface HoodedShooter { - public interface Hood { - public final TalonFXConfig HOOD = new TalonFXConfig() - .withCurrentLimitAmps(80.0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Hood.kP, Gains.HoodedShooter.Hood.kI, Gains.HoodedShooter.Hood.kD, 0) - .withFFConstants(Gains.HoodedShooter.Hood.kS, Gains.HoodedShooter.Hood.kV, Gains.HoodedShooter.Hood.kA, 0) - .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO); - - public final Slot0Configs SLOT_0 = new Slot0Configs() - .withKP(Gains.HoodedShooter.Hood.kP) - .withKI(Gains.HoodedShooter.Hood.kI) - .withKD(Gains.HoodedShooter.Hood.kD) - .withKS(Gains.HoodedShooter.Hood.kS) - .withKV(Gains.HoodedShooter.Hood.kV) - .withKA(Gains.HoodedShooter.Hood.kA) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations()) - .withReverseSoftLimitThreshold(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); - - public final CANcoderConfiguration HOOD_ENCODER = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1.0) - .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations())); - } - - public interface Shooter { - public final TalonFXConfig SHOOTER = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, - Gains.HoodedShooter.Shooter.kD, 0) - .withFFConstants(Gains.HoodedShooter.Shooter.kS, Gains.HoodedShooter.Shooter.kV, - Gains.HoodedShooter.Shooter.kA, 0) - .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); - } - } - - public interface Intake { - public final TalonFXConfig ROLLER = new TalonFXConfig() - .withCurrentLimitAmps(40.0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.Clockwise_Positive); - - public final TalonFXConfig PIVOT = new TalonFXConfig() - .withCurrentLimitAmps(40.0) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Intake.Pivot.kP, Gains.Intake.Pivot.kI, Gains.Intake.Pivot.kD, 0) - .withFFConstants(Gains.Intake.Pivot.kS, Gains.Intake.Pivot.kV, Gains.Intake.Pivot.kA, Gains.Intake.Pivot.kG, 0) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); - } - - public interface Spindexer { - public final TalonFXConfig SPINDEXER = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) - .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) - .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); - } - - public interface Turret { - public final TalonFXConfig TURRET = new TalonFXConfig() - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Turret.kP, 0.0, Gains.Turret.kD, 0) - .withFFConstants(Gains.Turret.kS, 0.0, 0.0, 0) - .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH); - - public final Slot0Configs SLOT_0 = new Slot0Configs() - .withKP(Gains.Turret.kP) - .withKI(Gains.Turret.kI) - .withKD(Gains.Turret.kD) - .withKS(Gains.Turret.kS) - .withKV(Gains.Turret.kV) - .withKA(Gains.Turret.kA) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - public final CANCoderConfig ENCODER_17T = new CANCoderConfig() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1.0); - - public final CANCoderConfig ENCODER_18T = new CANCoderConfig() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1.0); - - public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS) - .withReverseSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); - } - public static class CANCoderConfig { private final CANcoderConfiguration configuration = new CANcoderConfiguration(); private final MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs(); @@ -215,6 +88,16 @@ public static class TalonFXConfig { private final CurrentLimitsConfigs currentLimitsConfigs = new CurrentLimitsConfigs(); private final FeedbackConfigs feedbackConfigs = new FeedbackConfigs(); private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); + private final SoftwareLimitSwitchConfigs softwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs(); + private final ClosedLoopGeneralConfigs closedLoopGeneralConfigs = new ClosedLoopGeneralConfigs(); + private final VoltageConfigs voltageConfigs = new VoltageConfigs(); + + private final double[] lastKP = new double[3]; + private final double[] lastKI = new double[3]; + private final double[] lastKD = new double[3]; + private final double[] lastKS = new double[3]; + private final double[] lastKV = new double[3]; + private final double[] lastKA = new double[3]; public void configure(TalonFX motor) { TalonFXConfiguration defaultConfig = new TalonFXConfiguration(); @@ -223,6 +106,68 @@ public void configure(TalonFX motor) { motor.getConfigurator().apply(configuration); } + public TalonFXConfiguration getConfiguration() { + return this.configuration; + } + + // SMARTNUMBER TUNABLE GAINS FOR TALONFX MOTOR CONTROLLERS + // Note that this should ONLY be used during testing/debugging and not for competition code + public void updateGainsConfig(TalonFX motor, int slot, SmartNumber kP, SmartNumber kI, SmartNumber kD, SmartNumber kS, SmartNumber kV, SmartNumber kA) { + if (slot != 0 && slot != 1 && slot != 2) { + return; + } + + double currentKP = kP.getAsDouble(); + double currentKI = kI.getAsDouble(); + double currentKD = kD.getAsDouble(); + double currentKS = kS.getAsDouble(); + double currentKV = kV.getAsDouble(); + double currentKA = kA.getAsDouble(); + + boolean changed = + currentKP != lastKP[slot] || + currentKI != lastKI[slot] || + currentKD != lastKD[slot] || + currentKS != lastKS[slot] || + currentKV != lastKV[slot] || + currentKA != lastKA[slot]; + + if (!changed) { + return; + } + + SlotConfigs gainConfig = new SlotConfigs() + .withKP(currentKP) + .withKI(currentKI) + .withKD(currentKD) + .withKS(currentKS) + .withKV(currentKV) + .withKA(currentKA); + + gainConfig.SlotNumber = slot; + + motor.getConfigurator().apply(gainConfig); + + lastKP[slot] = currentKP; + lastKI[slot] = currentKI; + lastKD[slot] = currentKD; + lastKS[slot] = currentKS; + lastKV[slot] = currentKV; + lastKA[slot] = currentKA; + + switch (slot) { + case 0: + motor.getConfigurator().refresh(this.getConfiguration().Slot0); + break; + case 1: + motor.getConfigurator().refresh(this.getConfiguration().Slot1); + break; + case 2: + motor.getConfigurator().refresh(this.getConfiguration().Slot2); + break; + } + } + // SLOT CONFIGS public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) { @@ -280,6 +225,25 @@ public TalonFXConfig withFFConstants(double kS, double kV, double kA, double kG, return this; } + public TalonFXConfig withStaticFeedforwardSign(StaticFeedforwardSignValue staticFeedforwardSign, int slot) { + switch (slot) { + case 0: + slot0Configs.StaticFeedforwardSign = staticFeedforwardSign; + configuration.withSlot0(slot0Configs); + break; + case 1: + slot1Configs.StaticFeedforwardSign = staticFeedforwardSign; + configuration.withSlot1(slot1Configs); + break; + case 2: + slot2Configs.StaticFeedforwardSign = staticFeedforwardSign; + configuration.withSlot2(slot2Configs); + break; + } + + return this; + } + public TalonFXConfig withGravityType(GravityTypeValue gravityType) { slot0Configs.GravityType = gravityType; slot1Configs.GravityType = gravityType; @@ -292,6 +256,31 @@ public TalonFXConfig withGravityType(GravityTypeValue gravityType) { return this; } + public TalonFXConfig withGainSchedBehavior(GainSchedBehaviorValue value, double threshold, int slot) { + closedLoopGeneralConfigs.GainSchedErrorThreshold = threshold; + configuration.withClosedLoopGeneral(closedLoopGeneralConfigs); + + switch(slot) { + case 0: { + slot0Configs.GainSchedBehavior = value; + configuration.withSlot0(slot0Configs); + } + break; + case 1: { + slot1Configs.GainSchedBehavior = value; + configuration.withSlot1(slot1Configs); + } + break; + case 2: { + slot2Configs.GainSchedBehavior = value; + configuration.withSlot2(slot2Configs); + } + break; + } + + return this; + } + // MOTOR OUTPUT CONFIGS public TalonFXConfig withInvertedValue(InvertedValue invertedValue) { @@ -310,6 +299,14 @@ public TalonFXConfig withNeutralMode(NeutralModeValue neutralMode) { return this; } + public TalonFXConfig withVelocityTimeFilter(double filterInSeconds) { + feedbackConfigs.withVelocityFilterTimeConstant(filterInSeconds); + + configuration.withFeedback(feedbackConfigs); + + return this; + } + // RAMP RATE CONFIGS public TalonFXConfig withRampRate(double rampRate) { @@ -329,15 +326,6 @@ public TalonFXConfig withRampRate(double rampRate) { // CURRENT LIMIT CONFIGS - public TalonFXConfig withCurrentLimitAmps(double currentLimitAmps) { - currentLimitsConfigs.StatorCurrentLimit = currentLimitAmps; - currentLimitsConfigs.StatorCurrentLimitEnable = true; - - configuration.withCurrentLimits(currentLimitsConfigs); - - return this; - } - public TalonFXConfig withLowerLimitSupplyCurrent(double currentLowerLimitAmps) { currentLimitsConfigs.SupplyCurrentLowerLimit = currentLowerLimitAmps; currentLimitsConfigs.StatorCurrentLimitEnable = true; @@ -356,8 +344,24 @@ public TalonFXConfig withSupplyCurrentLimitAmps(double currentLimitAmps) { return this; } - public TalonFXConfig withCurrentLimitEnable(boolean enabled) { + public TalonFXConfig withSupplyCurrentLimitEnabled(boolean enabled) { currentLimitsConfigs.SupplyCurrentLimitEnable = enabled; + + configuration.withCurrentLimits(currentLimitsConfigs); + + return this; + } + + public TalonFXConfig withStatorCurrentLimitAmps(double currentLimitAmps) { + currentLimitsConfigs.StatorCurrentLimit = currentLimitAmps; + currentLimitsConfigs.StatorCurrentLimitEnable = true; + + configuration.withCurrentLimits(currentLimitsConfigs); + + return this; + } + + public TalonFXConfig withStatorCurrentLimitEnabled(boolean enabled) { currentLimitsConfigs.StatorCurrentLimitEnable = enabled; configuration.withCurrentLimits(currentLimitsConfigs); @@ -365,6 +369,30 @@ public TalonFXConfig withCurrentLimitEnable(boolean enabled) { return this; } + // VOLTAGE LIMIT CONFIGS + + public TalonFXConfig withVoltageLimits(double peakForwardVoltage, double peakReverseVoltage) { + voltageConfigs.PeakForwardVoltage = peakForwardVoltage; + voltageConfigs.PeakReverseVoltage = peakReverseVoltage; + + configuration.withVoltage(voltageConfigs); + + return this; + } + + // SOFTWARE LIMIT CONFIGS + + public TalonFXConfig withSoftLimits(boolean forwardEnable, boolean reverseEnable, double forwardThreshold, double reverseThreshold) { + softwareLimitSwitchConfigs.ForwardSoftLimitEnable = forwardEnable; + softwareLimitSwitchConfigs.ReverseSoftLimitEnable = reverseEnable; + softwareLimitSwitchConfigs.ForwardSoftLimitThreshold = forwardThreshold; + softwareLimitSwitchConfigs.ReverseSoftLimitThreshold = reverseThreshold; + + configuration.withSoftwareLimitSwitch(softwareLimitSwitchConfigs); + + return this; + } + // MOTION MAGIC CONFIGS public TalonFXConfig withMotionProfile(double maxVelocity, double maxAcceleration) { diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 0d8e164a..0874d3e3 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -9,7 +9,6 @@ /** This file contains the different ports of motors, solenoids and sensors */ public interface Ports { - // TODO: Get bus name public CANBus RIO = new CANBus("rio"); public CANBus CANIVORE = new CANBus("CANIVORE"); @@ -25,7 +24,7 @@ public interface Handoff { int HANDOFF = 43; } - public interface HoodedShooter { + public interface Superstructure { public interface Hood { int MOTOR = 45; int THROUGHBORE_ENCODER = 44; @@ -35,6 +34,12 @@ public interface Shooter { int MOTOR_LEAD = 47; int MOTOR_FOLLOW = 46; } + + public interface Turret { + int MOTOR = 40; + int ENCODER17T = 42; + int ENCODER18T = 41; + } } public interface Intake { @@ -47,10 +52,4 @@ public interface Spindexer { int SPINDEXER_LEAD_MOTOR = 30; int SPINDEXER_FOLLOW_MOTOR = 31; } - - public interface Turret { - int MOTOR = 40; - int ENCODER17T = 42; - int ENCODER18T = 41; - } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 0caa5baa..dc08e433 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,8 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.constants; -import com.ctre.phoenix6.CANBus; -import com.pathplanner.lib.path.PathConstraints; +import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; import edu.wpi.first.math.VecBuilder; @@ -17,14 +16,15 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; +import com.ctre.phoenix6.CANBus; +import com.pathplanner.lib.path.PathConstraints; + /*- * File containing constants and tunable settings for every subsystem on the robot. * * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ -import com.ctre.phoenix6.CANBus; -import com.pathplanner.lib.path.PathConstraints; public interface Settings { public final double DT = 0.020; @@ -32,42 +32,6 @@ public interface Settings { public final boolean DEBUG_MODE = true; public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); - public interface ClimberHopper { - public interface Constants { - public final double GEAR_RATIO = 45.0; - - public final double MIN_HEIGHT_METERS = 0.0; - public final double MAX_HEIGHT_METERS = 1.0; - - public final double MASS_KG = 1.0; - - public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); - public final double POSITION_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP; - public final double VELOCITY_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP / 60.0; - - public final double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2.0 / Math.PI; - } - - public final double CLIMBER_UP_HEIGHT_METERS = Constants.MAX_HEIGHT_METERS; - public final double CLIMBER_DOWN_HEIGHT_METERS = 0.2; - public final double HOPPER_DOWN_HEIGHT_METERS = Constants.MIN_HEIGHT_METERS; - public final double HOPPER_UP_HEIGHT_METERS = 0.5; - - public final double STALL = 10.0; - - public final double ROTATIONS_AT_BOTTOM = 0.0; - - public final double DEBOUNCE = 0.25; - - public final double GYRO_TOLERANCE = 0.0; - - public final double HEIGHT_TOLERANCE_METERS = 0.02; - - public final double RAMP_RATE = 50.0; - - public final double MOTOR_VOLTAGE = 3.5; - } - public interface Handoff { public final double GEAR_RATIO = 3.0 / 1.0; @@ -75,95 +39,126 @@ public interface Handoff { double HANDOFF_MAX = 4800.0; double HANDOFF_REVERSE = -500.0; double RPM_TOLERANCE = 200.0; + double RPM_SOTM_TOLERANCE = 700.0; SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); + + SmartNumber HANDOFF_STALL_CURRENT = new SmartNumber("Handoff/Stall Current Limit for Reverse", 30.0); } public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); - Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(0.0); + Rotation2d PIVOT_DEPLOY_ANGLE = Rotation2d.fromDegrees(0.0); + + Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(5.0); - Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); - double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; - double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; + Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(88.0); + Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(-10.0); - Rotation2d PIVOT_ANGLE_OFFSET = new Rotation2d(); - Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(190); - Rotation2d PIVOT_MIN_ANGLE = Rotation2d.fromDegrees(80); + Rotation2d THRESHOLD_TO_START_ROLLERS = Rotation2d.fromDegrees(10.0); - Rotation2d PIVOT_MAX_VEL = Rotation2d.fromDegrees(300.0); - Rotation2d PIVOT_MAX_ACCEL = Rotation2d.fromDegrees(300.0); + Rotation2d ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE = Rotation2d.fromDegrees(15.0); + double HOMING_VOLTAGE = 3.0; + + double PUSHDOWN_VOLTAGE = 3.0; - double GEAR_RATIO = 48.0; + double GEAR_RATIO = 37.93; + + double STALL_CURRENT_LIMIT = 0; //TODO: set value + double STALL_DEBOUNCE = 1.0; //TODO: VERIFY } public interface Spindexer { - double FORWARD_SPEED = 6000.0; - double REVERSE_SPEED = -6000.0; + double FORWARD_SPEED = 4500.0; + double REVERSE_SPEED = -4500.0; double STOP_SPEED = 0.0; - double RPM_TOLERANCE = 400.0; + double RPM_TOLERANCE = 800.0; + double STALL_CURRENT_LIMIT = 40.0; // random number as of 3/9 - public interface Constants { - double GEAR_RATIO = 8.0 / 1.0; - } + + /* CONSTANTS */ + double GEAR_RATIO = 8.0 / 1.0; } - public interface HoodedShooter { - public final double SHOOTER_TOLERANCE_RPM = 50.0; - public final double HOOD_TOLERANCE_DEG = 0.5; - - public interface RPMs { - public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); - public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); - public final double REVERSE = 0.0; - public final double HUB_RPM = 0.0; - public final double LEFT_CORNER_RPM = 0.0; - public final double RIGHT_CORNER_RPM = 0.0; - } - public interface Angles { - public final SmartNumber SHOOT_ANGLE = new SmartNumber("HoodedShooter/Shoot State Target Angle (deg)", 15.0); - public final SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); - - public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); - public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); - public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12.0); - public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); - public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); - } + public interface Superstructure { + public final double SHOOTER_TOLERANCE_RPM = 100.0; + public final Rotation2d HOOD_TOLERANCE = Rotation2d.fromDegrees(0.5); + + public final double SHOOTER_SOTM_TOLERANCE_RPM = 350.0; + public final Rotation2d HOOD_SOTM_TOLERANCE = Rotation2d.fromDegrees(3.0); public interface AngleInterpolation { - public final double[][] distanceAngleInterpolationValues = { - {1.30, Units.degreesToRadians(16.5)}, - {1.43, Units.degreesToRadians(21.0)}, - {2.15, Units.degreesToRadians(23.23)}, - {2.864967, Units.degreesToRadians(25.460189)}, - {3.65, Units.degreesToRadians(28.0)}, - {4.43, Units.degreesToRadians(30.65)}, - {5.32, Units.degreesToRadians(33.5)} + double[][] distanceAngleInterpolationValues = { + {1.22, Units.degreesToRadians(22.5)}, //BLAY-APPROVED (ALMOST AGAINST HUB), LOCKED IN + {2.15, Units.degreesToRadians(27)}, //BLAY-APPROVED + {3.38, Units.degreesToRadians(37)}, //BLAY-APPROVED + {4.43, Units.degreesToRadians(39)}, //BLAY-APPROVED + {5.66, Units.degreesToRadians(39)} //KEVIN-APPROVED }; } - public interface RPMInterpolation { - public final double[][] distanceRPMInterpolationValues = { - {1.30, 3000.0}, - {1.43, 3000.0}, - {2.15, 3050.0}, - {2.864967, 3215.271125}, - {3.65, 3400.0}, - {4.43, 3650.0}, - {5.32, 3950.0} + public interface RPMInterpolation{ + double[][] distanceRPMInterpolationValues = { + {1.22, 2700.0}, //BLAY-APPROVED, LOCKED IN + {2.15, 2930.0}, //BLAY-APPROVED + {3.38, 3200}, //BLAY-APPROVED + {4.43, 3550.0}, //BLAY-APPROVED + {5.66, 3900.0} //KEVIN-APPROVED + }; + } + + public interface TOFInterpolation{ + double[][] distanceTOFInterpolationValues = { + {1.22, 0.965}, // seconds + // {2.15, }, + {3.38, 1.32}, + {4.43, 1.125}, + {5.66, 1.29} }; } public interface FerryRPMInterpolation { - public final double[][] distanceRPMInterpolationValues = { - {3.79, 3450.0} + double[][] ferryDistanceRPMInterpolation = { + {5.16, 3500.0}, + {6.94, 3700.0}, + {7.87, 4000.0}, + {9.77, 4500.0}, + {10.694, 4795.0}, //STARTING FROM HERE THE DATA IS UNRELIABLE!!! + {11.516, 5000.0}, + {12.416, 5295.0}, + {13.316, 5500.0}, + {14.216, 5795.0}, + {15.148, 6000.0}, + {16.54, 6300} //FIELD LENGTH + }; + } + + public interface FerryTOFInterpolation { + double [][] FerryTOFInterpolationInterpolation = { + // {5.16, }, + // {6.94, }, + // {7.87, }, + // {9.77, }, }; } public interface Shooter { + public final double GEAR_RATIO = 1.0; public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); + + public interface RPM { + public final SmartBoolean OVERRIDEN = new SmartBoolean("Superstructure/Shooter/RPM Override enabled?", false); + public final SmartNumber OVERRIDE_VALUE = new SmartNumber("Superstructure/Shooter/RPM Override value", 0.0); + + public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target RPM", 3500.0); + public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target RPM", 2000.0); + + public final double REVERSE = 0.0; + public final double KB = 2720.0; + public final double LEFT_CORNER = 3850.0; + public final double RIGHT_CORNER = 3850.0; + } } public interface Hood { @@ -180,101 +175,134 @@ public interface Hood { * uniquely maps to the hood’s 0–33° mechanical range without any ambiguity. * */ - public final double GEAR_RATIO = 17024.0 / 135.0; + public final double GEAR_RATIO = 1064.0 / 9.0; public final double ENCODER_TO_MECH = 32.0 / 3.0; + public final double HOOD_HOMING_VOLTAGE = 2.0; - public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); - } - } - public interface ShootOnTheFly { - public final int MAX_ITERATIONS = 5; - public final double TIME_TOLERANCE = 0.01; - public final SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/SOTM/Update Delay", 0.00); - } + public final Rotation2d ENCODER_OFFSET = Rotation2d.fromRotations(0.795); - public interface Swerve { - public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; - public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; + public final Rotation2d FORWARD_SOFT_LIMIT = Rotation2d.fromDegrees(39.0); + public final Rotation2d REVERSE_SOFT_LIMIT = Rotation2d.fromDegrees(20.0); + public final Rotation2d MIN_FROM_HORIZON = Rotation2d.fromDegrees(7.0); + public final Rotation2d MAX_FROM_HORIZON = Rotation2d.fromDegrees(40.0); - public interface Alignment { - public interface Constraints { - public final double DEFAULT_MAX_VELOCITY = 4.3; - public final double DEFAULT_MAX_ACCELERATION = 15.0; - public final double DEFAULT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(400.0); - public final double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900.0); - } + public final double STALL_CURRENT_LIMIT = 20.0; + public final double STALL_DEBOUNCE = 0.5; - public interface Targets {} + public interface Angles { + public final SmartBoolean OVERRIDEN = new SmartBoolean("Superstructure/Hood/Angle Override enabled?", false); + public final SmartNumber OVERRIDE_VALUE_DEG = new SmartNumber("Superstructure/Hood/Angle Override value", 0.0); - public interface Tolerances { - public final double X_TOLERANCE = Units.inchesToMeters(2.0); - public final double Y_TOLERANCE = Units.inchesToMeters(2.0); - public final double THETA_TOLERANCE_DEG = 2.0; + public final SmartNumber SHOOT = new SmartNumber("InterpolationTesting/Shoot State Target Angle (deg)", 20.0); + public final SmartNumber FERRY = new SmartNumber("InterpolationTesting/Ferry State Target Angle (deg)", 20.0); - public final Pose2d POSE_TOLERANCE = new Pose2d( - Units.inchesToMeters(2.0), - Units.inchesToMeters(2.0), - Rotation2d.fromDegrees(2.0)); + public final Rotation2d MIN = Rotation2d.fromDegrees(20.0); + public final Rotation2d MAX = Rotation2d.fromDegrees(40.0); - public final double MAX_VELOCITY_WHEN_ALIGNED = 0.15; + public final Rotation2d STOW = Rotation2d.fromDegrees(21.0); + public final Rotation2d KB = Rotation2d.fromDegrees(22.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(38.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(38.0); + } + } - public final double ALIGNMENT_DEBOUNCE = 0.15; + public interface Turret { + public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); + public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); + public final Rotation2d TOLERANCE = Rotation2d.fromDegrees(2.0); + public final Rotation2d SOTM_TOLERANCE = Rotation2d.fromDegrees(5.0); + + public final Rotation2d KB = Rotation2d.fromDegrees(0.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(-233.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(53.0); + + double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; + double WRAP_DEBOUNCE = 0.5; + Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); + Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); + + /* CONSTANTS */ + public final double RANGE_LEFT = -360.0; + public final double RANGE_RIGHT = 85.0; + + public final Rotation2d GAIN_SWITCHING_THRESHOLD = Rotation2d.fromDegrees(30); + + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(-4.0), Units.inchesToMeters(8.0), Rotation2d.kZero); + public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); + + public final double GEAR_RATIO_MOTOR_TO_MECH = (60.0 / 9.0) * (95.0 / 12.0); //1425.0 / 36.0; + + public interface BigGear { + public final int TEETH = 95; + } + + public interface Encoder17t { + public final int TEETH = 17; + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.716); + } + + public interface Encoder18t { + public final int TEETH = 18; + public final Rotation2d OFFSET = Rotation2d.fromRotations(-0.559); + } + + public interface SoftwareLimit { + public final double FORWARD_MAX_ROTATIONS = 210.0 / 360.0; + public final double BACKWARDS_MAX_ROTATIONS = -210.0 / 360.0; } } + public interface SOTM { + public final int MAX_ITERATIONS = 10; + double TIME_TOLERANCE = 1e-5; + SmartNumber UPDATE_DELAY = new SmartNumber("Superstructure/SOTM/update delay", 0.23); + } + } + + + public interface Swerve { + public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; + public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; + public interface Constraints { - public final double MAX_VELOCITY_M_PER_S = 4.3; + public final double MAX_VELOCITY_M_PER_S = 4.93; + public final double MAX_VELOCITY_SOTM_M_PER_S = 1.00; + public final double MAX_VELOCITY_FOTM_M_PER_S = 2.00; + + public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(300.0); + public final double MAX_ANGULAR_VEL_SOTM_RAD_PER_S = Units.degreesToRadians(75.0); + public final double MAX_ANGULAR_VEL_FOTM_RAD_PER_S = Units.degreesToRadians(150.0); + public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; - public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400.0); - public final double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(900.0); + public final double MAX_ACCEL_M_PER_S_SQUARED_SOTM = 15.0; + public final double MAX_ACCEL_M_PER_S_SQUARED_FOTM = 15.0; + public final double MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED = Units.degreesToRadians(900.0); public final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( MAX_VELOCITY_M_PER_S, MAX_ACCEL_M_PER_S_SQUARED, MAX_ANGULAR_VEL_RAD_PER_S, - MAX_ANGULAR_ACCEL_RAD_PER_S); + MAX_ANGULAR_ACCEL_RAD_PER_S_SQUARED); } - } - - public interface Turret { - public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); - public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); - public final double TOLERANCE_DEG = 2.0; - public final Rotation2d HUB = Rotation2d.fromDegrees(0.0); - public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); - public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); - - double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; - Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); - Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); - - public interface Constants { - public final double RANGE = 210.0; - - public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(0.0), Units.inchesToMeters(0.0), Rotation2d.kZero); - public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); + public interface Alignment { - public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; + public interface Targets {} - public interface BigGear { - public final int TEETH = 95; - } + public interface Tolerances { + public final double X_TOLERANCE = Units.inchesToMeters(2.0); + public final double Y_TOLERANCE = Units.inchesToMeters(2.0); + public final double THETA_TOLERANCE_DEG = 3.0; - public interface Encoder17t { - public final int TEETH = 17; - public final Rotation2d OFFSET = new Rotation2d(); - } + public final Pose2d POSE_TOLERANCE = new Pose2d( + Units.inchesToMeters(2.0), + Units.inchesToMeters(2.0), + Rotation2d.fromDegrees(2.0)); - public interface Encoder18t { - public final int TEETH = 18; - public final Rotation2d OFFSET = new Rotation2d(); - } + public final double MAX_VELOCITY_WHEN_ALIGNED = 0.15; - public interface SoftwareLimit { - public final double FORWARD_MAX_ROTATIONS = 210.0 / 360.0; - public final double BACKWARDS_MAX_ROTATIONS = -210.0 / 360.0; + public final double ALIGNMENT_DEBOUNCE = 0.15; } } } @@ -282,5 +310,7 @@ public interface SoftwareLimit { public interface Vision { public final Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); public final Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694.0); + public final int RESET_IMU_INDEX = 1; + public final int INTERNAL_EXTERNAL_ASSIST_INDEX = 4; } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/.gitkeep b/src/main/java/com/stuypulse/robot/subsystems/.gitkeep deleted file mode 100644 index e69de29b..00000000 diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java deleted file mode 100644 index 35197ae1..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ /dev/null @@ -1,78 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.climberhopper; - -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import java.util.Optional; - -public abstract class ClimberHopper extends SubsystemBase { - private static final ClimberHopper instance; - - static { - if (Robot.isReal()) { - instance = new ClimberHopperImpl(); - } else { - instance = new ClimberHopperSim(); - } - } - - public static ClimberHopper getInstance() { - return instance; - } - - public enum ClimberHopperState { - CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP_HEIGHT_METERS), - CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN_HEIGHT_METERS), - // HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), - HOPPER_DOWN(Settings.ClimberHopper.HOPPER_DOWN_HEIGHT_METERS); - - private double targetHeight; - - private ClimberHopperState(double targetHeight) { - this.targetHeight = targetHeight; - } - - public double getTargetHeight() { - return targetHeight; - } - - } - - private ClimberHopperState state; - - protected ClimberHopper() { - this.state = ClimberHopperState.CLIMBER_UP; - } - - public ClimberHopperState getState() { - return state; - } - - public void setState(ClimberHopperState state) { - this.state = state; - } - - public abstract boolean getStalling(); - public abstract double getCurrentHeight(); - public abstract boolean atTargetHeight(); - - /** - * Resets the encoder postition to the upper hardstop - */ - public abstract void resetPostionUpper(); - - public abstract void setVoltageOverride(Optional voltage); - - @Override - public void periodic() { - SmartDashboard.putString("ClimberHopper/State", getState().toString()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java deleted file mode 100644 index fed7859b..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ /dev/null @@ -1,110 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.climberhopper; -import com.stuypulse.stuylib.streams.booleans.BStream; -import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; - -import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import java.util.Optional; - - -public class ClimberHopperImpl extends ClimberHopper { - private final TalonFX motor; - private final VoltageOut controller; - - private final BStream stalling; - private double voltage; - - private Optional voltageOverride; - - public ClimberHopperImpl() { - super(); - - motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER, Ports.CANIVORE); - Motors.ClimberHopper.MOTOR.configure(motor); - motor.getConfigurator().apply(Motors.ClimberHopper.SOFT_LIMITS); - - motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); - stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) - .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); - - // TODO: initialize voltage to default voltage and pass to controller initialization below - controller = new VoltageOut(0) - .withEnableFOC(true); - - voltageOverride = Optional.empty(); - } - - @Override - public boolean getStalling() { - return stalling.getAsBoolean(); - } - - @Override - public double getCurrentHeight() { - return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.Constants.POSITION_CONVERSION_FACTOR; - } - - private boolean isWithinTolerance(double toleranceMeters) { - return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; - } - - @Override - public boolean atTargetHeight() { - return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); - } - - @Override - public void setVoltageOverride(Optional voltage) { - this.voltageOverride = voltage; - } - - public void resetPostionUpper() { - motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP); - } - - @Override - public void periodic() { - super.periodic(); - - if (voltageOverride.isPresent()) { - voltage = voltageOverride.get(); - } else { - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; - } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; - } - } else { - voltage = 0; - } - } - - if (EnabledSubsystems.CLIMBER_HOPPER.get()) { - motor.setControl(controller.withOutput(voltage)); - } else { - motor.stopMotor(); - } - - SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - - if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("ClimberHopper/Current Height", getCurrentHeight()); - SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); - SmartDashboard.putNumber("ClimberHopper/Applied Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("ClimberHopper/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); - } - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java deleted file mode 100644 index 9ae11e3e..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ /dev/null @@ -1,100 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.climberhopper; - - -import com.stuypulse.robot.constants.Settings; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import java.util.Optional; - - -public class ClimberHopperSim extends ClimberHopper { - - private final ElevatorSim sim; - private final ClimberHopperVisualizer visualizer; - private double voltage; - - private Optional voltageOverride; - - public ClimberHopperSim() { - visualizer = ClimberHopperVisualizer.getInstance(); - - sim = new ElevatorSim( - DCMotor.getKrakenX60(1), - Settings.ClimberHopper.Constants.GEAR_RATIO, - Settings.ClimberHopper.Constants.MASS_KG, - Settings.ClimberHopper.Constants.DRUM_RADIUS_METERS, - Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS, - Settings.ClimberHopper.Constants.MAX_HEIGHT_METERS, - false, - Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS - ); - - voltageOverride = Optional.empty(); - } - - public boolean getStalling() { - return sim.getCurrentDrawAmps() > Settings.ClimberHopper.STALL; - } - - @Override - public double getCurrentHeight() { - return sim.getPositionMeters(); - } - - private boolean isWithinTolerance(double toleranceMeters) { - return Math.abs(getState().getTargetHeight() - getCurrentHeight()) < toleranceMeters; - } - - @Override - public boolean atTargetHeight() { - return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); - } - - @Override - public void setVoltageOverride(Optional voltage) { - this.voltageOverride = voltage; - } - - @Override - public void resetPostionUpper() { - // No encoder reset for sim - } - - - @Override - public void periodic() { - super.periodic(); - - if (voltageOverride.isPresent()) { - voltage = voltageOverride.get(); - } else { - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; - } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; - } - } else { - voltage = 0; - } - } - - sim.setInputVoltage(voltage); - - SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); - SmartDashboard.putNumber("ClimberHopper/Current", sim.getCurrentDrawAmps()); - SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); - SmartDashboard.putNumber("ClimberHopper/Height", getCurrentHeight()); - visualizer.update(getCurrentHeight()); - - sim.update(0.02); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java deleted file mode 100644 index cc890cf5..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperVisualizer.java +++ /dev/null @@ -1,69 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.climberhopper; - -import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; -import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; - -public class ClimberHopperVisualizer { - private static final ClimberHopperVisualizer instance; - - static { - instance = new ClimberHopperVisualizer(); - } - - public static ClimberHopperVisualizer getInstance() { - return instance; - } - - private final Mechanism2d canvas; - private final MechanismRoot2d hopper; - private final MechanismRoot2d bot; - private final MechanismRoot2d climber; - - ClimberHopperVisualizer() { - canvas = new Mechanism2d(2.5, 2.5); - hopper = canvas.getRoot("Hopper", 0.8, 1); - bot = canvas.getRoot("Bot", 0.8, 0.5); - climber = canvas.getRoot("Climber", 0.4, 0.5); - - hopper.append(new MechanismLigament2d( - "Hopper", - 0.5, - 0, - 0.5, - new Color8Bit(Color.kYellow) - ) - ); - - bot.append(new MechanismLigament2d( - "Bot", - 1, - 0, - 1, - new Color8Bit(Color.kRed) - )); - - climber.append(new MechanismLigament2d( - "Climber", - 0.2, - 90, - 0.5, - new Color8Bit(Color.kOrange) - )); - } - - public void update(double height) { - - hopper.setPosition(0.8, height + 0.5); - - SmartDashboard.putData("Visualizers/ClimberHopper", canvas); - } -} diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java index 469febbb..a14aa844 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/Handoff.java @@ -5,7 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; +import com.stuypulse.robot.subsystems.superstructure.Superstructure.SuperstructureState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -18,7 +21,11 @@ public abstract class Handoff extends SubsystemBase { private HandoffState state; static { - instance = new HandoffImpl(); + if (Robot.isReal()) { + instance = new HandoffImpl(); + } else { + instance = new HandoffSim(); + } } public static Handoff getInstance() { @@ -52,14 +59,19 @@ public void setState(HandoffState state) { } public boolean atTolerance() { - double diff = Math.abs(getTargetRPM() - getCurrentRPM()); - return diff < Settings.Handoff.RPM_TOLERANCE; + double error = Math.abs(getTargetRPM() - getCurrentRPM()); + SuperstructureState superstructureState = Superstructure.getInstance().getState(); + if (superstructureState == SuperstructureState.SOTM || superstructureState == SuperstructureState.FOTM) { + return error < Settings.Handoff.RPM_TOLERANCE; + } + return error < Settings.Handoff.RPM_TOLERANCE; } public abstract double getCurrentRPM(); public abstract SysIdRoutine getSysIdRoutine(); public abstract void setVoltageOverride(Optional voltage); + public abstract boolean isHandoffStalling(); @Override public void periodic() { @@ -69,5 +81,6 @@ public void periodic() { SmartDashboard.putNumber("Handoff/Target RPM", getTargetRPM()); SmartDashboard.putNumber("Handoff/Current RPM", getCurrentRPM()); + SmartDashboard.putBoolean("Handoff/At Tolerance?", atTolerance()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index 2bb053ad..e80f97cb 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -5,7 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.handoff; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -16,63 +20,96 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import java.util.Optional; public class HandoffImpl extends Handoff { + private final Motors.TalonFXConfig handoffConfig; + private final TalonFX motor; private final VelocityVoltage controller; private Optional voltageOverride; + private BStream isStalling; public HandoffImpl() { + handoffConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(80.0) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) + .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) + + .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); + motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); - Motors.Handoff.HANDOFF.configure(motor); + handoffConfig.configure(motor); - controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE) - .withEnableFOC(true); + controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); voltageOverride = Optional.empty(); - } - public double getCurrentRPM() { - return motor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + isStalling = BStream.create(() -> motor.getSupplyCurrent().getValueAsDouble() > Settings.Handoff.HANDOFF_STALL_CURRENT.getAsDouble()) + .filtered(new BDebounce.Both(0.5)); } @Override - public void setVoltageOverride(Optional voltage) { - this.voltageOverride = voltage; + public boolean isHandoffStalling() { + return isStalling.get(); } + public double getCurrentRPM() { + return motor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Handoff.GEAR_RATIO; + } + @Override public void periodic() { super.periodic(); - - if (EnabledSubsystems.HANDOFF.get()) { - if (getState() == HandoffState.STOP) { - motor.stopMotor(); - } else if (voltageOverride.isPresent()) { + + if (EnabledSubsystems.HANDOFF.get() && getState() != HandoffState.STOP) { + if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); - } else { - motor.setControl(controller.withVelocity(getTargetRPM() / 60.0)); + } + // else if (isHandoffStalling()) { //TODO: debug logic + // setState(HandoffState.REVERSE); + // motor.setControl(controller.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); + + // } + else { + motor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); } + } else { + motor.stopMotor(); } - + if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Handoff/Current (amps)", motor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Handoff/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Handoff/Stator Current", motor.getStatorCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Handoff (amps)", motor.getSupplyCurrent().getValueAsDouble()); } } - + + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } + @Override public SysIdRoutine getSysIdRoutine() { return SysId.getRoutine( - 2, - 8, - "Handoff", - voltage -> setVoltageOverride(Optional.of(voltage)), - () -> motor.getPosition().getValueAsDouble(), - () -> motor.getVelocity().getValueAsDouble(), - () -> motor.getMotorVoltage().getValueAsDouble(), - getInstance()); + 2, + 8, + "Handoff", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> motor.getPosition().getValueAsDouble(), + () -> motor.getVelocity().getValueAsDouble(), + () -> motor.getMotorVoltage().getValueAsDouble(), + getInstance()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java new file mode 100644 index 00000000..d5d3c71f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffSim.java @@ -0,0 +1,110 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.handoff; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.LinearQuadraticRegulator; +import edu.wpi.first.math.estimator.KalmanFilter; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.LinearSystemLoop; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import java.util.Optional; + +public class HandoffSim extends Handoff { + + private LinearSystemSim sim; + private final LinearSystemLoop controller; + + private Optional voltageOverride; + + public HandoffSim() { + LinearSystem flywheel = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX44(1), 0.01, 1.0); + + sim = new LinearSystemSim<>(flywheel); + + LinearQuadraticRegulator lqr = new LinearQuadraticRegulator( + flywheel, + VecBuilder.fill(8.0), + VecBuilder.fill(12.0), + Settings.DT); + + KalmanFilter kalmanFilter = new KalmanFilter<>( + Nat.N1(), + Nat.N1(), + flywheel, + VecBuilder.fill(3.0), + VecBuilder.fill(0.01), + Settings.DT); + + controller = new LinearSystemLoop<>(flywheel, lqr, kalmanFilter, 12.0, Settings.DT); + + voltageOverride = Optional.empty(); + } + + @Override + public double getCurrentRPM() { + return sim.getOutput(0) * 60.0 / (2.0 * Math.PI); // convert to RPM + } + + @Override + public void periodic() { + super.periodic(); + + controller.setNextR(VecBuilder.fill(getTargetRPM() * 2.0 * Math.PI / 60.0)); + controller.correct(VecBuilder.fill(sim.getOutput(0))); + controller.predict(Settings.DT); + + if (EnabledSubsystems.SHOOTER.get()) { + if (voltageOverride.isPresent()) { + sim.setInput(voltageOverride.get()); + SmartDashboard.putNumber("Handoff/Input Voltage", voltageOverride.get()); + } else { + SmartDashboard.putNumber("Handoff/Input Voltage", controller.getU(0)); + sim.setInput(controller.getU(0)); + } + } else { + sim.setInput(0); + SmartDashboard.putNumber("Handoff/Input Voltage", 0.0); + } + + sim.update(Settings.DT); + } + + @Override + public void setVoltageOverride(Optional volts) { + voltageOverride = volts; + } + + @Override + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Handoff", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> 0.0, + () -> 0.0, + () -> sim.getInput(0), + getInstance()); + } + + @Override + public boolean isHandoffStalling() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isHandoffStalling'"); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java deleted file mode 100644 index b1945ee9..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ /dev/null @@ -1,124 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.hoodedshooter; - -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood; -import com.stuypulse.robot.subsystems.hoodedshooter.hood.Hood.HoodState; -import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter; -import com.stuypulse.robot.subsystems.hoodedshooter.shooter.Shooter.ShooterState; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class HoodedShooter extends SubsystemBase { - - private static final HoodedShooter instance; - - static { - instance = new HoodedShooter(); - } - - public static HoodedShooter getInstance(){ - return instance; - } - - private HoodedShooterState state; - - private final Hood hood; - private final Shooter shooter; - - public HoodedShooter() { - state = HoodedShooterState.INTERPOLATION; - hood = Hood.getInstance(); - shooter = Shooter.getInstance(); - } - - public enum HoodedShooterState { - STOW(HoodState.STOW, ShooterState.SHOOT), - SHOOT(HoodState.SHOOT, ShooterState.SHOOT), - FERRY(HoodState.FERRY, ShooterState.FERRY), - REVERSE(HoodState.SHOOT, ShooterState.REVERSE), - HUB(HoodState.HUB, ShooterState.HUB), - LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER), - RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER), - INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION); - - private HoodState hoodState; - private ShooterState shooterState; - - private HoodedShooterState(HoodState hoodState, ShooterState shooterState) { - this.hoodState = hoodState; - this.shooterState = shooterState; - } - - public HoodState getHoodState() { - return hoodState; - } - - public ShooterState getShooterState() { - return shooterState; - } - } - - public void setState(HoodedShooterState state){ - this.state = state; - hood.setState(state.getHoodState()); - shooter.setState(state.getShooterState()); - } - - public HoodedShooterState getState(){ - return state; - } - - public boolean bothAtTolerance() { - return isShooterAtTolerance() && isHoodAtTolerance(); - } - - public boolean isShooterAtTolerance() { - return shooter.atTolerance(); - } - - public boolean isHoodAtTolerance() { - return hood.atTolerance(); - } - - public double getTargetRPM() { - return shooter.getTargetRPM(); - } - - public Rotation2d getTargetAngle() { - return hood.getTargetAngle(); - } - - public double getShooterRPM() { - return shooter.getShooterRPM(); - } - - public Rotation2d getHoodAngle() { - return hood.getHoodAngle(); - } - - @Override - public void periodic() { - SmartDashboard.putString("HoodedShooter/State", state.name()); - SmartDashboard.putString("States/HoodedShooter", state.name()); - - SmartDashboard.putNumber("HoodedShooter/Target RPM", shooter.getTargetRPM()); - SmartDashboard.putNumber("HoodedShooter/Target Angle", hood.getTargetAngle().getDegrees()); - - SmartDashboard.putNumber("HoodedShooter/Current RPM", getShooterRPM()); - SmartDashboard.putNumber("HoodedShooter/Current Angle", getHoodAngle().getDegrees()); - - SmartDashboard.putNumber("HoodedShooter/Angle Error (Deg)", hood.getTargetAngle().getDegrees() - getHoodAngle().getDegrees()); - - SmartDashboard.putBoolean("HoodedShooter/Shooter At Tolerance?", isShooterAtTolerance()); - SmartDashboard.putBoolean("HoodedShooter/Hood At Tolerance?", isHoodAtTolerance()); - - SmartDashboard.putNumber("InterpolationTesting/Hood Angle", getHoodAngle().getDegrees()); - SmartDashboard.putNumber("InterpolationTesting/Shooter RPM", getShooterRPM()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java deleted file mode 100644 index 9a637d5a..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java +++ /dev/null @@ -1,81 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.hoodedshooter.hood; - -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -public abstract class Hood extends SubsystemBase{ - private static final Hood instance; - - private HoodState state; - - static { - instance = new HoodImpl(); - } - - public static Hood getInstance(){ - return instance; - } - - public enum HoodState { - STOW, - FERRY, - SHOOT, - HUB, - LEFT_CORNER, - RIGHT_CORNER, - INTERPOLATION, - IDLE; - } - - public Hood() { - state = HoodState.STOW; - } - - public HoodState getState(){ - return state; - } - - public void setState(HoodState state){ - this.state = state; - } - - public Rotation2d getTargetAngle() { - return switch(state) { - case STOW -> Settings.HoodedShooter.Angles.MIN_ANGLE; - case FERRY -> Rotation2d.fromDegrees(30); - case SHOOT -> Rotation2d.fromDegrees(Settings.HoodedShooter.Angles.SHOOT_ANGLE.get()); - case HUB -> Settings.HoodedShooter.Angles.HUB_ANGLE; - case LEFT_CORNER -> Settings.HoodedShooter.Angles.LEFT_CORNER_ANGLE; - case RIGHT_CORNER -> Settings.HoodedShooter.Angles.RIGHT_CORNER_ANGLE; - case INTERPOLATION -> HoodAngleCalculator.interpolateHoodAngle().get(); - case IDLE -> getHoodAngle(); - }; - } - - public boolean atTolerance() { - return Math.abs(getHoodAngle().getDegrees() - getTargetAngle().getDegrees()) < Settings.HoodedShooter.HOOD_TOLERANCE_DEG; - } - - public abstract Rotation2d getHoodAngle(); - - public abstract SysIdRoutine getHoodSysIdRoutine(); - - @Override - public void periodic() { - SmartDashboard.putString("HoodedShooter/Hood/State", state.name()); - SmartDashboard.putString("States/Hood", state.name()); - - SmartDashboard.putNumber("HoodedShooter/Hood/Target Angle", getTargetAngle().getDegrees()); - SmartDashboard.putNumber("HoodedShooter/Hood/Current Angle", getHoodAngle().getDegrees()); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java deleted file mode 100644 index 6b8c814e..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ /dev/null @@ -1,111 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.hoodedshooter.hood; - -import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.SysId; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import java.util.Optional; - -public class HoodImpl extends Hood { - private final TalonFX hoodMotor; - private final CANcoder hoodEncoder; - - private final PositionVoltage controller; - - private Optional voltageOverride; - - private boolean hasUsedAbsoluteEncoder; - - public HoodImpl() { - hoodMotor = new TalonFX(Ports.HoodedShooter.Hood.MOTOR, Ports.RIO); - hoodEncoder = new CANcoder(Ports.HoodedShooter.Hood.THROUGHBORE_ENCODER, Ports.RIO); - - Motors.HoodedShooter.Hood.HOOD.configure(hoodMotor); - - hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SLOT_0); - hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SOFT_LIMITS); - - hoodEncoder.getConfigurator().apply(Motors.HoodedShooter.Hood.HOOD_ENCODER); - - controller = new PositionVoltage(getTargetAngle().getRotations()) - .withEnableFOC(true); - - voltageOverride = Optional.empty(); - } - - @Override - public Rotation2d getHoodAngle() { - return Rotation2d.fromRotations(hoodMotor.getPosition().getValueAsDouble()); - } - - @Override - public void periodic() { - super.periodic(); - - if (!hasUsedAbsoluteEncoder) { - /* - * Example: - * Let's say the hood rotates 0.1 rotations. Then, the encoder has rotated 0.1 * 10.67 rotations - * To convert the encoder reading to the mechanism position, we simply do (0.1 * 10.67) / 10.67 = 0.1 - */ - hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); - hasUsedAbsoluteEncoder = true; - } - - if (EnabledSubsystems.HOOD.get()) { - if (voltageOverride.isPresent()) { - hoodMotor.setVoltage(voltageOverride.get()); - } else { - hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); - } - } else { - hoodMotor.stopMotor(); - } - - if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); - - SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); - - SmartDashboard.putNumber("HoodedShooter/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); - SmartDashboard.putBoolean("HoodedShooter/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); - - SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("InterpolationTesting/Hood Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); - } - } - - public void setVoltageOverride(Optional voltageOverride) { - this.voltageOverride = voltageOverride; - } - - @Override - public SysIdRoutine getHoodSysIdRoutine() { - return SysId.getRoutine( - .45, - 2, - "Hood", - voltage -> setVoltageOverride(Optional.of(voltage)), - () -> hoodMotor.getPosition().getValueAsDouble(), - () -> hoodMotor.getVelocity().getValueAsDouble(), - () -> hoodMotor.getMotorVoltage().getValueAsDouble(), - getInstance() - ); - } - -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java deleted file mode 100644 index 2a88eff0..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java +++ /dev/null @@ -1,113 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.hoodedshooter.shooter; - -import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.SysId; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.MotorAlignmentValue; -import java.util.Optional; - -public class ShooterImpl extends Shooter { - private final TalonFX shooterLeader; - private final TalonFX shooterFollower; - - private final VelocityVoltage shooterController; - private final Follower follower; - - private Optional voltageOverride; - - public ShooterImpl() { - shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); - shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW, Ports.RIO); - - shooterController = new VelocityVoltage(getTargetRPM() / 60.0) - .withEnableFOC(true); - follower = new Follower(Ports.HoodedShooter.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); - - Motors.HoodedShooter.Shooter.SHOOTER.configure(shooterLeader); - Motors.HoodedShooter.Shooter.SHOOTER.configure(shooterFollower); - - shooterFollower.setControl(follower); - - voltageOverride = Optional.empty(); - } - - @Override - public double getShooterRPM() { - return getLeaderRPM(); - } - - public double getLeaderRPM() { - return shooterLeader.getVelocity().getValueAsDouble() * 60.0; - } - - public double getFollowerRPM() { - return shooterFollower.getVelocity().getValueAsDouble() * 60.0; - } - - @Override - public void periodic() { - super.periodic(); - - if (EnabledSubsystems.SHOOTER.get()) { - if (getState() == ShooterState.STOP) { - shooterLeader.stopMotor(); - shooterFollower.stopMotor(); - } else if (voltageOverride.isPresent()) { - shooterLeader.setVoltage(voltageOverride.get()); - shooterFollower.setControl(follower); - } else { - shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0)); - shooterFollower.setControl(follower); - } - } else { - shooterLeader.stopMotor(); - shooterFollower.stopMotor(); - } - - if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); - - SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); - - SmartDashboard.putNumber("HoodedShooter/Shooter/Follower RPM", getFollowerRPM()); - - SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error", shooterLeader.getClosedLoopError().getValueAsDouble() * 60.0); - SmartDashboard.putNumber("InterpolationTesting/Shooter Applied Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("InterpolationTesting/Shooter Supply Current", shooterLeader.getSupplyCurrent().getValueAsDouble()); - } - } - - public void setVoltageOverride(Optional voltageOverride) { - this.voltageOverride = voltageOverride; - } - - @Override - public SysIdRoutine getShooterSysIdRoutine() { - return SysId.getRoutine( - 1, - 5, - "Shooter", - voltage -> setVoltageOverride(Optional.of(voltage)), - () -> shooterLeader.getPosition().getValueAsDouble(), - () -> shooterLeader.getVelocity().getValueAsDouble(), - () -> shooterLeader.getMotorVoltage().getValueAsDouble(), - getInstance() - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index bcde7954..9c0a6cad 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -5,6 +5,7 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,17 +18,30 @@ public abstract class Intake extends SubsystemBase { private static final Intake instance; + private PivotState pivotState; + private RollerState rollerState; + static { - instance = new IntakeImpl(); + if (Robot.isReal()) { + instance = new IntakeImpl(); + } else { + instance = new IntakeSim(); + } } public static Intake getInstance() { return instance; } + protected Intake() { + this.pivotState = PivotState.STOW; + this.rollerState = RollerState.STOP; + } + public enum PivotState { - DEPLOYED(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE), - STOWED(Settings.Intake.PIVOT_STOW_ANGLE); + DEPLOY(Settings.Intake.PIVOT_DEPLOY_ANGLE), + HOMING(Settings.Intake.PIVOT_DEPLOY_ANGLE), + STOW(Settings.Intake.PIVOT_STOW_ANGLE); private final Rotation2d targetAngle; @@ -56,14 +70,6 @@ public double getTargetDutyCycle() { } } - private PivotState pivotState; - private RollerState rollerState; - - protected Intake() { - this.pivotState = PivotState.STOWED; - this.rollerState = RollerState.STOP; - } - public PivotState getPivotState() { return pivotState; } @@ -85,9 +91,16 @@ public void setRollerState(RollerState state) { public abstract Rotation2d getPivotAngle(); public abstract void setPivotVoltageOverride(Optional voltage); public abstract SysIdRoutine getPivotSysIdRoutine(); + public abstract boolean pivotStalling(); + + public abstract void zeroPivotDeployed(); + public abstract void zeroPivotStowed(); @Override public void periodic() { + SmartDashboard.putString("States/Intake Pivot", getPivotState().toString()); + SmartDashboard.putString("States/Intake Roller", getRollerState().toString()); + SmartDashboard.putString("Intake/Pivot State", getPivotState().toString()); SmartDashboard.putString("Intake/Roller State", getRollerState().toString()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index b89ccd0a..ad2c2b4d 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -5,10 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.intake; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SettableNumber; import com.stuypulse.robot.util.SysId; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,78 +22,173 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import java.util.Optional; public class IntakeImpl extends Intake { + private final Motors.TalonFXConfig pivotConfig; + private final Motors.TalonFXConfig rollerConfig; + private final TalonFX pivot; private final TalonFX rollerLeader; private final TalonFX rollerFollower; - private final MotionMagicVoltage pivotController; private final DutyCycleOut rollerController; private final Follower follower; + private SettableNumber velLimit; + private SettableNumber accelLimit; + private Optional pivotVoltageOverride; + private BStream pivotStalling; + public IntakeImpl() { + pivotConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(60) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .withPIDConstants(Gains.Intake.Pivot.kP.get(), Gains.Intake.Pivot.kI.get(), Gains.Intake.Pivot.kD.get(), 0) + .withFFConstants(Gains.Intake.Pivot.kS.get(), Gains.Intake.Pivot.kV.get(), Gains.Intake.Pivot.kA.get(), + Gains.Intake.Pivot.kG, 0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign, 0) + .withGravityType(GravityTypeValue.Arm_Cosine) + + .withSensorToMechanismRatio(Settings.Intake.GEAR_RATIO); + + rollerConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + .withSupplyCurrentLimitAmps(45.0) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.50); + pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); - Motors.Intake.PIVOT.configure(pivot); + pivotConfig.configure(pivot); rollerLeader = new TalonFX(Ports.Intake.ROLLER_LEADER, Ports.RIO); - Motors.Intake.ROLLER.configure(rollerLeader); + rollerConfig.configure(rollerLeader); rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER, Ports.RIO); - Motors.Intake.ROLLER.configure(rollerFollower); + rollerConfig.configure(rollerFollower); - pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) - .withEnableFOC(true); - rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) - .withEnableFOC(true); - follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Opposed); + rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()).withEnableFOC(true); + follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Aligned); + + rollerFollower.setControl(follower); pivotVoltageOverride = Optional.empty(); + + pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); + + pivotStalling = BStream.create( + () -> Math.abs(pivot.getSupplyCurrent().getValueAsDouble()) > Settings.Intake.STALL_CURRENT_LIMIT) + .filtered(new BDebounce.Both(Settings.Intake.STALL_DEBOUNCE)); + } + + @Override + public boolean pivotStalling() { + return pivotStalling.get(); } @Override public boolean pivotAtTolerance() { - return Math.abs( - getPivotAngle().getRotations() - getPivotState().getTargetAngle().getRotations()) - < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); + double error = getPivotAngle().minus(getPivotState().getTargetAngle()).getRotations(); + return Math.abs(error) < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); } @Override public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } - + + @Override + public void zeroPivotStowed() { + pivot.setPosition(Settings.Intake.PIVOT_MAX_ANGLE.getRotations()); + } + + @Override + public void zeroPivotDeployed() { + pivot.setPosition(Settings.Intake.PIVOT_MIN_ANGLE.getRotations()); + } + @Override public void periodic() { super.periodic(); + PivotState pivotState = getPivotState(); + + pivotConfig.updateGainsConfig( + pivot, + 0, + Gains.Intake.Pivot.kP, + Gains.Intake.Pivot.kI, + Gains.Intake.Pivot.kD, + Gains.Intake.Pivot.kS, + Gains.Intake.Pivot.kV, + Gains.Intake.Pivot.kA + ); + if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { - pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); - rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); + // PIVOT + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.ANGLE_THRESHOLD_FOR_HOLDING_VOLTAGE.getDegrees()) { + pivot.setControl(new VoltageOut(-Settings.Intake.PUSHDOWN_VOLTAGE)); // applying 3 volts + } else if (pivotState == PivotState.HOMING) { + pivot.setControl(new VoltageOut(-Settings.Intake.HOMING_VOLTAGE)); + } + else { + pivot.setControl(new PositionVoltage(pivotState.getTargetAngle().getRotations())); + } + + // ROLLERS + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.getDegrees()) { + rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); + } else { + rollerLeader.stopMotor(); + } rollerFollower.setControl(follower); } + + if (pivotState == PivotState.HOMING && pivotStalling()) { + zeroPivotDeployed(); + } } else { pivot.stopMotor(); rollerLeader.stopMotor(); rollerFollower.stopMotor(); } - SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", - Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); - if (Settings.DEBUG_MODE) { // PIVOT + SmartDashboard.putBoolean("Intake/Voltage Override", pivotVoltageOverride.isPresent()); + + SmartDashboard.putNumber("Intake/Pivot Target Angle", getPivotState().getTargetAngle().getRotations()); + SmartDashboard.putNumber("Intake/Pivot Voltage (volts)", pivot.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Intake/Pivot Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Pivot Supply Current (amps)", pivot.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Pivot Stator Current (amps)", pivot.getStatorCurrent().getValueAsDouble()); + + // SmartDashboard.putNumber("Intake/Pivot Max Velocity Limit (deg/s)", velLimit.get()); Causing issues + // SmartDashboard.putNumber("Intake/Pivot Max Accel Limit (deg/s^2)", accelLimit.get()); Causing issues + + SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", + Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); + + SmartDashboard.putNumber("Intake/Pivot Closed Loop Error (deg)", pivot.getClosedLoopError().getValueAsDouble() * 360.0); // ROLLERS SmartDashboard.putNumber("Intake/Roller Leader Voltage (volts)", rollerLeader.getMotorVoltage().getValueAsDouble()); @@ -96,6 +196,14 @@ public void periodic() { SmartDashboard.putNumber("Intake/Roller Leader Current (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Intake/Roller Follower Current (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Intake/Pivot Temperature (C)", pivot.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Leader Temperature (C)", rollerLeader.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("Intake/Follower Temperature (C)", rollerFollower.getDeviceTemp().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Intake Pivot (amps)", pivot.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Leader (amps)", rollerLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Intake Roller Follower (amps)", rollerFollower.getSupplyCurrent().getValueAsDouble()); } } @@ -107,13 +215,13 @@ public void setPivotVoltageOverride(Optional voltage) { @Override public SysIdRoutine getPivotSysIdRoutine() { return SysId.getRoutine( - 2, - 6, - "Intake Pivot", - voltage -> setPivotVoltageOverride(Optional.of(voltage)), - () -> getPivotAngle().getRotations(), - () -> pivot.getVelocity().getValueAsDouble(), - () -> pivot.getMotorVoltage().getValueAsDouble(), - getInstance()); + 2, + 6, + "Intake Pivot", + voltage -> setPivotVoltageOverride(Optional.of(voltage)), + () -> getPivotAngle().getRotations(), + () -> pivot.getVelocity().getValueAsDouble(), + () -> pivot.getMotorVoltage().getValueAsDouble(), + getInstance()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java new file mode 100644 index 00000000..c41a6e6a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeSim.java @@ -0,0 +1,214 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.intake; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; + +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.controller.LinearQuadraticRegulator; +import edu.wpi.first.math.estimator.KalmanFilter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.LinearSystemLoop; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.LinearSystemSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import java.util.Optional; + +public class IntakeSim extends Intake { + + private static final double ARM_LENGTH_METERS = 0.4; + private static final double ARM_MASS_KG = 2.0; + private static final double MOI = SingleJointedArmSim.estimateMOI(ARM_LENGTH_METERS, ARM_MASS_KG); + + private final SingleJointedArmSim pivotSim; + private final LinearSystemLoop pivotController; + + private final LinearSystemSim rollerLeaderSim; + private final LinearSystemSim rollerFollowerSim; + private final LinearSystemLoop rollerLeaderController; + private final LinearSystemLoop rollerFollowerController; + + private Optional pivotVoltageOverride; + + public IntakeSim() { + LinearSystem pivotSystem = LinearSystemId.createSingleJointedArmSystem( + DCMotor.getKrakenX60(1), + MOI, + Settings.Intake.GEAR_RATIO + ); + + KalmanFilter kalmanFilter = new KalmanFilter<>( + Nat.N2(), + Nat.N2(), + pivotSystem, + VecBuilder.fill(3.0, 3.0), + VecBuilder.fill(0.01, 0.01), + Settings.DT + ); + + LinearQuadraticRegulator lqr = new LinearQuadraticRegulator<>( + pivotSystem, + VecBuilder.fill(0.00001, 100), + VecBuilder.fill(12), + Settings.DT + ); + + pivotController = new LinearSystemLoop<>(pivotSystem, lqr, kalmanFilter, 12.0, Settings.DT); + + pivotSim = new SingleJointedArmSim( + DCMotor.getKrakenX60(1), + Settings.Intake.GEAR_RATIO, + MOI, + ARM_LENGTH_METERS, + Settings.Intake.PIVOT_MIN_ANGLE.getRadians(), + Settings.Intake.PIVOT_MAX_ANGLE.getRadians(), + true, + Settings.Intake.PIVOT_MAX_ANGLE.getRadians() // start stowed + ); + + LinearSystem rollerSystem = LinearSystemId.createFlywheelSystem( + DCMotor.getKrakenX60(1), 0.01, 1.0 + ); + + rollerLeaderSim = new LinearSystemSim<>(rollerSystem); + rollerFollowerSim = new LinearSystemSim<>(rollerSystem); + + LinearQuadraticRegulator rollerLQR = new LinearQuadraticRegulator<>( + rollerSystem, + VecBuilder.fill(8.0), + VecBuilder.fill(12.0), + Settings.DT + ); + + KalmanFilter rollerLeaderKalman = new KalmanFilter<>( + Nat.N1(), Nat.N1(), + rollerSystem, + VecBuilder.fill(3.0), + VecBuilder.fill(0.01), + Settings.DT + ); + + KalmanFilter rollerFollowerKalman = new KalmanFilter<>( + Nat.N1(), Nat.N1(), + rollerSystem, + VecBuilder.fill(3.0), + VecBuilder.fill(0.01), + Settings.DT + ); + + rollerLeaderController = new LinearSystemLoop<>(rollerSystem, rollerLQR, rollerLeaderKalman, 12.0, Settings.DT); + rollerFollowerController = new LinearSystemLoop<>(rollerSystem, rollerLQR, rollerFollowerKalman, 12.0, Settings.DT); + + pivotVoltageOverride = Optional.empty(); + } + + @Override + public Rotation2d getPivotAngle() { + return Rotation2d.fromRadians(pivotSim.getAngleRads()); + } + + @Override + public boolean pivotAtTolerance() { + double error = getPivotAngle().minus(getPivotState().getTargetAngle()).getRotations(); + return Math.abs(error) < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); + } + + @Override + public boolean pivotStalling() { + return false; + } + + @Override + public void zeroPivotStowed() { + pivotSim.setState(Settings.Intake.PIVOT_MAX_ANGLE.getRadians(), 0.0); + } + + @Override + public void zeroPivotDeployed() { + pivotSim.setState(Settings.Intake.PIVOT_MIN_ANGLE.getRadians(), 0.0); + } + + @Override + public void periodic() { + super.periodic(); + + PivotState pivotState = getPivotState(); + + double targetRadPerSec = getRollerState().getTargetDutyCycle() * 2.0 * Math.PI / 60.0 * 6000.0; + + rollerLeaderController.setNextR(VecBuilder.fill(targetRadPerSec)); + rollerLeaderController.correct(VecBuilder.fill(rollerLeaderSim.getOutput(0))); + rollerLeaderController.predict(Settings.DT); + + rollerFollowerController.setNextR(VecBuilder.fill(targetRadPerSec)); + rollerFollowerController.correct(VecBuilder.fill(rollerFollowerSim.getOutput(0))); + rollerFollowerController.predict(Settings.DT); + + if (EnabledSubsystems.INTAKE.get()) { + if (pivotVoltageOverride.isPresent()) { + pivotSim.setInputVoltage(pivotVoltageOverride.get()); + } else { + pivotController.setNextR(VecBuilder.fill(pivotState.getTargetAngle().getRadians(), 0.0)); + pivotController.correct(VecBuilder.fill(pivotSim.getAngleRads(), pivotSim.getVelocityRadPerSec())); + pivotController.predict(Settings.DT); + pivotSim.setInputVoltage(pivotController.getU(0)); + } + + if (pivotState == PivotState.DEPLOY && getPivotAngle().getDegrees() <= Settings.Intake.THRESHOLD_TO_START_ROLLERS.getDegrees()) { + rollerLeaderSim.setInput(rollerLeaderController.getU(0)); + rollerFollowerSim.setInput(rollerFollowerController.getU(0)); + } else { + rollerLeaderSim.setInput(0.0); + rollerFollowerSim.setInput(0.0); + } + } else { + pivotSim.setInputVoltage(0.0); + rollerLeaderSim.setInput(0.0); + rollerFollowerSim.setInput(0.0); + } + + pivotSim.update(Settings.DT); + rollerLeaderSim.update(Settings.DT); + rollerFollowerSim.update(Settings.DT); + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Intake/Sim Pivot Angle (deg)", getPivotAngle().getDegrees()); + SmartDashboard.putNumber("Intake/Sim Pivot Velocity (deg per s)", Units.radiansToDegrees(pivotSim.getVelocityRadPerSec())); + SmartDashboard.putNumber("Intake/Sim Roller Leader Velocity (RPM)", rollerLeaderSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + SmartDashboard.putNumber("Intake/Sim Roller Follower Velocity (RPM)", rollerFollowerSim.getOutput(0) * 60.0 / (2.0 * Math.PI)); + } + } + + @Override + public void setPivotVoltageOverride(Optional voltage) { + this.pivotVoltageOverride = voltage; + } + + @Override + public SysIdRoutine getPivotSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Intake Pivot", + voltage -> setPivotVoltageOverride(Optional.of(voltage)), + () -> getPivotAngle().getRotations(), + () -> pivotSim.getVelocityRadPerSec(), + () -> pivotVoltageOverride.orElse(0.0), + getInstance() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java index df2175f2..77a24a2e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/Spindexer.java @@ -5,12 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.spindexer; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.SpindexerInterpolation; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -31,7 +27,6 @@ public static Spindexer getInstance() { public enum SpindexerState { STOP, - DYNAMIC, // In case we need for future: interpolates RPM based on distance to hub FORWARD, REVERSE; } @@ -39,7 +34,6 @@ public enum SpindexerState { public double getTargetRPM() { return switch (getState()) { case STOP -> 0; - case DYNAMIC -> getRPMBasedOnDistance(); case FORWARD -> Settings.Spindexer.FORWARD_SPEED; case REVERSE -> Settings.Spindexer.REVERSE_SPEED; }; @@ -57,16 +51,6 @@ public void setState(SpindexerState state) { this.spindexerState = state; } - /** - * @return target RPM interpolated based on distance - */ - public double getRPMBasedOnDistance() { - Translation2d hubPos = Field.getHubPose().getTranslation(); - Translation2d robotPos = CommandSwerveDrivetrain.getInstance().getPose().getTranslation(); - double distance = hubPos.getDistance(robotPos); - return SpindexerInterpolation.getRPM(distance); - } - public abstract SysIdRoutine getSysIdRoutine(); public abstract void setVoltageOverride(Optional voltage); @@ -77,7 +61,6 @@ public void periodic() { SmartDashboard.putString("States/Spindexer", getState().name()); SmartDashboard.putNumber("Spindexer/Target RPM", getTargetRPM()); - SmartDashboard.putNumber("Spindexer/Interpolated RPM Based on Distance", getRPMBasedOnDistance()); } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index 7eb88853..36c7a196 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -5,10 +5,15 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.spindexer; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.Superstructure; import com.stuypulse.robot.util.SysId; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -17,56 +22,100 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import java.util.Optional; public class SpindexerImpl extends Spindexer { + private final Motors.TalonFXConfig spindexerLeadConfig; + private final Motors.TalonFXConfig spindexerFollowerConfig; + private final TalonFX leadMotor; private final TalonFX followerMotor; private final VelocityVoltage controller; private final Follower follower; + private final BStream isStalling; private Optional voltageOverride; public SpindexerImpl() { + spindexerLeadConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(45) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) + + .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); + + spindexerFollowerConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(45) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + + .withSensorToMechanismRatio(Settings.Spindexer.GEAR_RATIO); + leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); followerMotor = new TalonFX(Ports.Spindexer.SPINDEXER_FOLLOW_MOTOR, Ports.CANIVORE); - Motors.Spindexer.SPINDEXER.configure(leadMotor); - Motors.Spindexer.SPINDEXER.configure(followerMotor); + spindexerLeadConfig.configure(leadMotor); + spindexerFollowerConfig.configure(followerMotor); + + controller = new VelocityVoltage(getTargetRPM()).withEnableFOC(true); + follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Aligned); - controller = new VelocityVoltage(getTargetRPM()) - .withEnableFOC(true); + isStalling = BStream.create(() -> leadMotor.getSupplyCurrent().getValueAsDouble() > Settings.Spindexer.STALL_CURRENT_LIMIT) + .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); - follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Opposed); + followerMotor.setControl(follower); voltageOverride = Optional.empty(); } - public double getCurrentLeadMotorRPM() { - return leadMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + private double getCurrentLeadMotorRPM() { + return leadMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } - public double getCurrentFollowerMotorRPM() { - return followerMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + private double getCurrentFollowerMotorRPM() { + return followerMotor.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE * Settings.Spindexer.GEAR_RATIO; } - public boolean atTolerance() { - return Math.abs(getCurrentLeadMotorRPM() - getTargetRPM()) <= Settings.Spindexer.RPM_TOLERANCE; + private boolean atTolerance() { + double error = getCurrentLeadMotorRPM() - getTargetRPM(); + return Math.abs(error) <= Settings.Spindexer.RPM_TOLERANCE; } @Override public void periodic() { super.periodic(); + if (EnabledSubsystems.SPINDEXER.get()) { - if (voltageOverride.isPresent()){ + if (voltageOverride.isPresent()) { leadMotor.setVoltage(voltageOverride.get()); - followerMotor.setControl(follower); } else { - leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); - followerMotor.setControl(follower); + // DO NOT REMOVE BELOW LINE - needed to brake the motor in STOP state + if (getState() == SpindexerState.STOP) { + leadMotor.stopMotor(); + } else if (Superstructure.getInstance().isTurretWrapping()) { + leadMotor.stopMotor(); + } else { + leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); + } } + } else { + leadMotor.stopMotor(); } if (Settings.DEBUG_MODE) { @@ -75,16 +124,23 @@ public void periodic() { SmartDashboard.putBoolean("Spindexer/At Tolerance", atTolerance()); - SmartDashboard.putNumber("Spindexer/Lead Current (amps)", leadMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Lead Voltage", leadMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Spindexer/Lead Stator Current", leadMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Lead Supply Current", leadMotor.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("Spindexer/Follower Current (amps)", followerMotor.getStatorCurrent().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Voltage", followerMotor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Spindexer/Follower Supply Current", followerMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Spindexer/Follower Stator Current", followerMotor.getStatorCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Spindexer Leader (amps)", leadMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Spindexer Follower (amps)", followerMotor.getSupplyCurrent().getValueAsDouble()); } } + public boolean isStalling() { + return isStalling.get(); + } + @Override public void setVoltageOverride(Optional voltage) { this.voltageOverride = voltage; diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java new file mode 100644 index 00000000..9a8cd38b --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/Superstructure.java @@ -0,0 +1,149 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.superstructure; + +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood.HoodState; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter.ShooterState; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret.TurretState; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Superstructure extends SubsystemBase { + + private static final Superstructure instance; + + static { + instance = new Superstructure(); + } + + public static Superstructure getInstance(){ + return instance; + } + + private SuperstructureState state; + + private final Hood hood; + private final Shooter shooter; + private final Turret turret; + + public Superstructure() { + state = SuperstructureState.INTERPOLATION; + hood = Hood.getInstance(); + shooter = Shooter.getInstance(); + turret = Turret.getInstance(); + } + + public enum SuperstructureState { + STOW(HoodState.STOW, ShooterState.INTERPOLATION, TurretState.SHOOT), + SHOOT(HoodState.SHOOT, ShooterState.SHOOT, TurretState.SHOOT), + FERRY(HoodState.FERRY, ShooterState.FERRY, TurretState.FERRY), + FOTM(HoodState.FOTM, ShooterState.FOTM, TurretState.FOTM), + REVERSE(HoodState.SHOOT, ShooterState.REVERSE, TurretState.SHOOT), + KB(HoodState.KB, ShooterState.KB, TurretState.KB), + LEFT_CORNER(HoodState.LEFT_CORNER, ShooterState.LEFT_CORNER, TurretState.LEFT_CORNER), + RIGHT_CORNER(HoodState.RIGHT_CORNER, ShooterState.RIGHT_CORNER, TurretState.RIGHT_CORNER), + INTERPOLATION(HoodState.INTERPOLATION, ShooterState.INTERPOLATION, TurretState.SHOOT), + AUTO_INTERPOLATION(HoodState.STOW, ShooterState.INTERPOLATION, TurretState.SHOOT), + SOTM(HoodState.SOTM, ShooterState.SOTM, TurretState.SOTM); + + private HoodState hoodState; + private ShooterState shooterState; + private TurretState turretState; + + private SuperstructureState(HoodState hoodState, ShooterState shooterState, TurretState TurretState) { + this.hoodState = hoodState; + this.shooterState = shooterState; + this.turretState = TurretState; + } + + public HoodState getHoodState() { + return hoodState; + } + + public ShooterState getShooterState() { + return shooterState; + } + + public TurretState getTurretState(){ + return turretState; + } + } + + public void setState(SuperstructureState state){ + this.state = state; + hood.setState(state.getHoodState()); + shooter.setState(state.getShooterState()); + turret.setState(state.getTurretState()); + } + + public SuperstructureState getState(){ + return state; + } + + public boolean atTolerance() { + return isShooterAtTolerance() && isHoodAtTolerance() && isTurretAtTolerance(); + } + + public boolean isShooterAtTolerance() { + return shooter.atTolerance(); + } + + public boolean isHoodAtTolerance() { + return hood.atTolerance(); + } + + public boolean isTurretAtTolerance(){ + return turret.atTolerance(); + } + + public double getTargetRPM() { + return shooter.getTargetRPM(); + } + + public double getShooterRPM() { + return shooter.getRPM(); + } + + public Rotation2d getHoodAngle() { + return hood.getAngle(); + } + + public Rotation2d getTurretAngle(){ + return turret.getAngle(); + } + + public boolean isTurretWrapping() { + return turret.isWrapping(); + } + + @Override + public void periodic() { + SuperstructureState state = getState(); + if (state == SuperstructureState.SOTM) { + SOTMCalculator.updateSOTMSolution(); + } else if (state == SuperstructureState.FOTM){ + SOTMCalculator.updateFOTMSolution(); + } + + if (CommandSwerveDrivetrain.getInstance().isOutsideAllianceZone() && state == SuperstructureState.SOTM) { + setState(SuperstructureState.FERRY); + } + + SmartDashboard.putString("Superstructure/State", state.name()); + SmartDashboard.putString("States/SuperStructure", state.name()); + + SmartDashboard.putBoolean("Superstructure/Shooter At Tolerance?", isShooterAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Hood At Tolerance?", isHoodAtTolerance()); + SmartDashboard.putBoolean("Superstructure/Turret At Tolerance?", isTurretAtTolerance()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java new file mode 100644 index 00000000..85f5075f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/Hood.java @@ -0,0 +1,147 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.superstructure.hood; + +import com.stuypulse.stuylib.input.Gamepad; + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import com.stuypulse.robot.util.superstructure.VisualizerHood; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public abstract class Hood extends SubsystemBase{ + private static final Hood instance; + + private HoodState state; + + private Rotation2d driverInput; + + static { + if (Robot.isReal()) { + instance = new HoodImpl(); + } else { + instance = new HoodSim(); + } + } + + public static Hood getInstance(){ + return instance; + } + + public enum HoodState { + STOW, + FERRY, + SHOOT, + KB, + LEFT_CORNER, + RIGHT_CORNER, + INTERPOLATION, + SOTM, + FOTM, + ANALOG, + HOMING, + IDLE; + } + + public Hood() { + state = HoodState.STOW; + } + + public HoodState getState(){ + return state; + } + + public void setState(HoodState state){ + this.state = state; + } + + public Rotation2d getTargetAngle() { + if (CommandSwerveDrivetrain.getInstance().isUnderTrench()) { + return Settings.Superstructure.Hood.Angles.STOW; + } + + if(Settings.Superstructure.Hood.Angles.OVERRIDEN.get()) { + return Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.OVERRIDE_VALUE_DEG.get()); + } + + return switch(state) { + case STOW -> Settings.Superstructure.Hood.Angles.STOW; + case FERRY -> Rotation2d.fromDegrees(39); + case SHOOT -> Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.SHOOT.get()); + case KB -> Settings.Superstructure.Hood.Angles.KB; + case LEFT_CORNER -> Settings.Superstructure.Hood.Angles.LEFT_CORNER; + case RIGHT_CORNER -> Settings.Superstructure.Hood.Angles.RIGHT_CORNER; + case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetHoodAngle(); + case HOMING -> new Rotation2d(); //should just apply a voltage, not an angle! + case SOTM -> SOTMCalculator.calculateHoodAngleSOTM(); + case FOTM -> SOTMCalculator.calculateHoodAngleFOTM(); + case ANALOG -> hoodAnalogToOutput(); + case IDLE -> getAngle(); + }; + } + + public boolean atTolerance() { + double error = getAngle().minus(getTargetAngle()).getRotations(); + if (Robot.isReal()) { + if (state == HoodState.SOTM || state == HoodState.FOTM) { + return Math.abs(error) < Settings.Superstructure.HOOD_SOTM_TOLERANCE.getRotations(); + } else { + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations(); + } + } else { + return Math.abs(error) < Settings.Superstructure.HOOD_TOLERANCE.getRotations() + (5 / 360.0); + } + } + + public abstract Rotation2d getAngle(); + + public void hoodAnalogToInput(Gamepad gamepad) { + double hoodMin = Settings.Superstructure.Hood.Angles.MIN.getDegrees(); + double hoodMax = Settings.Superstructure.Hood.Angles.MAX.getDegrees(); + + this.driverInput = Rotation2d.fromDegrees(hoodMin + (gamepad.getLeftX() + 1.0) * ((hoodMax - hoodMin) / 2)); + } + + public Rotation2d hoodAnalogToOutput() { + return this.driverInput; + } + + public abstract boolean isStalling(); + + public abstract SysIdRoutine getHoodSysIdRoutine(); + + public abstract void zeroHoodEncoderAtUpperHardstop(); + public abstract void seedHood(); + + public abstract void seedHoodAtUpperHardStop(); + public abstract void zeroHoodEncodersAfterSeed(); + + + @Override + public void periodic() { + SmartDashboard.putString("Superstructure/Hood/State", state.name()); + SmartDashboard.putString("States/Hood", state.name()); + + SmartDashboard.putNumber("Superstructure/Hood/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Hood/Current Angle", getAngle().getDegrees()); + + if (Settings.DEBUG_MODE) { + if (EnabledSubsystems.HOOD.get()) { + VisualizerHood.getInstance().update(getAngle(), atTolerance()); + } else { + // VisualizerHood.getInstance().update(new Rotation2d(), false); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java new file mode 100644 index 00000000..085d5568 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodImpl.java @@ -0,0 +1,210 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.superstructure.hood; + +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import java.util.Optional; + +public class HoodImpl extends Hood { + private final Motors.TalonFXConfig hoodConfig; + private final Motors.CANCoderConfig hoodEncoderConfig; + + private final TalonFX hoodMotor; + private final CANcoder hoodEncoder; + + private final PositionVoltage controller; + + private Optional voltageOverride; + + private boolean hasUsedAbsoluteEncoder; + + private final BStream isStalling; + + public HoodImpl() { + hoodConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + .withSupplyCurrentLimitAmps(80.0) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + .withPIDConstants(Gains.Superstructure.Hood.kP, Gains.Superstructure.Hood.kI, Gains.Superstructure.Hood.kD, 0) + .withFFConstants(Gains.Superstructure.Hood.kS, Gains.Superstructure.Hood.kV, Gains.Superstructure.Hood.kA, 0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) + .withSensorToMechanismRatio(Settings.Superstructure.Hood.GEAR_RATIO) + .withSoftLimits( + true, true, + Settings.Superstructure.Hood.FORWARD_SOFT_LIMIT.getRotations(), + Settings.Superstructure.Hood.REVERSE_SOFT_LIMIT.getRotations()); + + hoodEncoderConfig = new Motors.CANCoderConfig() + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0) + .withMagnetOffset(Settings.Superstructure.Hood.ENCODER_OFFSET.getRotations()); + + hoodMotor = new TalonFX(Ports.Superstructure.Hood.MOTOR, Ports.RIO); + hoodEncoder = new CANcoder(Ports.Superstructure.Hood.THROUGHBORE_ENCODER, Ports.RIO); + + hoodConfig.configure(hoodMotor); + hoodEncoderConfig.configure(hoodEncoder); + + controller = new PositionVoltage(getTargetAngle().getRotations()) + .withEnableFOC(true); + + voltageOverride = Optional.empty(); + + isStalling = BStream.create(() -> hoodMotor.getSupplyCurrent().getValueAsDouble() > Settings.Superstructure.Hood.STALL_CURRENT_LIMIT) //TODO: update value in Settings after testing + .filtered(new BDebounce.Both(Settings.Superstructure.Hood.STALL_DEBOUNCE)); + } + + @Override + public Rotation2d getAngle() { + return Rotation2d.fromRotations(hoodMotor.getPosition().getValueAsDouble()); + } + + @Override + public boolean isStalling() { + return isStalling.getAsBoolean(); + } + + /* + * Example: + * Let's say the hood rotates 0.1 rotations. Then, the encoder has rotated 0.1 * 10.67 rotations + * To convert the encoder reading to the mechanism position, we simply do (0.1 * 10.67) / 10.67 = 0.1 + */ + @Override + public void seedHood() { + hoodMotor.setPosition(getAbsoluteHoodAngleDeg() / 360.0); + } + + @Override + public void seedHoodAtUpperHardStop() { + hoodMotor.setPosition(Rotation2d.fromDegrees(40).getRotations()); + } + + private double getAbsoluteHoodAngleDeg() { + return Settings.Superstructure.Hood.MIN_FROM_HORIZON.getDegrees() + hoodEncoder.getAbsolutePosition().getValueAsDouble() * 360.0 / Settings.Superstructure.Hood.ENCODER_TO_MECH; + } + + @Override + public void periodic() { + super.periodic(); + + if (!hasUsedAbsoluteEncoder) { + // seedHood(); + seedHoodAtUpperHardStop(); + hasUsedAbsoluteEncoder = true; + } + + if (isStalling() && getState() == HoodState.HOMING) { + // seedHood(); + seedHoodAtUpperHardStop(); + setState(HoodState.IDLE); + SmartDashboard.putBoolean("Superstructure/Hood/SUCCESFULLY HOMED", true); + } + + if (EnabledSubsystems.HOOD.get()) { + if (voltageOverride.isPresent()) { + hoodMotor.setVoltage(voltageOverride.get()); + } else if (getState() == HoodState.HOMING && !isStalling()) { + hoodMotor.setVoltage(Settings.Superstructure.Hood.HOOD_HOMING_VOLTAGE); + } else { + hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); + } + } else { + hoodMotor.stopMotor(); + } + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Superstructure/Hood/Correct Hood Angle (deg)", getAbsoluteHoodAngleDeg()); + + SmartDashboard.putNumber("Superstructure/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Hood/Stator Current", hoodMotor.getStatorCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Superstructure/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putBoolean("Superstructure/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); + + SmartDashboard.putNumber("Superstructure/Hood/Raw Motor Encoder Value", hoodMotor.getPosition().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Hood (amps)", hoodMotor.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putBoolean("Prematch Checks/Hood at Top?", getAngle().getDegrees() > 39.0); + + } + } + + private void setVoltageOverride(Optional voltageOverride) { + this.voltageOverride = voltageOverride; + } + + @Override + public SysIdRoutine getHoodSysIdRoutine() { + return SysId.getRoutine( + .45, + 2, + "Hood", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> hoodMotor.getPosition().getValueAsDouble(), + () -> hoodMotor.getVelocity().getValueAsDouble(), + () -> hoodMotor.getMotorVoltage().getValueAsDouble(), + getInstance() + ); + } + + + //TODO: Implementation as of 3/3 but not yet tested + // Should ONLY be called at the lower hardstop! + @Override + public void zeroHoodEncoderAtUpperHardstop() { + hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); + + double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; + + double positionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); + double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MAX.getRotations()); + + hoodEncoderConfig.withMagnetOffset(newOffset); + + hoodEncoderConfig.configure(hoodEncoder); + } + + @Override + public void zeroHoodEncodersAfterSeed() { //only use if you are seeded -> might add a boolean to double check that we are in seed at Upper Hardstop ^^ + hoodEncoder.getConfigurator().refresh(hoodEncoderConfig.getConfiguration().MagnetSensor); + + double currentOffset = hoodEncoderConfig.getConfiguration().MagnetSensor.MagnetOffset; + + double encoderPositionWithCurrentOffset = hoodEncoder.getPosition().getValueAsDouble(); + double encoderPositionWithOutOffset = encoderPositionWithCurrentOffset - currentOffset; + + //double newOffset = -((positionWithCurrentOffset - currentOffset) - Settings.Superstructure.Hood.Angles.MAX.getRotations()); + double newOffset = encoderPositionWithOutOffset - hoodMotor.getPosition().getValueAsDouble(); + + hoodEncoderConfig.withMagnetOffset(newOffset); + + hoodEncoderConfig.configure(hoodEncoder); + } + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java new file mode 100644 index 00000000..7b140b29 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/hood/HoodSim.java @@ -0,0 +1,130 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.superstructure.hood; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; +import com.stuypulse.robot.util.superstructure.VisualizerHood; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import java.util.Optional; + +public class HoodSim extends Hood { + + // Simulates hood motion as a linear elevator, then converts to angle + private final ElevatorSim sim; + + private Optional voltageOverride; + + // Arc length constants — tune to match your hood geometry + private static final double HOOD_ARM_LENGTH_METERS = 0.3; + + private static final double MIN_HEIGHT = HOOD_ARM_LENGTH_METERS * + Math.sin(Settings.Superstructure.Hood.Angles.MIN.getRadians()); + private static final double MAX_HEIGHT = HOOD_ARM_LENGTH_METERS * + Math.sin(Settings.Superstructure.Hood.Angles.MAX.getRadians()); + + public HoodSim() { + sim = new ElevatorSim( + DCMotor.getKrakenX60(1), + 1.0, // gear ratio + 1.0, // carriage mass kg + 0.01, // drum radius m + MIN_HEIGHT, + MAX_HEIGHT, + true, + MIN_HEIGHT, + 0.001, 0.001 // measurementStdDevs as plain double vararg + ); + + voltageOverride = Optional.empty(); + } + + // Convert linear elevator height back to hood angle + @Override + public Rotation2d getAngle() { + double height = sim.getPositionMeters(); + double clampedHeight = Math.max(MIN_HEIGHT, Math.min(MAX_HEIGHT, height)); + double angleRad = Math.asin(clampedHeight / HOOD_ARM_LENGTH_METERS); + return Rotation2d.fromRadians(angleRad); + } + + @Override + public boolean isStalling() { + return false; + } + + @Override + public void zeroHoodEncoderAtUpperHardstop() { + // No-op in sim + } + + @Override + public void seedHood() { + // No-op in sim — ElevatorSim starts at MIN_HEIGHT + } + + private void setVoltageOverride(Optional volts) { + this.voltageOverride = volts; + } + + @Override + public SysIdRoutine getHoodSysIdRoutine() { + return SysId.getRoutine( + 0.45, + 2, + "Hood", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> sim.getPositionMeters(), + () -> sim.getVelocityMetersPerSecond(), + () -> voltageOverride.orElse(0.0), + getInstance() + ); + } + + @Override + public void periodic() { + super.periodic(); + + // Drive sim toward target angle via a simple proportional voltage + double targetHeight = HOOD_ARM_LENGTH_METERS * + Math.sin(getTargetAngle().getRadians()); + double error = targetHeight - sim.getPositionMeters(); + double voltage = Math.max(-12.0, Math.min(12.0, error * 50.0)); + + if (EnabledSubsystems.HOOD.get()) { + sim.setInputVoltage(voltageOverride.orElse(voltage)); + } else { + sim.setInputVoltage(0.0); + } + + sim.update(Settings.DT); + + VisualizerHood.getInstance().update(getAngle(), atTolerance()); + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Superstructure/Hood/Sim Height (m)", sim.getPositionMeters()); + } + } + + @Override + public void seedHoodAtUpperHardStop() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'seedHoodAtUpperHardStop'"); + } + + @Override + public void zeroHoodEncodersAfterSeed() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'zeroHoodEncodersAfterSeed'"); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java similarity index 50% rename from src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java index c18ed7c3..41737758 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/Shooter.java @@ -3,11 +3,12 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.subsystems.hoodedshooter.shooter; +package com.stuypulse.robot.subsystems.superstructure.shooter; import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.util.hoodedshooter.HoodAngleCalculator; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -35,10 +36,12 @@ public enum ShooterState { SHOOT, FERRY, REVERSE, - HUB, + KB, LEFT_CORNER, RIGHT_CORNER, - INTERPOLATION; + INTERPOLATION, + SOTM, + FOTM; } public Shooter() { @@ -54,39 +57,48 @@ public ShooterState getState() { } public double getTargetRPM() { + if(Settings.Superstructure.Shooter.RPM.OVERRIDEN.get()) { + return Settings.Superstructure.Shooter.RPM.OVERRIDE_VALUE.get(); + } + return switch(state) { case STOP -> 0; case SHOOT -> getShootRPM(); - case FERRY -> HoodAngleCalculator.interpolateFerryingRPM().get(); - case REVERSE -> Settings.HoodedShooter.RPMs.REVERSE; - case HUB -> Settings.HoodedShooter.RPMs.HUB_RPM; - case LEFT_CORNER -> Settings.HoodedShooter.RPMs.LEFT_CORNER_RPM; - case RIGHT_CORNER -> Settings.HoodedShooter.RPMs.RIGHT_CORNER_RPM; - case INTERPOLATION -> HoodAngleCalculator.interpolateShooterRPM().get(); + case FERRY -> InterpolationCalculator.interpolateFerryingInfo().targetRPM(); + case REVERSE -> Settings.Superstructure.Shooter.RPM.REVERSE; + case KB -> Settings.Superstructure.Shooter.RPM.KB; + case LEFT_CORNER -> Settings.Superstructure.Shooter.RPM.LEFT_CORNER; + case RIGHT_CORNER -> Settings.Superstructure.Shooter.RPM.RIGHT_CORNER; + case INTERPOLATION -> InterpolationCalculator.interpolateShotInfo().targetRPM(); + case SOTM -> SOTMCalculator.calculateShooterRPMSOTM(); + case FOTM -> SOTMCalculator.calculateShooterRPMFOTM(); }; } public double getShootRPM() { - return Settings.HoodedShooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass + return Settings.Superstructure.Shooter.RPM.SHOOT.get(); // Adjustable RPM on Glass } public boolean atTolerance() { - double diff = Math.abs(getTargetRPM() - getShooterRPM()); - return diff < Settings.HoodedShooter.SHOOTER_TOLERANCE_RPM; + double error = Math.abs(getTargetRPM() - getRPM()); + + if (state == ShooterState.SOTM || state == ShooterState.FOTM) { + return error < Settings.Superstructure.SHOOTER_SOTM_TOLERANCE_RPM; + } else { + return error < Settings.Superstructure.SHOOTER_TOLERANCE_RPM; + } } - public abstract double getShooterRPM(); + public abstract double getRPM(); public abstract SysIdRoutine getShooterSysIdRoutine(); @Override public void periodic() { - SmartDashboard.putString("HoodedShooter/Shooter/State", state.name()); + SmartDashboard.putString("Superstructure/Shooter/State", state.name()); SmartDashboard.putString("States/Shooter", state.name()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getShooterRPM()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Target RPM", getTargetRPM()); - - SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", HoodAngleCalculator.interpolateShooterRPM().get()); + SmartDashboard.putNumber("Superstructure/Shooter/Current RPM", getRPM()); + SmartDashboard.putNumber("Superstructure/Shooter/Target RPM", getTargetRPM()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java new file mode 100644 index 00000000..0c274dc7 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterImpl.java @@ -0,0 +1,152 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.superstructure.shooter; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import java.util.Optional; + +public class ShooterImpl extends Shooter { + private final Motors.TalonFXConfig shooterConfig; + + private final TalonFX shooterLeader; + private final TalonFX shooterFollower; + + private final VelocityVoltage shooterController; + private final Follower follower; + + private Optional voltageOverride; + + public ShooterImpl() { + shooterConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast) + + .withSupplyCurrentLimitEnabled(false) + .withStatorCurrentLimitEnabled(false) + + .withPIDConstants(Gains.Superstructure.Shooter.kP.get(), Gains.Superstructure.Shooter.kI.get(), Gains.Superstructure.Shooter.kD.get(), 0) + .withFFConstants(Gains.Superstructure.Shooter.kS.get(), Gains.Superstructure.Shooter.kV.get(), Gains.Superstructure.Shooter.kA.get(), 0) + + .withSensorToMechanismRatio(Settings.Superstructure.Shooter.GEAR_RATIO) + .withVelocityTimeFilter(0.1); + + shooterLeader = new TalonFX(Ports.Superstructure.Shooter.MOTOR_LEAD, Ports.RIO); + shooterLeader.getVelocity().setUpdateFrequency(1000.0); + + shooterFollower = new TalonFX(Ports.Superstructure.Shooter.MOTOR_FOLLOW, Ports.RIO); + shooterFollower.getVelocity().setUpdateFrequency(1000.0); + + shooterConfig.configure(shooterLeader); + shooterConfig.configure(shooterFollower); + + shooterController = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true); + follower = new Follower(Ports.Superstructure.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); + + shooterFollower.setControl(follower); + + voltageOverride = Optional.empty(); + } + + @Override + public double getRPM() { + return getLeaderRPM(); + } + + public double getLeaderRPM() { + return shooterLeader.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + } + + public double getFollowerRPM() { + return shooterFollower.getVelocity().getValueAsDouble() * Settings.SECONDS_IN_A_MINUTE; + } + + @Override + public void periodic() { + super.periodic(); + + shooterConfig.updateGainsConfig( + shooterLeader, + 0, + Gains.Superstructure.Shooter.kP, + Gains.Superstructure.Shooter.kI, + Gains.Superstructure.Shooter.kD, + Gains.Superstructure.Shooter.kS, + Gains.Superstructure.Shooter.kV, + Gains.Superstructure.Shooter.kA + ); + + shooterConfig.updateGainsConfig( + shooterFollower, + 0, + Gains.Superstructure.Shooter.kP, + Gains.Superstructure.Shooter.kI, + Gains.Superstructure.Shooter.kD, + Gains.Superstructure.Shooter.kS, + Gains.Superstructure.Shooter.kV, + Gains.Superstructure.Shooter.kA + ); + + if (EnabledSubsystems.SHOOTER.get() || getState() == ShooterState.STOP) { + if (voltageOverride.isPresent()) { + shooterLeader.setVoltage(voltageOverride.get()); + } else { + shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); + } + } else { + shooterLeader.stopMotor(); + } + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("Superstructure/Shooter/Leader Current (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Supply Current (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Stator Current", shooterFollower.getStatorCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Superstructure/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("Superstructure/Shooter/Follower RPM", getFollowerRPM()); + + SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error", shooterLeader.getClosedLoopError().getValueAsDouble() * 60.0); + SmartDashboard.putNumber("InterpolationTesting/Shooter Applied Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("Current Draws/Shooter Leader (amps)", shooterLeader.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Current Draws/Shooter Follower (amps)", shooterFollower.getSupplyCurrent().getValueAsDouble()); + } + } + + private void setVoltageOverride(Optional voltageOverride) { + this.voltageOverride = voltageOverride; + } + + @Override + public SysIdRoutine getShooterSysIdRoutine() { + return SysId.getRoutine( + 1, + 5, + "Shooter", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> shooterLeader.getPosition().getValueAsDouble(), + () -> shooterLeader.getVelocity().getValueAsDouble(), + () -> shooterLeader.getMotorVoltage().getValueAsDouble(), + getInstance() + ); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java similarity index 87% rename from src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterSim.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java index c0fefef9..cd2b2c2f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/shooter/ShooterSim.java @@ -3,7 +3,7 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.subsystems.hoodedshooter.shooter; +package com.stuypulse.robot.subsystems.superstructure.shooter; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; @@ -32,7 +32,7 @@ public class ShooterSim extends Shooter { private Optional voltageOverride; public ShooterSim() { - LinearSystem flywheel = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX44(2), 0.20, 1.0); + LinearSystem flywheel = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX44(2), 0.05, 1.0); sim = new LinearSystemSim<>(flywheel); @@ -56,7 +56,7 @@ public ShooterSim() { } @Override - public double getShooterRPM() { + public double getRPM() { return sim.getOutput(0) * 60.0 / (2.0 * Math.PI); // convert to RPM } @@ -75,14 +75,14 @@ public void periodic() { if (EnabledSubsystems.SHOOTER.get()) { if (voltageOverride.isPresent()) { sim.setInput(voltageOverride.get()); - SmartDashboard.putNumber("HoodedShooter/Shooter/Input Voltage", voltageOverride.get()); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", voltageOverride.get()); } else { - SmartDashboard.putNumber("HoodedShooter/Shooter/Input Voltage", controller.getU(0)); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", controller.getU(0)); sim.setInput(controller.getU(0)); } } else { sim.setInput(0); - SmartDashboard.putNumber("HoodedShooter/Shooter/Input Voltage", 0.0); + SmartDashboard.putNumber("Superstructure/Shooter/Input Voltage", 0.0); } sim.update(Settings.DT); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java similarity index 50% rename from src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java index 7dc79e49..39597a2f 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/Turret.java @@ -3,7 +3,7 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.subsystems.turret; +package com.stuypulse.robot.subsystems.superstructure.turret; import com.stuypulse.stuylib.input.Gamepad; import com.stuypulse.stuylib.math.Vector2D; @@ -13,10 +13,13 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.turret.TurretVisualizer; +import com.stuypulse.robot.util.superstructure.SOTMCalculator; +import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; +import com.stuypulse.robot.util.superstructure.VisualizerTurret; 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.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -35,8 +38,8 @@ public static Turret getInstance() { } public Turret() { - driverInput = new Vector2D(0, 0); - state = TurretState.IDLE; + driverInput = Vector2D.kOrigin; + state = TurretState.SHOOT; } public void setDriverInput(Gamepad gamepad) { @@ -46,11 +49,13 @@ public void setDriverInput(Gamepad gamepad) { public enum TurretState { IDLE, ZERO, - SHOOTING, - FERRYING, - HUB, + SHOOT, + SOTM, + FOTM, + FERRY, LEFT_CORNER, RIGHT_CORNER, + KB, TESTING; } @@ -58,31 +63,44 @@ public Rotation2d getTargetAngle() { return switch (getState()) { case IDLE -> getAngle(); case ZERO -> Rotation2d.kZero; - case SHOOTING -> getScoringAngle(); - case FERRYING -> getFerryAngle(); - case HUB -> Settings.Turret.HUB; - case LEFT_CORNER -> Settings.Turret.LEFT_CORNER; - case RIGHT_CORNER -> Settings.Turret.RIGHT_CORNER; + case SHOOT -> getScoringAngle(); + case SOTM -> SOTMCalculator.calculateTurretAngleSOTM(); + case FOTM -> SOTMCalculator.calculateTurretAngleFOTM(); + case FERRY -> getFerryAngle(); + case LEFT_CORNER -> Settings.Superstructure.Turret.LEFT_CORNER; + case RIGHT_CORNER -> Settings.Superstructure.Turret.RIGHT_CORNER; + case KB -> Settings.Superstructure.Turret.KB; case TESTING -> driverInputToAngle(); }; } public Rotation2d driverInputToAngle() { - SmartDashboard.putNumber("Turret/Driver Input", driverInput.x); + SmartDashboard.putNumber("Superstructure/Turret/Driver Input", driverInput.x); return Rotation2d.fromDegrees(driverInput.x * 180); } - public boolean atTargetAngle() { - return Math.abs(getAngle().minus(getTargetAngle()).getDegrees()) < Settings.Turret.TOLERANCE_DEG; + public boolean atTolerance() { + double error = getAngle().minus(getTargetAngle()).getRotations(); + + if (state == TurretState.SOTM || state == TurretState.FOTM) { + return Math.abs(error) < Settings.Superstructure.Turret.SOTM_TOLERANCE.getRotations(); + } else { + return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); + } } public Rotation2d getScoringAngle() { - return getPointAtTargetAngle(Field.getHubPose()); + Translation2d target = Field.getHubPose().getTranslation(); + Translation2d turret = CommandSwerveDrivetrain.getInstance().getTurretPose().getTranslation(); + return TurretAngleCalculator.getPointAtTargetAngle(target, turret); } public Rotation2d getFerryAngle() { Pose2d robot = CommandSwerveDrivetrain.getInstance().getPose(); - return getPointAtTargetAngle(Field.getFerryZonePose(robot.getTranslation())); + Translation2d target = Field.getFerryZonePose(robot.getTranslation()).getTranslation(); + Translation2d turret = CommandSwerveDrivetrain.getInstance().getTurretPose().getTranslation(); + + return TurretAngleCalculator.getPointAtTargetAngle(target, turret); } public abstract Rotation2d getAngle(); @@ -90,6 +108,9 @@ public Rotation2d getFerryAngle() { public abstract SysIdRoutine getSysIdRoutine(); public abstract void seedTurret(); + public abstract void zeroEncoders(); + + public abstract boolean isWrapping(); public void setState(TurretState state) { this.state = state; @@ -101,46 +122,19 @@ public TurretState getState() { @Override public void periodic() { - SmartDashboard.putString("Turret/State", state.name()); + SmartDashboard.putString("Superstructure/Turret/State", state.name()); SmartDashboard.putString("States/Turret", state.name()); - SmartDashboard.putNumber("Turret/Target Angle", getTargetAngle().getDegrees()); + + SmartDashboard.putNumber("Superstructure/Turret/Target Angle", getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/Turret/Current Angle", getAngle().getDegrees()); if (Settings.DEBUG_MODE) { if (EnabledSubsystems.TURRET.get()) { - TurretVisualizer.getInstance().updateTurretAngle(getAngle().plus((Robot.isBlue() ? Rotation2d.kZero : Rotation2d.k180deg)), atTargetAngle()); + VisualizerTurret.getInstance().updateTurretAngle(getAngle().plus((Robot.isBlue() ? Rotation2d.kZero : Rotation2d.k180deg)), atTolerance()); } else { - TurretVisualizer.getInstance().updateTurretAngle(new Rotation2d(), false); + VisualizerTurret.getInstance().updateTurretAngle(new Rotation2d(), false); } } } - - public Rotation2d getPointAtTargetAngle(Pose2d targetPose) { - Pose2d robotPose = CommandSwerveDrivetrain.getInstance().getPose(); - Pose2d turretPose = CommandSwerveDrivetrain.getInstance().getTurretPose(); - - Vector2D turret = new Vector2D(turretPose.getTranslation()); - Vector2D target = new Vector2D(targetPose.getTranslation()); - - Vector2D turretToTarget = target.sub(turret); - Vector2D zeroVector = new Vector2D(robotPose.getRotation().getCos(), robotPose.getRotation().getSin()); - - // https://www.youtube.com/watch?v=_VuZZ9_58Wg - double crossProduct = zeroVector.x * turretToTarget.y - zeroVector.y * turretToTarget.x; - double dotProduct = zeroVector.dot(turretToTarget); - - SmartDashboard.putNumber("Turret/Turret to Target Vector X", turretToTarget.x); - SmartDashboard.putNumber("Turret/Turret to Target Vector Y", turretToTarget.y); - - SmartDashboard.putNumber("Turret/Target Pose X", targetPose.getX()); - SmartDashboard.putNumber("Turret/Target Pose Y", targetPose.getY()); - SmartDashboard.putNumber("Turret/Zero Vector X", zeroVector.x); - SmartDashboard.putNumber("Turret/Zero Vector Y", zeroVector.y); - - Rotation2d targetAngle = (Robot.isReal() ? - Rotation2d.fromRadians(-Math.atan2(crossProduct, dotProduct)) : - Rotation2d.fromRadians(Math.atan2(crossProduct, dotProduct))); - - return targetAngle; - } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java new file mode 100644 index 00000000..79839e48 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretImpl.java @@ -0,0 +1,220 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.subsystems.superstructure.turret; + +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; +import com.stuypulse.robot.constants.Gains; +import com.stuypulse.robot.constants.Motors; +import com.stuypulse.robot.constants.Ports; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.util.SysId; +import com.stuypulse.robot.util.superstructure.TurretAngleCalculator; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; +import java.util.Optional; + +public class TurretImpl extends Turret { + private final Motors.TalonFXConfig turretConfig; + private final Motors.CANCoderConfig encoder17tConfig; + private final Motors.CANCoderConfig encoder18tConfig; + + private final TalonFX turretMotor; + private final CANcoder encoder17t; + private final CANcoder encoder18t; + + private boolean hasUsedAbsoluteEncoder; + private Optional voltageOverride; + private final PositionVoltage controller; + + private boolean isWrapping; + + public TurretImpl() { + turretConfig = new Motors.TalonFXConfig() + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Brake) + + .withSupplyCurrentLimitAmps(80) + .withStatorCurrentLimitEnabled(false) + .withRampRate(0.25) + + .withPIDConstants(Gains.Superstructure.Turret.slot0.kP, Gains.Superstructure.Turret.slot0.kI, Gains.Superstructure.Turret.slot0.kD, 0) + .withFFConstants(Gains.Superstructure.Turret.slot0.kS, Gains.Superstructure.Turret.slot0.kV, Gains.Superstructure.Turret.slot0.kA, 0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 0) + + .withPIDConstants(Gains.Superstructure.Turret.slot1.kP.get(), Gains.Superstructure.Turret.slot1.kI.get(), Gains.Superstructure.Turret.slot1.kD.get(), 1) + .withFFConstants(Gains.Superstructure.Turret.slot1.kS.get(), Gains.Superstructure.Turret.slot1.kV.get(), Gains.Superstructure.Turret.slot1.kA.get(), 1) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign, 1) + + .withSensorToMechanismRatio(Settings.Superstructure.Turret.GEAR_RATIO_MOTOR_TO_MECH) + + .withSoftLimits( + false, false, + Settings.Superstructure.Turret.SoftwareLimit.FORWARD_MAX_ROTATIONS, + Settings.Superstructure.Turret.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); + + encoder17tConfig = new Motors.CANCoderConfig() + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(Settings.Superstructure.Turret.Encoder17t.OFFSET.getRotations()) + .withAbsoluteSensorDiscontinuityPoint(1.0); + + encoder18tConfig = new Motors.CANCoderConfig() + .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) + .withMagnetOffset(Settings.Superstructure.Turret.Encoder18t.OFFSET.getRotations()) + .withAbsoluteSensorDiscontinuityPoint(1.0); + + turretMotor = new TalonFX(Ports.Superstructure.Turret.MOTOR, Ports.RIO); + encoder17t = new CANcoder(Ports.Superstructure.Turret.ENCODER17T, Ports.RIO); + encoder18t = new CANcoder(Ports.Superstructure.Turret.ENCODER18T, Ports.RIO); + + turretConfig.configure(turretMotor); + encoder17tConfig.configure(encoder17t); + encoder18tConfig.configure(encoder18t); + + hasUsedAbsoluteEncoder = false; + voltageOverride = Optional.empty(); + controller = new PositionVoltage(getTargetAngle().getRotations()).withEnableFOC(true); + } + + private Rotation2d getEncoderPos17t() { + return Rotation2d.fromRotations(this.encoder17t.getAbsolutePosition().getValueAsDouble()); + } + + private Rotation2d getEncoderPos18t() { + return Rotation2d.fromRotations(this.encoder18t.getAbsolutePosition().getValueAsDouble()); + } + + public Rotation2d getVectorSpaceAngle() { + return TurretAngleCalculator.getAbsoluteAngle(getEncoderPos17t().getDegrees(), getEncoderPos18t().getDegrees()); + } + + public void zeroEncoders() { + double encoderPos17T = encoder17t.getAbsolutePosition().getValueAsDouble(); + double encoderPos18T = encoder18t.getAbsolutePosition().getValueAsDouble(); + + encoder17t.getConfigurator().refresh(encoder17tConfig.getConfiguration().MagnetSensor); + encoder18t.getConfigurator().refresh(encoder18tConfig.getConfiguration().MagnetSensor); + + double currentOffset17T = encoder17tConfig.getConfiguration().MagnetSensor.MagnetOffset; + double currentOffset18T = encoder18tConfig.getConfiguration().MagnetSensor.MagnetOffset; + + double newOffset17T = currentOffset17T - encoderPos17T; + double newOffset18T = currentOffset18T - encoderPos18T; + + encoder17tConfig.withMagnetOffset(newOffset17T); + encoder18tConfig.withMagnetOffset(newOffset18T); + + encoder17tConfig.configure(encoder17t); + encoder18tConfig.configure(encoder18t); + } + + public void seedTurret() { + turretMotor.setPosition(getVectorSpaceAngle().getRotations()); + } + + public boolean isWrapping() { + return isWrapping; + } + + @Override + public Rotation2d getAngle() { + return Rotation2d.fromRotations(turretMotor.getPosition().getValueAsDouble()); + } + + private double getDelta(double target, double current) { + double delta = (target - current) % 360; + + if (delta > 180.0) delta -= 360; + else if (delta < -180) delta += 360; + + if (current + delta < Settings.Superstructure.Turret.RANGE_LEFT) return delta + 360; + if (current + delta > Settings.Superstructure.Turret.RANGE_RIGHT) return delta - 360; + + return delta; + } + + @Override + public void periodic() { + super.periodic(); + + turretConfig.updateGainsConfig(turretMotor, 1, + Gains.Superstructure.Turret.slot1.kP, + Gains.Superstructure.Turret.slot1.kI, + Gains.Superstructure.Turret.slot1.kD, + Gains.Superstructure.Turret.slot1.kS, + Gains.Superstructure.Turret.slot1.kV, + Gains.Superstructure.Turret.slot1.kA); + + if (!hasUsedAbsoluteEncoder) { + seedTurret(); + hasUsedAbsoluteEncoder = true; + } + + double currentAngle = getAngle().getDegrees(); + double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); + + isWrapping = Math.abs(actualTargetDeg - currentAngle) > Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); + int slot = 0; + + if (isWrapping) { + slot = 1; + } + + if (EnabledSubsystems.TURRET.get()) { + if (voltageOverride.isPresent()) { + turretMotor.setVoltage(voltageOverride.get()); + } else { + turretMotor.setControl(controller.withPosition(actualTargetDeg / 360.0).withSlot(slot)); + } + } else { + turretMotor.stopMotor(); + } + + if (Settings.DEBUG_MODE) { + + SmartDashboard.putNumber("Superstructure/Turret/Relative Encoder Position (Rot)", turretMotor.getPosition().getValueAsDouble() * 360.0); + SmartDashboard.putNumber("Superstructure/Turret/Closed Loop Error (deg)", turretMotor.getClosedLoopError().getValueAsDouble() * 360.0); + + SmartDashboard.putNumber("Superstructure/Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); + // SmartDashboard.putNumber("Superstructure/Turret/Vector Space Position (Deg)", getVectorSpaceAngle().getDegrees()); + + SmartDashboard.putNumber("Superstructure/Turret/Voltage", turretMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Stator Current", turretMotor.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Superstructure/Turret/Supply Current", turretMotor.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetDeg); + + SmartDashboard.putNumber("Current Draws/Turret (amps)", turretMotor.getSupplyCurrent().getValueAsDouble()); + } + } + + private void setVoltageOverride(Optional volts) { + this.voltageOverride = volts; + } + + @Override + public SysIdRoutine getSysIdRoutine() { + return SysId.getRoutine( + 2, + 6, + "Turret", + voltage -> setVoltageOverride(Optional.of(voltage)), + () -> this.turretMotor.getPosition().getValueAsDouble(), + () -> this.turretMotor.getVelocity().getValueAsDouble(), + () -> this.turretMotor.getMotorVoltage().getValueAsDouble(), + getInstance()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java similarity index 65% rename from src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java rename to src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java index 5354d939..e4e02aa0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/superstructure/turret/TurretSim.java @@ -3,7 +3,7 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.subsystems.turret; +package com.stuypulse.robot.subsystems.superstructure.turret; import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Settings; @@ -40,6 +40,8 @@ public class TurretSim extends Turret { private double maxAngularVelRadiansPerSecond; private double maxAngularAccelRadiansPerSecondSquared; + private boolean isWrapping; + private Optional voltageOverride; public TurretSim() { @@ -72,6 +74,7 @@ public TurretSim() { setpoint = new TrapezoidProfile.State(); voltageOverride = Optional.empty(); + isWrapping = false; } @Override @@ -84,6 +87,22 @@ public void seedTurret() { return; } + @Override + public void zeroEncoders() { + return; + } + + @Override + public boolean isWrapping() { + return isWrapping; + } + + @Override + public boolean atTolerance() { + double error = getAngle().minus(getTargetAngle()).getRotations(); + return Math.abs(error) < Settings.Superstructure.Turret.TOLERANCE.getRotations(); + } + private double getAngularVelocityRadPerSec() { return sim.getOutput(1); } @@ -92,24 +111,43 @@ private void setVoltageOverride(Optional volts) { voltageOverride = volts; } + private double getDelta(double target, double current) { + double delta = (target - current) % 360; + + if (delta > 180.0) delta -= 360; + else if (delta < -180) delta += 360; + + if (current + delta < Settings.Superstructure.Turret.RANGE_LEFT) return delta + 360; + if (current + delta > Settings.Superstructure.Turret.RANGE_RIGHT) return delta - 360; + + return delta; + } + @Override public void periodic() { super.periodic(); + + double currentAngle = getAngle().getDegrees(); + double targetAngle = getTargetAngle().getDegrees(); + double delta = getDelta(targetAngle, currentAngle); + double actualTargetDeg = currentAngle + delta; + + isWrapping = Math.abs(actualTargetDeg - currentAngle) > + Settings.Superstructure.Turret.GAIN_SWITCHING_THRESHOLD.getDegrees(); - goal = new TrapezoidProfile.State(getTargetAngle().getRadians(), 0.0); + goal = new TrapezoidProfile.State(Units.degreesToRadians(actualTargetDeg), 0.0); setpoint = profile.calculate(Settings.DT, setpoint, goal); - SmartDashboard.putNumber("Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); - SmartDashboard.putNumber("Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); - - SmartDashboard.putNumber("Turret/Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); - SmartDashboard.putNumber("Turret/Target (deg)", Units.radiansToDegrees(getTargetAngle().getRadians())); - SmartDashboard.putBoolean("Turret/At Target", atTargetAngle()); - SmartDashboard.putNumber("Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); + SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Vel (deg per s)", Units.radiansToDegrees(maxAngularVelRadiansPerSecond)); + SmartDashboard.putNumber("Superstructure/Turret/Constraints/Max Accel (deg per s per s)", Units.radiansToDegrees(maxAngularAccelRadiansPerSecondSquared)); - SmartDashboard.putNumber("Turret/Current Angle (deg)", sim.getOutput(0)); + SmartDashboard.putNumber("Superstructure/Turret/Motion Profile Setpoint (deg)", Units.radiansToDegrees(setpoint.position)); + SmartDashboard.putNumber("Superstructure/Turret/Error: abs(turret - target) (deg)", Math.abs(getAngle().minus(getTargetAngle()).getDegrees())); + SmartDashboard.putNumber("Superstructure/Turret/Current Angle (deg)", Units.radiansToDegrees(sim.getOutput(0))); + SmartDashboard.putNumber("Superstructure/Turret/Wrapped Target Angle (deg)", actualTargetDeg); + SmartDashboard.putBoolean("Superstructure/Turret/Is Wrapping", isWrapping); - controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); + controller.setNextR(VecBuilder.fill(setpoint.position, 0.0)); controller.correct(VecBuilder.fill(sim.getOutput(0), sim.getOutput(1))); controller.predict(Settings.DT); diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 65944d27..851e4cf1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -16,12 +16,13 @@ import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Gains; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; import com.stuypulse.robot.subsystems.swerve.TunerConstants.TunerSwerveDrivetrain; -import com.stuypulse.robot.subsystems.turret.Turret; import edu.wpi.first.math.Matrix; 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.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -56,6 +57,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private FieldObject2d turret2d = Field.FIELD2D.getObject("Turret 2D"); private Pose2d turretPose = new Pose2d(); + + + static { instance = TunerConstants.createDrivetrain(); } @@ -113,7 +117,7 @@ public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { private final SysIdRoutine m_sysIdRoutineChassisTranslation = new SysIdRoutine( new SysIdRoutine.Config( /* This is in meters per second², but SysId only supports "volts per second" */ - Volts.of(1).per(Second), + Volts.of(0.5).per(Second), /* This is in meters per second, but SysId only supports "volts" */ Volts.of(Settings.Swerve.Constraints.MAX_VELOCITY_M_PER_S), null, // Use default timeout (10 s) @@ -123,7 +127,7 @@ public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { new SysIdRoutine.Mechanism( output -> { /* output is actually meters per second, but SysId only supports "volts" */ - setControl(getFieldCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0).withRotationalRate(0)); + setControl(getRobotCentricSwerveRequest().withVelocityX(output.in(Volts)).withVelocityY(0).withRotationalRate(0)); /* also log the requested output for SysId */ SignalLogger.writeDouble("Target X Velocity ('voltage')", output.in(Volts)); SignalLogger.writeDouble("X Position", getPose().getX()); @@ -180,7 +184,7 @@ public SwerveRequest.RobotCentric getRobotCentricSwerveRequest() { ); /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineModuleTranslation; + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineChassisTranslation; /** * Constructs a CTRE SwerveDrivetrain using the specified constants. @@ -290,6 +294,14 @@ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return m_sysIdRoutineToApply.dynamic(direction); } + public Command sysidRotationDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.dynamic(direction); + } + + public Command sysidRotationQuasiStatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineRotation.quasistatic(direction); + } + private void startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds(); @@ -405,6 +417,8 @@ private void setChassisSpeeds(ChassisSpeeds robotSpeeds) { .withRotationalRate(robotSpeeds.omegaRadiansPerSecond)); } + + public void drive(Vector2D velocity, double rotation) { ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds( Robot.isBlue() ? velocity.y : -velocity.y, @@ -429,18 +443,75 @@ public Pose2d getTurretPose() { return turretPose; } + public double[] getWheelDrivePositionsRadians(){ + double[] positions = new double[4]; + double kDriveGearRatio = 6.48; + for(int i = 0; i < 4; i++){ + positions[i] = getModule(i).getDriveMotor().getPosition().getValueAsDouble() * 2 * Math.PI / kDriveGearRatio; + } + return positions; + } + + public boolean isUnderTrench() { + Translation2d turretTranslation = getTurretPose().getTranslation(); + + boolean isBetweenRightTrenchesY = Field.AllianceRightTrench.rightEdge.getY() < turretTranslation.getY() && Field.AllianceRightTrench.leftEdge.getY() > turretTranslation.getY(); + + boolean isBetweenLeftTrenchesY = Field.AllianceLeftTrench.rightEdge.getY() < turretTranslation.getY() && Field.AllianceLeftTrench.leftEdge.getY() > turretTranslation.getY(); + + boolean isUnderAllianceTrenchX = Math.abs(turretTranslation.getX() - Field.AllianceRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; + + boolean isUnderOpponentTrenchX = Math.abs(turretTranslation.getX() - Field.OpponentRightTrench.rightEdge.getX()) < Field.TRENCH_HOOD_TOLERANCE; + + boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isUnderAllianceTrenchX || isUnderOpponentTrenchX); + + return isUnderTrench; + } + + + public boolean isInOpponentZone(){ + Translation2d turretTranslation = getTurretPose().getTranslation(); + return turretTranslation.getX() > Field.OPPONENT_ZONE_X; + } + + public boolean isBehindTower() { + boolean withinTowerX = getPose().getTranslation().getX() < Field.towerFarCenter.getX(); + boolean withinTowerY = Field.towerFarRight.getY() < getTurretPose().getTranslation().getY() && getTurretPose().getTranslation().getY() < Field.towerFarLeft.getY(); + return withinTowerX && withinTowerY; + } + + // Stop ferrying when in rectangle behind hub (in neutral zone) + public boolean isBehindHub() { + Translation2d turretTranslation = getTurretPose().getTranslation(); + boolean behindHubX = Field.hubFarLeftCorner.getX() < turretTranslation.getX() && turretTranslation.getX() < Field.hubFarLeftCorner.getX() + Field.hubTolerance; + boolean withinHubY = Field.hubFarRightCorner.getY() < getTurretPose().getY() && getTurretPose().getY() < Field.hubFarLeftCorner.getY(); + + return behindHubX && withinHubY; + } + + public boolean isOutsideAllianceZone() { + return getPose().getX() > Field.AllianceRightTrench.rightEdge.getX() + Field.TRENCH_HOOD_TOLERANCE; + } + @Override public void periodic() { Pose2d pose = getPose(); turretPose = new Pose2d( - pose.getTranslation().plus(Settings.Turret.Constants.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), + pose.getTranslation().plus(Settings.Superstructure.Turret.TURRET_OFFSET.getTranslation().rotateBy(pose.getRotation())), pose.getRotation().plus(Turret.getInstance().getAngle()) ); turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); - SmartDashboard.putNumber("Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); + SmartDashboard.putBoolean("FieldPositions/isBehindTower", isBehindTower()); + SmartDashboard.putBoolean("FieldPositions/isUnderTrench", isUnderTrench()); + SmartDashboard.putBoolean("FieldPositions/isBehindHub", isBehindHub()); + SmartDashboard.putBoolean("FieldPositions/isInOpponentZone", isInOpponentZone()); + + + SmartDashboard.putNumber("Superstructure/Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); + SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Ferry Zone", turretPose.getTranslation().getDistance(Field.getFerryZonePose(pose.getTranslation()).getTranslation())); SmartDashboard.putNumber("Swerve/Pose/X", pose.getX()); SmartDashboard.putNumber("Swerve/Pose/Y", pose.getY()); @@ -466,18 +537,16 @@ public void periodic() { Field.FIELD2D.getRobotObject().setPose(Robot.isBlue() ? pose : Field.transformToOppositeAlliance(pose)); - if (Settings.DEBUG_MODE) { - } - - SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", getChassisSpeeds().vxMetersPerSecond); - SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", getChassisSpeeds().vyMetersPerSecond); + if (Settings.DEBUG_MODE) {} + ChassisSpeeds chassisSpeeds = getChassisSpeeds(); + SmartDashboard.putNumber("Swerve/Velocity Robot Relative X (m per s)", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Swerve/Velocity Robot Relative Y (m per s)", chassisSpeeds.vyMetersPerSecond); SmartDashboard.putNumber("Swerve/Velocity Field Relative X (m per s)", getFieldRelativeSpeeds().x); SmartDashboard.putNumber("Swerve/Field Relative Rotation", pose.getRotation().getDegrees()); SmartDashboard.putNumber("Swerve/Velocity Field Relative Y (m per s)", getFieldRelativeSpeeds().y); - SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", getChassisSpeeds().omegaRadiansPerSecond); - + SmartDashboard.putNumber("Swerve/Angular Velocity (rad per s)", chassisSpeeds.omegaRadiansPerSecond); SmartDashboard.putNumber("Swerve/Distance From Hub (meters)", Field.hubCenter.getTranslation().getDistance(getPose().getTranslation())); - } -} + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java index f5a896ff..f8ad7f30 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/TunerConstants.java @@ -7,9 +7,6 @@ import static edu.wpi.first.units.Units.*; -import com.stuypulse.robot.constants.Gains; -import com.stuypulse.robot.constants.Settings; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -22,6 +19,7 @@ import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { @@ -30,14 +28,14 @@ public class TunerConstants { // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(Gains.Swerve.Turn.kP).withKI(Gains.Swerve.Turn.kI).withKD(Gains.Swerve.Turn.kD) - .withKS(Gains.Swerve.Turn.kS).withKV(Gains.Swerve.Turn.kV).withKA(Gains.Swerve.Turn.kA) + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(1.16).withKA(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(Gains.Swerve.Drive.kP).withKI(Gains.Swerve.Drive.kI).withKD(Gains.Swerve.Drive.kV) - .withKS(Gains.Swerve.Drive.kS).withKV(Gains.Swerve.Drive.kV); + .withKP(0.115).withKI(0).withKD(0) + .withKS(0.19896).withKV(0.12528).withKA(0.011662); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors @@ -61,7 +59,11 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(Amps.of(80)) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -76,24 +78,24 @@ public class TunerConstants { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = Settings.CANIVORE; + public static final CANBus kCANBus = new CANBus("CANIVORE", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.76); + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.76); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.125; + private static final double kCoupleRatio = 5.4; - private static final double kDriveGearRatio = 5.357142857142857; - private static final double kSteerGearRatio = 21.428571428571427; + private static final double kDriveGearRatio = 6.48; + private static final double kSteerGearRatio = 12.1; private static final Distance kWheelRadius = Inches.of(2); private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 5; + private static final int kPigeonId = 0; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -132,48 +134,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 32; - private static final int kFrontLeftSteerMotorId = 42; - private static final int kFrontLeftEncoderId = 4; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.09375); + private static final int kFrontLeftDriveMotorId = 14; + private static final int kFrontLeftSteerMotorId = 15; + private static final int kFrontLeftEncoderId = 2; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.21435546875); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(17.15); - private static final Distance kFrontLeftYPos = Inches.of(19.7); + private static final Distance kFrontLeftXPos = Inches.of(9.0); + private static final Distance kFrontLeftYPos = Inches.of(13.0); // Front Right - private static final int kFrontRightDriveMotorId = 50; - private static final int kFrontRightSteerMotorId = 13; - private static final int kFrontRightEncoderId = 2; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.14306640625); + private static final int kFrontRightDriveMotorId = 13; + private static final int kFrontRightSteerMotorId = 12; + private static final int kFrontRightEncoderId = 3; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.291259765625); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(17.15); - private static final Distance kFrontRightYPos = Inches.of(-19.7); + private static final Distance kFrontRightXPos = Inches.of(9.0); + private static final Distance kFrontRightYPos = Inches.of(-13.0); // Back Left - private static final int kBackLeftDriveMotorId = 22; - private static final int kBackLeftSteerMotorId = 11; - private static final int kBackLeftEncoderId = 3; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.251708984375); + private static final int kBackLeftDriveMotorId = 16; + private static final int kBackLeftSteerMotorId = 17; + private static final int kBackLeftEncoderId = 1; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.484375); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-17.15); - private static final Distance kBackLeftYPos = Inches.of(19.7); + private static final Distance kBackLeftXPos = Inches.of(-9.0); + private static final Distance kBackLeftYPos = Inches.of(13.0); // Back Right - private static final int kBackRightDriveMotorId = 21; - private static final int kBackRightSteerMotorId = 0; - private static final int kBackRightEncoderId = 1; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.17822265625); + private static final int kBackRightDriveMotorId = 11; + private static final int kBackRightSteerMotorId = 10; + private static final int kBackRightEncoderId = 4; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.462890625); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-17.15); - private static final Distance kBackRightYPos = Inches.of(-19.7); + private static final Distance kBackRightXPos = Inches.of(-9.0); + private static final Distance kBackRightYPos = Inches.of(-13.0); public static final SwerveModuleConstants FrontLeft = @@ -289,4 +291,4 @@ public TunerSwerveDrivetrain( ); } } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java deleted file mode 100644 index 97d58657..00000000 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ /dev/null @@ -1,162 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.subsystems.turret; - -import com.stuypulse.robot.RobotContainer.EnabledSubsystems; -import com.stuypulse.robot.constants.Motors; -import com.stuypulse.robot.constants.Ports; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.Turret.Constants; -import com.stuypulse.robot.util.SysId; -import com.stuypulse.robot.util.turret.TurretAngleCalculator; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; - -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import java.util.Optional; - -public class TurretImpl extends Turret { - private final TalonFX motor; - private final CANcoder encoder17t; - private final CANcoder encoder18t; - - private boolean hasUsedAbsoluteEncoder; - private Optional voltageOverride; - private final PositionVoltage controller; - - - public TurretImpl() { - motor = new TalonFX(Ports.Turret.MOTOR, Ports.RIO); - encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); - encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.RIO); - - Motors.Turret.TURRET.configure(motor); - - motor.getConfigurator().apply(Motors.Turret.SLOT_0); - motor.getConfigurator().apply(Motors.Turret.SOFT_LIMITS); - - seedTurret(); - - hasUsedAbsoluteEncoder = false; - voltageOverride = Optional.empty(); - controller = new PositionVoltage(getTargetAngle().getRotations()) - .withEnableFOC(true); - } - - private Rotation2d getEncoderPos17t() { - return Rotation2d.fromRotations(this.encoder17t.getAbsolutePosition().getValueAsDouble()); - } - - private Rotation2d getEncoderPos18t() { - return Rotation2d.fromRotations(this.encoder18t.getAbsolutePosition().getValueAsDouble()); - } - - public Rotation2d getVectorSpaceAngle() { - return TurretAngleCalculator.getAbsoluteAngle(getEncoderPos17t().getDegrees(), getEncoderPos18t().getDegrees()); - } - - public void zeroEncoders() { - double encoderPos17T = encoder17t.getAbsolutePosition().getValueAsDouble(); - double encoderPos18T = encoder18t.getAbsolutePosition().getValueAsDouble(); - - encoder17t.getConfigurator().refresh(Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor); - encoder18t.getConfigurator().refresh(Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor); - - double currentOffset17T = Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor.MagnetOffset; - double currentOffset18T = Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor.MagnetOffset; - - double newOffset17T = currentOffset17T - encoderPos17T; - double newOffset18T = currentOffset18T - encoderPos18T; - - Motors.Turret.ENCODER_17T.withMagnetOffset(newOffset17T); - Motors.Turret.ENCODER_18T.withMagnetOffset(newOffset18T); - - Motors.Turret.ENCODER_17T.configure(encoder17t); - Motors.Turret.ENCODER_18T.configure(encoder18t); - } - - public void seedTurret() { - motor.setPosition(getVectorSpaceAngle().getRotations()); - } - - @Override - public Rotation2d getAngle() { - return Rotation2d.fromDegrees(motor.getPosition().getValueAsDouble()); - } - - @Override - public boolean atTargetAngle() { - return Math.abs(getAngle().minus(getTargetAngle()).getDegrees() + 180.0) < Settings.Turret.TOLERANCE_DEG; - } - - private double getDelta(double target, double current) { - double delta = (target - current) % 360; - - if (delta > 180.0) delta -= 360; - else if (delta < -180) delta += 360; - - if (Math.abs(current + delta) < Constants.RANGE) return delta; - - return delta < 0 ? delta + 360 : delta - 360; - } - - @Override - public void periodic() { - super.periodic(); - - if (!hasUsedAbsoluteEncoder) { - motor.setPosition(getVectorSpaceAngle().getRotations()); - hasUsedAbsoluteEncoder = true; - System.out.println("Absolute Encoder Reset triggered"); - } - - double currentAngle = getAngle().getDegrees(); - double actualTargetDeg = currentAngle + getDelta(getTargetAngle().getDegrees(), currentAngle); - - SmartDashboard.putNumber("Turret/Delta (deg)", getDelta(getTargetAngle().getDegrees(), getAngle().getDegrees())); - SmartDashboard.putNumber("Turret/Actual Target (deg)", actualTargetDeg); - - if (EnabledSubsystems.TURRET.get() && getState() != TurretState.IDLE) { - if (voltageOverride.isPresent()) { - motor.setVoltage(voltageOverride.get()); - } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0)); - } - } else { - motor.stopMotor(); - } - - if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Vector Space Position (Rot)", getVectorSpaceAngle().getRotations()); - SmartDashboard.putNumber("Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); - SmartDashboard.putNumber("Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); - SmartDashboard.putNumber("Turret/Error", motor.getClosedLoopError().getValueAsDouble() * 360.0); - } - } - - @Override - public SysIdRoutine getSysIdRoutine() { - return SysId.getRoutine( - 2, - 6, - "Turret", - voltage -> setVoltageOverride(Optional.of(voltage)), - () -> this.motor.getPosition().getValueAsDouble(), - () -> this.motor.getVelocity().getValueAsDouble(), - () -> this.motor.getMotorVoltage().getValueAsDouble(), - getInstance()); - } - - private void setVoltageOverride(Optional volts) { - this.voltageOverride = volts; - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java index 79efc83d..1b035c46 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/LimelightVision.java @@ -12,11 +12,14 @@ import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; import com.stuypulse.robot.util.vision.LimelightHelpers; +import com.stuypulse.robot.util.vision.LimelightHelpers.IMUData; import com.stuypulse.robot.util.vision.LimelightHelpers.PoseEstimate; 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.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructArrayPublisher; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -34,8 +37,10 @@ public static LimelightVision getInstance() { private String[] names; private SmartBoolean enabled; - private SmartBoolean[] camerasEnabled; + // private SmartBoolean[] camerasEnabled; private MegaTagMode megaTagMode; + private Pose2d[] arrayOfLimelightPoses; + private final StructArrayPublisher arrayPublisher; public enum MegaTagMode { MEGATAG1, @@ -43,7 +48,12 @@ public enum MegaTagMode { } public LimelightVision() { + arrayPublisher = NetworkTableInstance.getDefault() + .getStructArrayTopic("MyPoseArray", Pose2d.struct).publish(); + + arrayOfLimelightPoses = new Pose2d[Cameras.LimelightCameras.length]; names = new String[Cameras.LimelightCameras.length]; + for (int i = 0; i < Cameras.LimelightCameras.length; i++) { names[i] = Cameras.LimelightCameras[i].getName(); Pose3d robotRelativePose = Cameras.LimelightCameras[i].getLocation(); @@ -56,15 +66,17 @@ public LimelightVision() { Rotation2d.fromRadians(robotRelativePose.getRotation().getY()).getDegrees(), Rotation2d.fromRadians(robotRelativePose.getRotation().getZ()).getDegrees() ); + + arrayOfLimelightPoses[i] = new Pose2d(); } - camerasEnabled = new SmartBoolean[Cameras.LimelightCameras.length]; + // camerasEnabled = new SmartBoolean[Cameras.LimelightCameras.length]; - for (int i = 0; i < camerasEnabled.length; i++) { - camerasEnabled[i] = new SmartBoolean("Vision/" + names[i] + " Is Enabled", true); - LimelightHelpers.SetIMUMode(names[i], 0); - SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); - } + // for (int i = 0; i < camerasEnabled.length; i++) { + // camerasEnabled[i] = new SmartBoolean("Vision/" + names[i] + " Is Enabled", true); + // LimelightHelpers.SetIMUMode(names[i], Settings.Vision.INTERNAL_EXTERNAL_ASSIST_INDEX); + // SmartDashboard.putBoolean("Vision/" + names[i] + " Has Data", false); + // } enabled = new SmartBoolean("Vision/Is Enabled", true); megaTagMode = MegaTagMode.MEGATAG1; @@ -84,14 +96,6 @@ public void disable() { enabled.set(false); } - public void setCameraEnabled(String name, boolean enabled) { - for (int i = 0; i < names.length; i++) { - if (names[i].equals(name)) { - camerasEnabled[i].set(enabled); - } - } - } - public void setMegaTagMode(MegaTagMode mode) { this.megaTagMode = mode; } @@ -102,11 +106,35 @@ public void setIMUMode(int mode) { } } + /** + * Allows you to set the convergence speed of the internal LL IMU and robot gyro. + * + * @param assistValue, an double that sets the correction speed of the complementary filter for the IMU. IMU Mode 4 + * uses the fusing of the internal IMU (1khz) with the external gyro reading as well. Higher values ranging towards 1 + * indicate a faster convergence of internal IMU to the robot IMU mode. Defaults to 0.001. + */ + public void setIMUAssistValue(double assistValue) { + for (String name : names) { + LimelightHelpers.SetIMUAssistAlpha(name, assistValue); + } + } + + public IMUData[] getIMUData() { + IMUData[] data = new IMUData[Cameras.LimelightCameras.length]; + + for (int i = 0; i < Cameras.LimelightCameras.length; i++) { + data[i] = LimelightHelpers.getIMUData(Cameras.LimelightCameras[i].getName()); + } + + return data; + } + + @Override public void periodic() { if (enabled.get()) { for (int i = 0; i < names.length; i++) { - if (camerasEnabled[i].get()) { + if (Cameras.LimelightCameras[i].isEnabled()) { String limelightName = names[i]; // Seed robot heading (used by MT2) @@ -138,9 +166,15 @@ public void periodic() { if (poseEstimate != null && poseEstimate.tagCount > 0) { Pose2d robotPose = poseEstimate.pose; double timestamp = poseEstimate.timestampSeconds; - - CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); - + + if (megaTagMode == MegaTagMode.MEGATAG1) { + CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT1_STDEVS); + } else { + CommandSwerveDrivetrain.getInstance().addVisionMeasurement(robotPose, timestamp, Settings.Vision.MT2_STDEVS); + } + + arrayOfLimelightPoses[i] = robotPose; + SmartDashboard.putNumber("Vision/Pose X Component", robotPose.getX()); SmartDashboard.putNumber("Vision/Pose Y Component", robotPose.getY()); SmartDashboard.putNumber("Vision/Pose Theta (Degrees)", robotPose.getRotation().getDegrees()); @@ -151,8 +185,25 @@ public void periodic() { } SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + // this yaw is seems to be the robot yaw passed into the LL + SmartDashboard.putNumber("Vision/Limelight Robot Yaw", LimelightHelpers.getIMUData(limelightName).robotYaw); + // this is just the yaw of the internal imu + SmartDashboard.putNumber("Vision/Limelight Yaw", LimelightHelpers.getIMUData(limelightName).Yaw); + + } + if (Settings.DEBUG_MODE) { + String limelightName = names[i]; + SmartDashboard.putString("Vision/MegaTag Mode", megaTagMode.toString()); + // this yaw is seems to be the robot yaw passed into the LL + SmartDashboard.putNumber("Vision/Limelight Robot Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).robotYaw); + // this is just the yaw of the internal imu + SmartDashboard.putNumber("Vision/Limelight Yaw " + limelightName, LimelightHelpers.getIMUData(limelightName).Yaw); + SmartDashboard.putNumber("Vision/Limelight Robot Yaw Passed in", (CommandSwerveDrivetrain.getInstance().getPose().getRotation().getDegrees() + (Robot.isBlue() ? 0 : 180)) % 360); } } + if (Settings.DEBUG_MODE) { + arrayPublisher.set(arrayOfLimelightPoses); + } } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/FMSUtil.java b/src/main/java/com/stuypulse/robot/util/FMSUtil.java new file mode 100644 index 00000000..8158e407 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/FMSUtil.java @@ -0,0 +1,126 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.Optional; + +public class FMSUtil { + + private final Timer timer = new Timer(); + private boolean autoMode; + private boolean autoOverride = false; + + public enum FieldState { + AUTO(0.0, 20.0), + TRANSITION(0.0, 10.0), + SHIFT_1(10.0, 35.0), + SHIFT_2(35.0, 60.0), + SHIFT_3(60.0, 85.0), + SHIFT_4(85.0, 110.0), + ENDGAME(110.0, 140.0); + + private final double startTime; + private final double endTime; + + FieldState(double startTime, double endTime) { + this.startTime = startTime; + this.endTime = endTime; + } + + public boolean isActive(double time) { + return startTime <= time && time < endTime; + } + + public double timeLeft(double time) { + return Math.max(0.0, endTime - time); + } + + public double timeElapsed(double time) { + return Math.max(0.0, time - startTime); + } + } + + public FMSUtil(boolean autoMode) { + this.autoMode = autoMode; + timer.start(); + } + + public FMSUtil() { + this(true); + } + + public void restartTimer(boolean autoMode) { + timer.restart(); + this.autoMode = autoMode; + } + + public FieldState getCurrentFieldState() { + if (autoMode) { + return FieldState.AUTO; + } + + double time = timer.get(); + + for (FieldState state : FieldState.values()) { + if (state != FieldState.AUTO && state.isActive(time)) { + return state; + } + } + + return FieldState.ENDGAME; + } + + public boolean isActiveShift() { + boolean wonAuto = didWinAuto(); + + switch (getCurrentFieldState()) { + case AUTO: + return true; + case TRANSITION: + return true; + case ENDGAME: + return true; + case SHIFT_1: + return wonAuto; + case SHIFT_2: + return !wonAuto; + case SHIFT_3: + return wonAuto; + case SHIFT_4: + return !wonAuto; + default: + return false; + } + } + + public void overrideFMSAutoVictor(boolean didWin) { + this.autoOverride = didWin; + } + + public boolean didWinAuto() { + String winner = DriverStation.getGameSpecificMessage(); + Optional allianceOpt = DriverStation.getAlliance(); + + if (winner == null || winner.isEmpty() || allianceOpt.isEmpty()) { + DriverStation.reportWarning("No FMS auto winner data available", false); + SmartDashboard.putBoolean("FMSUtil/No Auto Winner Data", true); + return autoOverride; + } + + String allianceLetter = allianceOpt.get() == Alliance.Blue ? "B" : "R"; + + return allianceLetter.equalsIgnoreCase(winner); + } + + public double getTimeLeftInShift() { + return getCurrentFieldState().timeLeft(timer.get()); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/SettableNumber.java b/src/main/java/com/stuypulse/robot/util/SettableNumber.java new file mode 100644 index 00000000..b458bf3a --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/SettableNumber.java @@ -0,0 +1,42 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.util; + +public class SettableNumber extends Number { + private double value; + + public SettableNumber(double value) { + this.value = value; + } + + public void set(double value) { + this.value = value; + } + + public double get() { + return value; + } + + @Override + public int intValue() { + return (int) value; + } + + @Override + public long longValue() { + return (long) value; + } + + @Override + public float floatValue() { + return (float) value; + } + + @Override + public double doubleValue() { + return (double) value; + } +} diff --git a/src/main/java/com/stuypulse/robot/util/SpindexerInterpolation.java b/src/main/java/com/stuypulse/robot/util/SpindexerInterpolation.java deleted file mode 100644 index cb83b1b0..00000000 --- a/src/main/java/com/stuypulse/robot/util/SpindexerInterpolation.java +++ /dev/null @@ -1,28 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.util; - -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; - -public class SpindexerInterpolation { - private static final InterpolatingDoubleTreeMap interpolatingDoubleTreeMap; - - private static final double[][] rpmAndDistance = { - /* { RPM, Distance (m) } */ - {1,1} - }; - - static { - interpolatingDoubleTreeMap = new InterpolatingDoubleTreeMap(); - for (double[] data: rpmAndDistance) { - interpolatingDoubleTreeMap.put(data[1], data[0]); - } - } - - public static double getRPM(double distanceInMeters){ - return interpolatingDoubleTreeMap.get(distanceInMeters); - } -} diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java deleted file mode 100644 index 551f1ac7..00000000 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ /dev/null @@ -1,160 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.util.hoodedshooter; -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Field; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; -import com.stuypulse.robot.constants.Settings.HoodedShooter.FerryRPMInterpolation; -import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.SOTMSolution; - -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.geometry.Twist2d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import java.util.function.Supplier; - -public class HoodAngleCalculator { - - public static SOTMSolution sol; - - private static FieldObject2d hubPose2d; - private static FieldObject2d virtualHubPose2d; - private static FieldObject2d futureTurretPose2d; - - public static InterpolatingDoubleTreeMap distanceAngleInterpolator; - public static InterpolatingDoubleTreeMap distanceRPMInterpolator; - public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; - - static { - distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { - distanceAngleInterpolator.put(pair[0], pair[1]); - } - } - - static { - distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { - distanceRPMInterpolator.put(pair[0], pair[1]); - } - } - - static { - ferryingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap(); - for(double[] pair: FerryRPMInterpolation.distanceRPMInterpolationValues) { - ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); - } - } - - public static Supplier interpolateHoodAngle() { - return () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - - Translation2d hubPose = Field.getHubPose().getTranslation(); - Translation2d currentPose = swerve.getTurretPose().getTranslation(); - - double distanceMeters = hubPose.getDistance(currentPose); - - Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); - - SmartDashboard.putNumber("HoodedShooter/Interpolated Target Angle", targetAngle.getDegrees()); - - return targetAngle; - }; - } - - public static Supplier interpolateShooterRPM() { - return () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - - Pose2d hubPose = Field.getHubPose(); - Pose2d turretPose = swerve.getTurretPose(); - - double targetRPM = ShotCalculator.solveInterpolation(turretPose, hubPose).targetRPM(); - - SmartDashboard.putNumber("HoodedShooter/Interpolated RPM", targetRPM); - - return targetRPM; - }; - } - - public static Supplier interpolateFerryingRPM() { - return () -> { - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - - Translation2d currentPose = swerve.getTurretPose().getTranslation(); - Translation2d cornerPose = Field.getFerryZonePose(currentPose).getTranslation(); - - double distanceMeters = cornerPose.getDistance(currentPose); - - double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); - - SmartDashboard.putNumber("HoodedShooter/Interpolated Ferrying RPM", targetRPM); - - return targetRPM; - }; - } - - public static void updateSOTMSolution() { - - CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - - Pose2d robotPose = swerve.getPose(); - Pose2d hubPose = Field.getHubPose(); - - ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); - ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( - robotRelativeSpeeds, - robotPose.getRotation() - ); - - Pose2d futureTurretPose = swerve.getTurretPose().exp( - new Twist2d( - robotRelativeSpeeds.vxMetersPerSecond * Settings.ShootOnTheFly.UPDATE_DELAY.doubleValue(), - robotRelativeSpeeds.vyMetersPerSecond * Settings.ShootOnTheFly.UPDATE_DELAY.doubleValue(), - 0 - ) - ); - - - SOTMSolution solution = ShotCalculator.solveShootOnTheMove( - futureTurretPose, - robotPose, - hubPose, - fieldRelativeSpeeds, - Settings.ShootOnTheFly.MAX_ITERATIONS, - Settings.ShootOnTheFly.TIME_TOLERANCE - ); - - sol = solution; - - hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); - virtualHubPose2d.setPose((Robot.isBlue() ? sol.virtualPose() : Field.transformToOppositeAlliance(sol.virtualPose()))); - futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); - - - SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Turret Angle", sol.targetTurretAngle().getDegrees()); - SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Hood Angle", sol.targetHoodAngle().getDegrees()); - SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Flight time", sol.flightTime()); - SmartDashboard.putNumber("HoodedShooter/SOTM/Turret Dist to Virtual Pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); - } - - public static Supplier calculateHoodAngleSOTM() { - return () -> sol.targetHoodAngle(); - } - - public static Supplier calculateTurretAngleSOTM() { - return () -> sol.targetTurretAngle(); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java deleted file mode 100644 index 0d5930cd..00000000 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java +++ /dev/null @@ -1,164 +0,0 @@ -/************************ PROJECT TRIBECBOT *************************/ -/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ -/* Use of this source code is governed by an MIT-style license */ -/* that can be found in the repository LICENSE file. */ -/***************************************************************/ -package com.stuypulse.robot.util.hoodedshooter; - -import com.stuypulse.robot.Robot; -import com.stuypulse.robot.constants.Settings; -import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; -import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; - -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.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.math.kinematics.ChassisSpeeds; - -public final class ShotCalculator { - public static final double g = 9.81; - - public static InterpolatingDoubleTreeMap distanceAngleInterpolator; - public static InterpolatingDoubleTreeMap distanceRPMInterpolator; - - static { - distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { - distanceAngleInterpolator.put(pair[0], pair[1]); - } - } - - static { - distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); - for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { - distanceRPMInterpolator.put(pair[0], pair[1]); - } - } - - public record StationarySolution( - Rotation2d targetHoodAngle, - double targetRPM, - double flightTimeSeconds) { - } - - public static StationarySolution solveInterpolation(Pose2d turretPose, Pose2d targetPose) { - - double distanceMeters = turretPose.getTranslation().getDistance(targetPose.getTranslation()); - - // Interpolated Angle - Rotation2d targetHoodAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); - - // Interpolated RPM - double targetRPM = distanceRPMInterpolator.get(distanceMeters); - - // Physics-based TOF - double launchSpeed = 0.5 * targetRPM * (2 * Math.PI / 60.0) * Settings.HoodedShooter.Shooter.FLYWHEEL_RADIUS; - Rotation2d launchAngle = Rotation2d.kCCW_Pi_2.minus(targetHoodAngle); - - double v_x = launchSpeed * Math.cos(launchAngle.getRadians()); - double flightTime = distanceMeters / v_x; - - return new StationarySolution( - targetHoodAngle, - targetRPM, - flightTime - ); - } - - public record SOTMSolution( - Rotation2d targetHoodAngle, - Rotation2d targetTurretAngle, - Pose2d virtualPose, - double flightTime) { - } - - public static SOTMSolution solveShootOnTheMove( - Pose2d turretPose, - Pose2d robotPose, - Pose2d targetPose, - ChassisSpeeds fieldRelativeSpeeds, - int maxIterations, - double timeTolerance) { - - /* - * Start with v_ball * flightTime = distanceToTargetPose. - * - * We know that v_ball = v_robot + v_shooter, so - * (v_robot + v_shooter) * flightTime = distanceToTargetPose - * - * Rearranging, we can get - * (v_shooter) * flight_time = distanceToTargetPose - v_robot * flightTime - * - * So we can instead shoot at a virtual pose and treat the robot as stationary: - * distanceToVirtualPose = distanceToTargetPose - v_robot * flightTime - * (v_shooter) * flight_time = distanceToVirtualPose - * - * Looking at the first equation, we can find the virtual pose with the flight time, - * but looking at the second equation, to get the flight time we need to solveBallisticWithSpeed() - * using the virtual pose, so we have a circular dependence. - * - * Thus, we can make an initial guess for the flight time: the flight time if the robot were stationary - * We want our guess to converge such that the left side equals the right side: - * (v_shooter) * t_guess = distanceToVirtualPose = distance - v_robot * t_guess, which would make t_guess = flightTime - * - * We do the right side first using our inital guess, and then update t_guess with a new guess by - * calculating the flightTime to that virtualPose. - * - * The pose is that the flightTime converges within maxIterations. - */ - - StationarySolution sol = solveInterpolation( - turretPose, - targetPose - ); - - double t_guess = sol.flightTimeSeconds(); - - Pose2d virtualPose = targetPose; - - - for (int i = 0; i < maxIterations; i++) { - - double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; - double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; - - virtualPose = new Pose2d( - targetPose.getX() - dx, - targetPose.getY() - dy, - targetPose.getRotation()); - - StationarySolution newSol = solveInterpolation( - turretPose, - virtualPose - ); - - if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { - break; - } - - t_guess = newSol.flightTimeSeconds(); - - sol = newSol; - } - - Translation2d virtualTranslation = virtualPose.getTranslation(); - Translation2d turretTranslation = turretPose.getTranslation(); - - double yaw = Math.atan2( - virtualTranslation.getY() - turretTranslation.getY(), - virtualTranslation.getX() - turretTranslation.getX() - ); - - Rotation2d targetTurretAngle = Robot.isReal() ? - Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : - Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); - - return new SOTMSolution( - sol.targetHoodAngle(), - targetTurretAngle, - virtualPose, - sol.flightTimeSeconds() - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java new file mode 100644 index 00000000..ac3c0113 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/superstructure/InterpolationCalculator.java @@ -0,0 +1,128 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.util.superstructure; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Superstructure.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.FerryRPMInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.RPMInterpolation; +import com.stuypulse.robot.constants.Settings.Superstructure.TOFInterpolation; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; + +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.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class InterpolationCalculator { + + public static InterpolatingDoubleTreeMap distanceAngleInterpolator; + public static InterpolatingDoubleTreeMap distanceRPMInterpolator; + public static InterpolatingDoubleTreeMap distanceTOFInterpolator; + + public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; + + public record InterpolatedShotInfo( + Rotation2d targetHoodAngle, + double targetRPM, + double flightTimeSeconds) { + } + + public record InterpolatedFerryInfo( + Rotation2d targetHoodAngle, + double targetRPM, + double flightTimeSeconds) { + } + + + static { + distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { + distanceAngleInterpolator.put(pair[0], pair[1]); + } + + distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { + distanceRPMInterpolator.put(pair[0], pair[1]); + } + + distanceTOFInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : TOFInterpolation.distanceTOFInterpolationValues) { + distanceTOFInterpolator.put(pair[0], pair[1]); + } + + ferryingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for(double[] pair: FerryRPMInterpolation.ferryDistanceRPMInterpolation) { + ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); + } + } + + public static InterpolatedShotInfo interpolateShotInfo(){ + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + return interpolateShotInfo(swerve.getTurretPose(), Field.getHubPose()); + } + + + public static InterpolatedShotInfo interpolateShotInfo(Pose2d turretPose, Pose2d targetPose) { + Translation2d hubPose = targetPose.getTranslation(); + Translation2d currentPose = turretPose.getTranslation(); + + double distanceMeters = currentPose.getDistance(hubPose); + + Rotation2d targetAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); + double targetRPM = distanceRPMInterpolator.get(distanceMeters); + double flightTime = distanceTOFInterpolator.get(distanceMeters); + + + SmartDashboard.putNumber("InterpolationTesting/Interpolated Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("InterpolationTesting/Interpolated RPM", targetRPM); + SmartDashboard.putNumber("InterpolationTesting/Interpolated TOF", flightTime); + + return new InterpolatedShotInfo( + targetAngle, + targetRPM, + flightTime + ); + } + + + public static InterpolatedFerryInfo interpolateFerryingInfo() { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + Pose2d turretPose = swerve.getTurretPose(); + + return interpolateFerryingInfo( + turretPose, + Field.getFerryZonePose(turretPose.getTranslation()) + ); + } + + public static InterpolatedFerryInfo interpolateFerryingInfo(Pose2d turretPose, Pose2d targetPose) { + + Translation2d currentPose = turretPose.getTranslation(); + Translation2d ferryPose = targetPose.getTranslation(); + + double distanceMeters = currentPose.getDistance(ferryPose); + + Rotation2d targetAngle = Rotation2d.fromDegrees(Settings.Superstructure.Hood.Angles.FERRY.getAsDouble()); + double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); + double flightTime = 2.1; + + SmartDashboard.putNumber("Superstructure/Interpolated Ferry Target Angle", targetAngle.getDegrees()); + SmartDashboard.putNumber("Superstructure/Interpolated Ferry RPM", targetRPM); + SmartDashboard.putNumber("Superstructure/Interpolated Ferry TOF", flightTime); + + return new InterpolatedFerryInfo( + targetAngle, + targetRPM, + flightTime + ); + } + + +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java new file mode 100644 index 00000000..9c5d77ae --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/superstructure/SOTMCalculator.java @@ -0,0 +1,348 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.util.superstructure; + + +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.superstructure.hood.Hood; +import com.stuypulse.robot.subsystems.superstructure.shooter.Shooter; +import com.stuypulse.robot.subsystems.superstructure.turret.Turret; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedFerryInfo; +import com.stuypulse.robot.util.superstructure.InterpolationCalculator.InterpolatedShotInfo; + +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.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class SOTMCalculator { + public static final double g = 9.81; + + public static MoveSolution hubSol; + public static MoveSolution ferrySol; + + private static FieldObject2d hubPose2d; + private static FieldObject2d virtualHubPose2d; + private static FieldObject2d futureTurretPose2d; + + private static FieldObject2d ferryPose2d; + private static FieldObject2d virtualFerryPose2d; + + public record MoveSolution( + Rotation2d targetHoodAngle, + Rotation2d targetTurretAngle, + double targetShooterRPM, + Pose2d virtualPose, + double flightTime) { + } + + static { + hubSol = new MoveSolution( + Hood.getInstance().getAngle(), + Turret.getInstance().getAngle(), + Shooter.getInstance().getRPM(), + Field.getHubPose(), + 0.0 + ); + + ferrySol = new MoveSolution( + Hood.getInstance().getAngle(), + Turret.getInstance().getAngle(), + Shooter.getInstance().getRPM(), + Field.getFerryZonePose(CommandSwerveDrivetrain.getInstance().getPose().getTranslation()), + 0.0 + ); + + hubPose2d = Field.FIELD2D.getObject("hubPose"); + virtualHubPose2d = Field.FIELD2D.getObject("virtualHubPose"); + + ferryPose2d = Field.FIELD2D.getObject("ferryPose"); + virtualFerryPose2d = Field.FIELD2D.getObject("virtualFerryPose"); + + futureTurretPose2d = Field.FIELD2D.getObject("futureTurretPose"); + } + + + public static MoveSolution solveSOTM( + Pose2d turretPose, + Pose2d targetPose, + ChassisSpeeds fieldRelativeSpeeds, + int maxIterations, + double timeTolerance) { + + /* + Start with v_ball * flightTime = distanceToTargetPose. + + We know that v_ball = v_robot + v_shooter, so + (v_robot + v_shooter) * flightTime = distanceToTargetPose + + Rearranging, we can get + (v_shooter) * flight_time = distanceToTargetPose - v_robot * flightTime + + So we can instead shoot at a virtual pose and treat the robot as stationary: + distanceToVirtualPose = distanceToTargetPose - v_robot * flightTime + (v_shooter) * flight_time = distanceToVirtualPose + + Looking at the first equation, we can find the virtual pose with the flight time, + but looking at the second equation, to get the flight time we need to solveBallisticWithSpeed() + using the virtual pose, so we have a circular dependence. + + Thus, we can make an initial guess for the flight time: the flight time if the robot were stationary + We want our guess to converge such that the left side equals the right side: + (v_shooter) * t_guess = distanceToVirtualPose = distance - v_robot * t_guess, which would make t_guess = flightTime + + We do the right side first using our inital guess, and then update t_guess with a new guess by + calculating the flightTime to that virtualPose. + + The pose is that the flightTime converges within maxIterations. + */ + + + InterpolatedShotInfo sol = InterpolationCalculator.interpolateShotInfo(turretPose, targetPose); + + + double t_guess = sol.flightTimeSeconds(); + + Pose2d virtualPose = targetPose; + + + for (int i = 0; i < maxIterations; i++) { + + SmartDashboard.putNumber("Superstructure/SOTM/iteration #", i); + + double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; + double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; + + virtualPose = new Pose2d( + targetPose.getX() - dx, + targetPose.getY() - dy, + targetPose.getRotation()); + + + InterpolatedShotInfo newSol = InterpolationCalculator.interpolateShotInfo(turretPose, virtualPose); + + if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { + break; + } + + t_guess = newSol.flightTimeSeconds(); + + sol = newSol; + + } + + Translation2d virtualTranslation = virtualPose.getTranslation(); + Translation2d turretTranslation = turretPose.getTranslation(); + + // double yaw = Math.atan2( + // virtualTranslation.getY() - turretTranslation.getY(), + // virtualTranslation.getX() - turretTranslation.getX() + // ); + + // Rotation2d targetTurretAngle = Robot.isReal() ? + // Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : + // Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); + + return new MoveSolution( + sol.targetHoodAngle(), + TurretAngleCalculator.getPointAtTargetAngle(virtualTranslation, turretTranslation), + sol.targetRPM(), + virtualPose, + sol.flightTimeSeconds() + ); + } + + public static MoveSolution solveFOTM( + Pose2d turretPose, + Pose2d targetPose, + ChassisSpeeds fieldRelativeSpeeds, + int maxIterations, + double timeTolerance) { + + InterpolatedFerryInfo sol = InterpolationCalculator.interpolateFerryingInfo(turretPose, targetPose); + + + double t_guess = sol.flightTimeSeconds(); + + Pose2d virtualPose = targetPose; + + + for (int i = 0; i < maxIterations; i++) { + + SmartDashboard.putNumber("Superstructure/SOTM/iteration #", i); + + double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; + double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; + + virtualPose = new Pose2d( + targetPose.getX() - dx, + targetPose.getY() - dy, + targetPose.getRotation()); + + + InterpolatedFerryInfo newSol = InterpolationCalculator.interpolateFerryingInfo(turretPose, virtualPose); + + if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { + break; + } + + t_guess = newSol.flightTimeSeconds(); + + sol = newSol; + + } + + Translation2d virtualTranslation = virtualPose.getTranslation(); + Translation2d turretTranslation = turretPose.getTranslation(); + + return new MoveSolution( + sol.targetHoodAngle(), + TurretAngleCalculator.getPointAtTargetAngle(virtualTranslation, turretTranslation), + sol.targetRPM(), + virtualPose, + sol.flightTimeSeconds() + ); + } + + public static void updateSOTMSolution() { + + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Pose2d turretPose = swerve.getTurretPose(); + Pose2d robotPose = swerve.getPose(); + Pose2d hubPose = Field.getHubPose(); + + ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); + ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + robotRelativeSpeeds, + robotPose.getRotation() + ); + + Transform2d robotToTurret = turretPose.minus(robotPose); + + Pose2d futureTurretPose = robotPose.exp( + new Twist2d( + robotRelativeSpeeds.vxMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + robotRelativeSpeeds.vyMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + 0 + ) + ).transformBy(robotToTurret); + + + // Vector2D oppositeDirection = new Vector2D(new Translation2d( + // -fieldRelativeSpeeds.vxMetersPerSecond, + // -fieldRelativeSpeeds.vyMetersPerSecond + // )); + + // if (oppositeDirection.magnitude() < Settings.Swerve.MODULE_VELOCITY_DEADBAND_M_PER_S) { + // oppositeDirection = Vector2D.kOrigin; + // } + // else { + // oppositeDirection = oppositeDirection.normalize(); + // } + + // hubPose = hubPose.exp( + // new Twist2d( + // oppositeDirection.x * Field.HUB_RADIUS, + // oppositeDirection.y * Field.HUB_RADIUS, + // 0 + // ) + // ); + + hubSol = solveSOTM( + futureTurretPose, + hubPose, + fieldRelativeSpeeds, + Settings.Superstructure.SOTM.MAX_ITERATIONS, + Settings.Superstructure.SOTM.TIME_TOLERANCE + ); + + hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); + virtualHubPose2d.setPose((Robot.isBlue() ? hubSol.virtualPose() : Field.transformToOppositeAlliance(hubSol.virtualPose()))); + futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); + + + SmartDashboard.putNumber("Superstructure/SOTM/calculated turret angle", hubSol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated hood angle", hubSol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/SOTM/calculated flight time", hubSol.flightTime()); + SmartDashboard.putNumber("Superstructure/SOTM/turret dist to virtual pose", futureTurretPose.getTranslation().getDistance(hubSol.virtualPose().getTranslation())); + } + + public static void updateFOTMSolution() { + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Pose2d turretPose = swerve.getTurretPose(); + Pose2d robotPose = swerve.getPose(); + Pose2d ferryPose = Field.getFerryZonePose(robotPose.getTranslation()); + + ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); + ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + robotRelativeSpeeds, + robotPose.getRotation() + ); + + Transform2d robotToTurret = turretPose.minus(robotPose); + + Pose2d futureTurretPose = robotPose.exp( + new Twist2d( + robotRelativeSpeeds.vxMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + robotRelativeSpeeds.vyMetersPerSecond * Settings.Superstructure.SOTM.UPDATE_DELAY.doubleValue(), + 0 + ) + ).transformBy(robotToTurret); + + ferrySol = solveFOTM( + futureTurretPose, + ferryPose, + fieldRelativeSpeeds, + Settings.Superstructure.SOTM.MAX_ITERATIONS, + Settings.Superstructure.SOTM.TIME_TOLERANCE + ); + + + ferryPose2d.setPose(Robot.isBlue() ? ferryPose : Field.transformToOppositeAlliance(ferryPose)); + virtualFerryPose2d.setPose((Robot.isBlue() ? ferrySol.virtualPose() : Field.transformToOppositeAlliance(ferrySol.virtualPose()))); + futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); + + SmartDashboard.putNumber("Superstructure/FOTM/calculated turret angle", ferrySol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/FOTM/calculated hood angle", ferrySol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("Superstructure/FOTM/calculated flight time", ferrySol.flightTime()); + SmartDashboard.putNumber("Superstructure/FOTM/turret dist to ferry pose", futureTurretPose.getTranslation().getDistance(ferrySol.virtualPose().getTranslation())); + } + + public static Rotation2d calculateHoodAngleSOTM() { + return hubSol.targetHoodAngle(); + } + + public static Rotation2d calculateTurretAngleSOTM() { + return hubSol.targetTurretAngle(); + } + + public static double calculateShooterRPMSOTM() { + return hubSol.targetShooterRPM(); + } + + public static Rotation2d calculateHoodAngleFOTM() { + // return ferrySol.targetHoodAngle(); + return Rotation2d.fromDegrees(39); + } + + public static Rotation2d calculateTurretAngleFOTM() { + return ferrySol.targetTurretAngle(); + } + + public static double calculateShooterRPMFOTM() { + return ferrySol.targetShooterRPM(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java similarity index 50% rename from src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java rename to src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java index 263ae98e..5454983f 100644 --- a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/TurretAngleCalculator.java @@ -3,17 +3,21 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.util.turret; +package com.stuypulse.robot.util.superstructure; +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; public class TurretAngleCalculator { - private static final double MAX_ANGLE_DEGREES = Settings.Turret.MAX_THEORETICAL_ROTATION.getDegrees(); - private static final double MIN_ANGLE_DEGREES = Settings.Turret.MIN_THEORETICAL_ROTATION.getDegrees(); - private static final double RESOLUTION = Settings.Turret.RESOLUTION_OF_ABSOLUTE_ENCODER; + private static final double MAX_ANGLE_DEGREES = Settings.Superstructure.Turret.MAX_THEORETICAL_ROTATION.getDegrees(); + private static final double MIN_ANGLE_DEGREES = Settings.Superstructure.Turret.MIN_THEORETICAL_ROTATION.getDegrees(); + private static final double RESOLUTION = Settings.Superstructure.Turret.RESOLUTION_OF_ABSOLUTE_ENCODER; private static final int NUM_POINTS = (int) ((MAX_ANGLE_DEGREES - MIN_ANGLE_DEGREES) / RESOLUTION); private static int leastDistanceIndex = 0; @@ -23,7 +27,7 @@ public class TurretAngleCalculator { private static double[] generateEncoderValues(int teeth) { double[] values = new double[NUM_POINTS]; - double gearRatio = 1.0 * Settings.Turret.Constants.BigGear.TEETH / teeth; + double gearRatio = 1.0 * Settings.Superstructure.Turret.BigGear.TEETH / teeth; int i = 0; for (double angle = MIN_ANGLE_DEGREES; angle < MAX_ANGLE_DEGREES; angle += RESOLUTION) { @@ -61,4 +65,36 @@ public static Rotation2d getAbsoluteAngle(double encoder17TValue, double encoder public static int lowestDistanceIndex() { return leastDistanceIndex; } + + public static Rotation2d getPointAtTargetAngle(Translation2d targetTranslation, Translation2d turretTranslation) { + + // Vector2D turret = new Vector2D(turretPose.getTranslation()); + // Vector2D target = new Vector2D(targetPose.getTranslation()); + + // Vector2D turretToTarget = target.sub(turret); + // Vector2D zeroVector = new Vector2D(robotPose.getRotation().getCos(), robotPose.getRotation().getSin()); + + // // https://www.youtube.com/watch?v=_VuZZ9_58Wg + // double crossProduct = zeroVector.x * turretToTarget.y - zeroVector.y * turretToTarget.x; + // double dotProduct = zeroVector.dot(turretToTarget); + + // Rotation2d targetAngle = (Robot.isReal() ? + // Rotation2d.fromRadians(-Math.atan2(crossProduct, dotProduct)) : + // Rotation2d.fromRadians(Math.atan2(crossProduct, dotProduct))); + + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Pose2d robotPose = swerve.getPose(); + + double yaw = Math.atan2( + targetTranslation.getY() - turretTranslation.getY(), + targetTranslation.getX() - turretTranslation.getX() + ); + + Rotation2d targetAngle = Robot.isReal() ? + Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : + Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); + + return targetAngle; + } } diff --git a/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerHood.java b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerHood.java new file mode 100644 index 00000000..bf785425 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerHood.java @@ -0,0 +1,73 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.util.superstructure; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class VisualizerHood { + + private static final VisualizerHood instance; + + static { + instance = new VisualizerHood(); + } + + public static VisualizerHood getInstance() { + return instance; + } + + private final Mechanism2d canvas; + private final double width, height; + + private final MechanismRoot2d hoodPivot; + + private final MechanismLigament2d hoodArm; + + private final MechanismLigament2d hoodTip; + + private VisualizerHood() { + width = Units.inchesToMeters(30); + height = Units.inchesToMeters(30); + + canvas = new Mechanism2d(width, height); + + hoodPivot = canvas.getRoot("Hood Pivot", width / 2, height * 0.2); + + hoodArm = new MechanismLigament2d( + "Hood Arm", + Units.inchesToMeters(12), + 0.0, + 6, + new Color8Bit(Color.kOrange) + ); + + hoodTip = new MechanismLigament2d( + "Hood Tip", + Units.inchesToMeters(3), + -90.0, + 4, + new Color8Bit(Color.kOrangeRed) + ); + + hoodPivot.append(hoodArm); + hoodArm.append(hoodTip); + } + + public void update(Rotation2d hoodAngle, boolean atTarget) { + hoodArm.setAngle(hoodAngle); + hoodArm.setColor(atTarget ? new Color8Bit(Color.kGreen) : new Color8Bit(Color.kRed)); + hoodTip.setColor(atTarget ? new Color8Bit(Color.kGreen) : new Color8Bit(Color.kRed)); + + SmartDashboard.putData("Visualizers/Hood", canvas); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/turret/TurretVisualizer.java b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerTurret.java similarity index 87% rename from src/main/java/com/stuypulse/robot/util/turret/TurretVisualizer.java rename to src/main/java/com/stuypulse/robot/util/superstructure/VisualizerTurret.java index 6271e0d6..6ebde597 100644 --- a/src/main/java/com/stuypulse/robot/util/turret/TurretVisualizer.java +++ b/src/main/java/com/stuypulse/robot/util/superstructure/VisualizerTurret.java @@ -3,7 +3,7 @@ /* Use of this source code is governed by an MIT-style license */ /* that can be found in the repository LICENSE file. */ /***************************************************************/ -package com.stuypulse.robot.util.turret; +package com.stuypulse.robot.util.superstructure; import edu.wpi.first.math.geometry.Rotation2d; @@ -15,14 +15,14 @@ import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj.util.Color8Bit; -public class TurretVisualizer { - public static TurretVisualizer instance; +public class VisualizerTurret { + public static VisualizerTurret instance; static { - instance = new TurretVisualizer(); + instance = new VisualizerTurret(); } - public static TurretVisualizer getInstance() { + public static VisualizerTurret getInstance() { return instance; } @@ -32,7 +32,7 @@ public static TurretVisualizer getInstance() { private final MechanismRoot2d turretPivot; private final MechanismLigament2d turret; - private TurretVisualizer() { + private VisualizerTurret() { width = Units.inchesToMeters(20); height = Units.inchesToMeters(20);