diff --git a/.vscode/settings.json b/.vscode/settings.json index 6cf194b..5c330e4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -18,9 +18,11 @@ { "name": "WPIlibUnitTests", "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ], + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" , + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" } }, @@ -67,5 +69,7 @@ }, "editor.formatOnSave": true, "editor.formatOnPaste": true, - "terminal.integrated.defaultProfile.windows": "Git Bash" + "terminal.integrated.defaultProfile.windows": "Git Bash", + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable", + "java.compile.nullAnalysis.mode": "disabled" } diff --git a/build.gradle b/build.gradle index 4e5ecde..541fbe4 100644 --- a/build.gradle +++ b/build.gradle @@ -82,7 +82,8 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() - annotationProcessor 'com.github.Frc5572:RobotTools:v1.1.1' + annotationProcessor 'com.github.Frc5572:RobotTools:v2.0' + implementation 'org.jspecify:jspecify:1.0.0' implementation 'org.apache.httpcomponents:httpclient:4.5.14' implementation 'org.apache.httpcomponents:httpmime:4.5.14' diff --git a/checks.xml b/checks.xml index 67cbaef..5845c26 100644 --- a/checks.xml +++ b/checks.xml @@ -1,7 +1,5 @@ - + - - + + - + - + - + - + default="checkstyle-suppressions.xml" /> + - + - - + + - + - + - + + value="\\u00(09|0(a|A)|0(c|C)|0(d|D)|22|27|5(C|c))|\\(0(10|11|12|14|15|42|47)|134)" /> + value="Consider using special escape sequence instead of octal value or Unicode escaped value." /> - - - + + + - - + + - + - + + value="LITERAL_TRY, LITERAL_FINALLY, LITERAL_IF, LITERAL_ELSE, LITERAL_SWITCH" /> + value="LITERAL_DO, LITERAL_ELSE, LITERAL_FOR, LITERAL_IF, LITERAL_WHILE" /> + OBJBLOCK, STATIC_INIT, RECORD_DEF, COMPACT_CTOR_DEF" /> - + + value="LITERAL_TRY, LITERAL_CATCH, LITERAL_FINALLY, LITERAL_IF, LITERAL_ELSE, + LITERAL_DO" /> - - + + + COMPACT_CTOR_DEF" /> - - + + + value="COMMA, SEMI, TYPECAST, LITERAL_IF, LITERAL_ELSE, + LITERAL_WHILE, LITERAL_DO, LITERAL_FOR, DO_WHILE" /> - - - - - - + + + + + + + SR_ASSIGN, STAR, STAR_ASSIGN, LITERAL_ASSERT, TYPE_EXTENSION_AND" /> + value="WhitespaceAround: ''{0}'' is not followed by whitespace. Empty blocks may only be represented as '{}' when not part of a multi-block statement (4.1.3)" /> - - - - - - - - + value="WhitespaceAround: ''{0}'' is not preceded with whitespace." /> + + + + + + + + - + COMPACT_CTOR_DEF" /> + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - + + value="Package name ''{0}'' must match pattern ''{1}''." /> - - + + + value="Type name ''{0}'' must match pattern ''{1}''." /> - + + value="Member name ''{0}'' must match pattern ''{1}''." /> - + + value="Parameter name ''{0}'' must match pattern ''{1}''." /> - + + value="Lambda parameter name ''{0}'' must match pattern ''{1}''." /> - + + value="Catch parameter name ''{0}'' must match pattern ''{1}''." /> - + + value="Local variable name ''{0}'' must match pattern ''{1}''." /> - + + value="Pattern variable name ''{0}'' must match pattern ''{1}''." /> - + + value="Class type name ''{0}'' must match pattern ''{1}''." /> - + + value="Record component name ''{0}'' must match pattern ''{1}''." /> - + + value="Record type name ''{0}'' must match pattern ''{1}''." /> - + + value="Method type name ''{0}'' must match pattern ''{1}''." /> - + + value="Interface type name ''{0}'' must match pattern ''{1}''." /> - + + value="GenericWhitespace ''{0}'' is followed by whitespace." /> + value="GenericWhitespace ''{0}'' is preceded with whitespace." /> + value="GenericWhitespace ''{0}'' should followed by whitespace." /> + value="GenericWhitespace ''{0}'' is not preceded with whitespace." /> - - - - - - + + + + + + - - + + + RECORD_COMPONENT_DEF" /> - - + + - + + value="CTOR_DEF, LITERAL_NEW, METHOD_CALL, METHOD_DEF, + SUPER_CTOR_CALL, ENUM_CONSTANT_DEF, RECORD_DEF" /> - + value="COMMA, SEMI, POST_INC, POST_DEC, DOT, + LABELED_STAT, METHOD_REF" /> + + RECORD_DEF" /> - + + TYPE_EXTENSION_AND " /> - + + value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF, + RECORD_DEF, COMPACT_CTOR_DEF" /> - - - + + + - - - + + + - + - + - + - + + value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF, VARIABLE_DEF" /> - - - - - + + + + + - - - - + + + + - + - + value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, + RECORD_DEF, ANNOTATION_DEF" /> + - + + value="Method name ''{0}'' must match pattern ''{1}''." /> - + - + - + - + default="checkstyle-xpath-suppressions.xml" /> + - + \ No newline at end of file diff --git a/gradle.properties b/gradle.properties new file mode 100644 index 0000000..c7df913 --- /dev/null +++ b/gradle.properties @@ -0,0 +1 @@ +org.gradle.jvmargs=-Xms512m -Xmx8g diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/limelight pipelines/2022_Shop_Targeting.vpr b/limelight pipelines/2022_Shop_Targeting.vpr deleted file mode 100644 index d59cb87..0000000 --- a/limelight pipelines/2022_Shop_Targeting.vpr +++ /dev/null @@ -1,68 +0,0 @@ -area_max:100 -area_min:0.0026873856000000015 -area_similarity:26 -aspect_max:20 -aspect_min:0 -black_level:40 -blue_balance:2500 -calibration_type:0 -contour_grouping:3 -contour_sort_final:0 -convexity_max:100 -convexity_min:80 -corner_approx:5.000000 -crop_x_max:1 -crop_x_min:-1 -crop_y_max:1.000000 -crop_y_min:-1.000000 -cross_a_a:1 -cross_a_x:0 -cross_a_y:0 -cross_b_a:1 -cross_b_x:0 -cross_b_y:0 -desc:Shop_Targeting -desired_contour_region:0 -dilation_steps:1 -direction_filter:0 -dual_close_sort_origin:0 -erosion_steps:0 -exposure:3 -force_convex:1 -hue_max:94 -hue_min:22 -image_flip:0 -image_source:0 -img_to_show:0 -intersection_filter:3 -invert_hue:0 -multigroup_max:5 -multigroup_min:2 -multigroup_rejector:1 -pipeline_led_enabled:1 -pipeline_led_power:100 -pipeline_res:0 -pipeline_type:0 -red_balance:2500 -roi_x:0.000000 -roi_y:0.000000 -sat_max:255 -sat_min:83 -send_corners:0 -send_raw_contours:0 -solve3d:0 -solve3d_algo:0 -solve3d_bindtarget:1 -solve3d_conf:0.990000 -solve3d_error:8 -solve3d_guess:0 -solve3d_iterations:50 -solve3d_precies:1 -solve3d_precise:0 -solve3d_zoffset:0.000000 -val_max:220 -val_min:18 -x_outlier_miqr:1 -y_max:1.000000 -y_min:-1.000000 -y_outlier_miqr:2.5 diff --git a/shuffleboard config/shuffleboard.json b/shuffleboard config/shuffleboard.json deleted file mode 100644 index 631b51d..0000000 --- a/shuffleboard config/shuffleboard.json +++ /dev/null @@ -1,868 +0,0 @@ -{ - "tabPane": [ - { - "title": "SmartDashboard", - "autoPopulate": true, - "autoPopulatePrefix": "SmartDashboard/", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "titleType": 0, - "tiles": { - "0,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/limelight_PipelineName", - "_title": "limelight_PipelineName" - } - }, - "1,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 2 Integrated", - "_title": "Mod 2 Integrated" - } - }, - "2,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "ComboBox Chooser", - "_source0": "network_table:///SmartDashboard/Choose Auto: ", - "_title": "Choose Auto: " - } - }, - "3,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 0 Velocity", - "_title": "Mod 0 Velocity" - } - }, - "4,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 3 Cancoder", - "_title": "Mod 3 Cancoder" - } - }, - "5,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 0 Cancoder", - "_title": "Mod 0 Cancoder" - } - }, - "6,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 0 Integrated", - "_title": "Mod 0 Integrated" - } - }, - "7,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/Magazine Switch", - "_title": "Magazine Switch", - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - } - }, - "8,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 3 Integrated", - "_title": "Mod 3 Integrated" - } - }, - "9,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 1 Velocity", - "_title": "Mod 1 Velocity" - } - }, - "10,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/limelight_Interface", - "_title": "limelight_Interface" - } - }, - "11,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 1 Cancoder", - "_title": "Mod 1 Cancoder" - } - }, - "12,0": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 1 Integrated", - "_title": "Mod 1 Integrated" - } - }, - "0,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 2 Velocity", - "_title": "Mod 2 Velocity" - } - }, - "1,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 2 Cancoder", - "_title": "Mod 2 Cancoder" - } - }, - "2,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/limelight_Stream", - "_title": "limelight_Stream" - } - }, - "3,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Mod 3 Velocity", - "_title": "Mod 3 Velocity" - } - }, - "4,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/SetPoint (RPM)", - "_title": "SetPoint (RPM)" - } - }, - "5,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/Slider 0", - "_title": "DB/Slider 0" - } - }, - "6,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/Slider 1", - "_title": "DB/Slider 1" - } - }, - "7,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/Slider 2", - "_title": "DB/Slider 2" - } - }, - "8,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/Slider 3", - "_title": "DB/Slider 3" - } - }, - "9,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/Auto Selector", - "_title": "Auto Selector" - } - }, - "10,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/DB/Button 0", - "_title": "DB/Button 0", - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - } - }, - "11,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/DB/Button 1", - "_title": "DB/Button 1", - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - } - }, - "12,1": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/DB/Button 2", - "_title": "DB/Button 2", - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - } - }, - "0,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/DB/Button 3", - "_title": "DB/Button 3", - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - } - }, - "1,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 0", - "_title": "DB/String 0" - } - }, - "2,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 1", - "_title": "DB/String 1" - } - }, - "3,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 2", - "_title": "DB/String 2" - } - }, - "4,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 3", - "_title": "DB/String 3" - } - }, - "5,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 4", - "_title": "DB/String 4" - } - }, - "6,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 5", - "_title": "DB/String 5" - } - }, - "7,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 6", - "_title": "DB/String 6" - } - }, - "8,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 7", - "_title": "DB/String 7" - } - }, - "9,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 8", - "_title": "DB/String 8" - } - }, - "10,2": { - "size": [ - 1, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///SmartDashboard/DB/String 9", - "_title": "DB/String 9" - } - } - } - } - }, - { - "title": "LiveWindow", - "autoPopulate": true, - "autoPopulatePrefix": "LiveWindow/", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "titleType": 0, - "tiles": { - "0,0": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/Ungrouped", - "_title": "Ungrouped", - "Layout/Label position": "BOTTOM", - "_children": [ - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[16]", - "_title": "PIDController[16]" - }, - { - "_type": "Text View", - "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[1,2]/Value", - "_title": "DoubleSolenoid[1,2]/Value" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[25]", - "_title": "PIDController[25]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[13]", - "_title": "PIDController[13]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[17]", - "_title": "PIDController[17]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[14]", - "_title": "PIDController[14]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[22]", - "_title": "PIDController[22]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[26]", - "_title": "PIDController[26]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[6]", - "_title": "PIDController[6]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[23]", - "_title": "PIDController[23]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[7]", - "_title": "PIDController[7]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[18]", - "_title": "PIDController[18]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[8]", - "_title": "PIDController[8]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[11]", - "_title": "PIDController[11]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[19]", - "_title": "PIDController[19]" - }, - { - "_type": "Gyro", - "_source0": "network_table:///LiveWindow/Ungrouped/navX-Sensor[4]", - "_title": "navX-Sensor[4]", - "Visuals/Major tick spacing": 45.0, - "Visuals/Starting angle": 180.0, - "Visuals/Show tick mark ring": true, - "Visuals/Counter clockwise": false - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[10]", - "_title": "PIDController[10]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[20]", - "_title": "PIDController[20]" - }, - { - "_type": "Boolean Box", - "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[1]/Value", - "_title": "DigitalInput[1]/Value", - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[5]", - "_title": "PIDController[5]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[15]", - "_title": "PIDController[15]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[24]", - "_title": "PIDController[24]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[4]", - "_title": "PIDController[4]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[9]", - "_title": "PIDController[9]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[12]", - "_title": "PIDController[12]" - }, - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[21]", - "_title": "PIDController[21]" - }, - { - "_type": "Text View", - "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[1,0]/Value", - "_title": "DoubleSolenoid[1,0]/Value" - }, - { - "_type": "Motor Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [11]", - "_title": "LiveWindow/Ungrouped/Talon FX [11]", - "Visuals/Orientation": "HORIZONTAL" - }, - { - "_type": "Motor Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [12]", - "_title": "LiveWindow/Ungrouped/Talon FX [12]", - "Visuals/Orientation": "HORIZONTAL" - }, - { - "_type": "Motor Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [9]", - "_title": "LiveWindow/Ungrouped/Talon FX [9]", - "Visuals/Orientation": "HORIZONTAL" - }, - { - "_type": "Motor Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/MotorControllerGroup[2]", - "_title": "LiveWindow/Ungrouped/MotorControllerGroup[2]", - "Visuals/Orientation": "HORIZONTAL" - }, - { - "_type": "Motor Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/MotorControllerGroup[1]", - "_title": "LiveWindow/Ungrouped/MotorControllerGroup[1]", - "Visuals/Orientation": "HORIZONTAL" - }, - { - "_type": "Motor Controller", - "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [10]", - "_title": "LiveWindow/Ungrouped/Talon FX [10]", - "Visuals/Orientation": "HORIZONTAL" - } - ] - } - }, - "2,0": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/OutsideClimber", - "_title": "OutsideClimber", - "Layout/Label position": "BOTTOM", - "_children": [] - } - }, - "4,0": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/InsideClimber", - "_title": "InsideClimber", - "Layout/Label position": "BOTTOM", - "_children": [] - } - }, - "6,0": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/OuterMagazine", - "_title": "OuterMagazine", - "Layout/Label position": "BOTTOM", - "_children": [ - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/OuterMagazine/PID Controller", - "_title": "PID Controller" - } - ] - } - }, - "8,0": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/LEDs", - "_title": "LEDs", - "Layout/Label position": "BOTTOM", - "_children": [] - } - }, - "10,0": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/Intake", - "_title": "Intake", - "Layout/Label position": "BOTTOM", - "_children": [] - } - }, - "0,4": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/InnerMagazine", - "_title": "InnerMagazine", - "Layout/Label position": "BOTTOM", - "_children": [ - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/InnerMagazine/PID Controller", - "_title": "PID Controller" - } - ] - } - }, - "2,4": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/Turret", - "_title": "Turret", - "Layout/Label position": "BOTTOM", - "_children": [] - } - }, - "4,4": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/Swerve", - "_title": "Swerve", - "Layout/Label position": "BOTTOM", - "_children": [] - } - }, - "6,4": { - "size": [ - 2, - 4 - ], - "content": { - "_type": "Subsystem Layout", - "_source0": "network_table:///LiveWindow/Shooter", - "_title": "Shooter", - "Layout/Label position": "BOTTOM", - "_children": [ - { - "_type": "PID Controller", - "_source0": "network_table:///LiveWindow/Shooter/PID Controller", - "_title": "PID Controller" - } - ] - } - } - } - } - }, - { - "title": "Tab 3", - "autoPopulate": false, - "autoPopulatePrefix": "", - "widgetPane": { - "gridSize": 128.0, - "showGrid": true, - "hgap": 16.0, - "vgap": 16.0, - "titleType": 0, - "tiles": { - "0,1": { - "size": [ - 7, - 4 - ], - "content": { - "_type": "Camera Stream", - "_source0": "camera_server://Magazine Camera", - "_title": "Magazine Camera", - "Crosshair/Show crosshair": true, - "Crosshair/Crosshair color": "#FFFFFFFF", - "Controls/Show controls": false, - "Controls/Rotation": "NONE", - "compression": -1.0, - "fps": -1, - "imageWidth": -1, - "imageHeight": -1 - } - }, - "7,1": { - "size": [ - 6, - 4 - ], - "content": { - "_type": "Camera Stream", - "_source0": "camera_server://limelight", - "_title": "limelight", - "Crosshair/Show crosshair": true, - "Crosshair/Crosshair color": "#FFFFFFFF", - "Controls/Show controls": false, - "Controls/Rotation": "NONE", - "compression": -1.0, - "fps": -1, - "imageWidth": -1, - "imageHeight": -1 - } - }, - "0,0": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "ComboBox Chooser", - "_source0": "network_table:///SmartDashboard/Choose Auto: ", - "_title": "SmartDashboard/Choose Auto: " - } - }, - "2,0": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Boolean Box", - "_source0": "network_table:///SmartDashboard/Magazine Switch", - "_title": "SmartDashboard/Magazine Switch", - "Colors/Color when true": "#7CFC00FF", - "Colors/Color when false": "#8B0000FF" - } - }, - "4,0": { - "size": [ - 2, - 1 - ], - "content": { - "_type": "Text View", - "_source0": "network_table:///LiveWindow/Ungrouped/navX-Sensor[4]/Value", - "_title": "LiveWindow/Ungrouped/navX-Sensor[4]/Value" - } - } - } - } - } - ], - "windowGeometry": { - "x": -8.0, - "y": -8.0, - "width": 1936.0, - "height": 1056.0 - } -} diff --git a/src/main/java/frc/lib/math/Conversions.java b/src/main/java/frc/lib/math/Conversions.java deleted file mode 100644 index 7871d53..0000000 --- a/src/main/java/frc/lib/math/Conversions.java +++ /dev/null @@ -1,94 +0,0 @@ -package frc.lib.math; - -/** - * Mathematical conversions for swerve calculations - */ -public class Conversions { - - /** - * @param counts Falcon Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Degrees of Rotation of Mechanism falconToDegrees - */ - public static double falconToDegrees(double counts, double gearRatio) { - return counts * (360.0 / (gearRatio * 2048.0)); - } - - /** - * @param degrees Degrees of rotation of Mechanism - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @return Falcon Counts degreesToFalcon - */ - public static double degreesToFalcon(double degrees, double gearRatio) { - double ticks = degrees / (360.0 / (gearRatio * 2048.0)); - return ticks; - } - - /** - * @param velocityCounts Falcon Velocity Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return RPM of Mechanism - */ - public static double falconToRPM(double velocityCounts, double gearRatio) { - double motorRPM = velocityCounts * (600.0 / 2048.0); - double mechRPM = motorRPM / gearRatio; - return mechRPM; - } - - /** - * @param rpm RPM of mechanism - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return RPM of Mechanism - */ - public static double rpmToFalcon(double rpm, double gearRatio) { - double motorRPM = rpm * gearRatio; - double sensorCounts = motorRPM * (2048.0 / 600.0); - return sensorCounts; - } - - /** - * @param counts Falcon Counts - * @param gearRatio Gear Ratio between Falcon and Mechanism - * @param circumference Circumference of Wheel - * @return Degrees of Rotation of Mechanism falconToDegrees - */ - public static double falconToMeters(double counts, double gearRatio, double circumference) { - return counts * circumference / (gearRatio * 2048.0); - } - - /** - * @param velocitycounts Falcon Velocity Counts - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return Falcon Velocity Counts - */ - public static double falconToMPS(double velocitycounts, double circumference, - double gearRatio) { - double wheelRPM = falconToRPM(velocitycounts, gearRatio); - double wheelMPS = (wheelRPM * circumference) / 60; - return wheelMPS; - } - - /** - * @param velocity Velocity MPS - * @param circumference Circumference of Wheel - * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM) - * @return Falcon Velocity Counts - */ - public static double mpsToFalcon(double velocity, double circumference, double gearRatio) { - double wheelRPM = ((velocity * 60) / circumference); - double wheelVelocity = rpmToFalcon(wheelRPM, gearRatio); - return wheelVelocity; - } - - /** - * Normalize angle to between 0 to 360 - * - * @param goal initial angle - * @return normalized angle - */ - public static double reduceTo0_360(double goal) { - return goal % 360; - } - -} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e4c1815..ce41608 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,33 +1,219 @@ package frc.robot; +import static edu.wpi.first.units.Units.Inches; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.studica.frc.AHRS.NavXComType; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; +import frc.robot.subsystems.swerve.mod.ModuleConstants; +import frc.robot.subsystems.swerve.mod.ModuleConstantsBuilder; +import frc.robot.subsystems.vision.CameraConstants; +import frc.robot.subsystems.vision.CameraConstantsBuilder; + /** * Constants file. */ public final class Constants { - /** - * Stick Deadband - */ - public static final double stickDeadband = 0.1; - /** - * Driver ID - */ - public static final int driverID = 0; - /** - * Operator ID - */ - public static final int operatorID = 1; + + /** Constants for driver controls */ + public static class DriverControls { + /** Driverstation controller Index */ + public static final int controllerId = 0; + /** Stick axis controls less than this amount are treated as 0. */ + public static final double stickDeadband = 0.1; + + /** Maximum Translational speed (in m/s) */ + public static final double driverTranslationalMaxSpeed = 3.0; + /** Maximum Rotational speed (in rad/s) */ + public static final double driverRotationalMaxSpeed = 4.0; + } /** - * Motor CAN id's. + * MoveToPos constants. */ - public static final class Motors { + public static class SwerveTransformPID { + public static final double translationP = 3.5; + public static final double translationI = 0.0; + public static final double translationD = 0.0; + public static final double rotationP = 3.0; + public static final double rotationI = 0.0; + public static final double rotationD = 0.0; + + public static final double maxAngularVelocity = 9.0; + public static final double maxAngularAcceleration = 9 * 5; } /** - * Pneumatics CAN id constants. + * Swerve Constants */ - public static final class Pneumatics { + public static final class Swerve { + /** If true, motors and absolute encoders are on canivore loop. Otherwise on rio. */ + public static final boolean isCanviore = true; + + public static final NavXComType navXID = NavXComType.kMXP_SPI; + public static final boolean invertGyro = true; + + /* Drivetrain Constants */ + public static final double trackWidth = Units.inchesToMeters(24.229); + public static final double wheelBase = Units.inchesToMeters(24.229); + public static final Distance wheelRadius = Inches.of(1.913); + public static final Distance wheelDiameter = wheelRadius.times(2); + public static final Distance wheelCircumference = wheelDiameter.times(Math.PI); + + public static final Distance bumperFront = Inches.of(17.5); + public static final Distance bumperRight = Inches.of(17.5); + + public static final Translation2d[] swerveTranslations = + new Translation2d[] {new Translation2d(wheelBase / 2.0, trackWidth / 2.0), + new Translation2d(wheelBase / 2.0, -trackWidth / 2.0), + new Translation2d(-wheelBase / 2.0, trackWidth / 2.0), + new Translation2d(-wheelBase / 2.0, -trackWidth / 2.0)}; + + /* + * Swerve Kinematics No need to ever change this unless you are not doing a traditional + * rectangular/square 4 module swerve + */ + public static final SwerveDriveKinematics swerveKinematics = + new SwerveDriveKinematics(swerveTranslations); + + /* Module Gear Ratios */ + public static final double driveGearRatio = (8.14 / 1.0); // MK4i L1 + public static final double angleGearRatio = ((150.0 / 7.0) / 1.0); // (150 / 7) : 1 + + /* Motor Inverts */ + public static final InvertedValue angleMotorInvert = InvertedValue.Clockwise_Positive; + public static final InvertedValue driveMotorInvert = + InvertedValue.CounterClockwise_Positive; + + /* Angle Encoder Invert */ + public static final SensorDirectionValue cancoderInvert = + SensorDirectionValue.CounterClockwise_Positive; + + /* Swerve Current Limiting */ + public static final int angleCurrentLimit = 25; + public static final int angleCurrentLowerLimit = 40; + public static final double angleCurrentLowerTimeThreshold = 0.1; + public static final boolean angleEnableCurrentLimit = true; + + public static final int driveCurrentLimit = 35; + public static final int driveCurrentLowerLimit = 60; + public static final double driveCurrentLowerTimeThreshold = 0.1; + public static final boolean driveEnableCurrentLimit = true; + + /* + * These values are used by the drive falcon to ramp in open loop and closed loop driving. + * We found a small open loop ramp (0.25) helps with wear, tipping, etc + */ + public static final double openLoopRamp = 0.25; + public static final double closedLoopRamp = 0.0; + + /* Angle Motor PID Values */ + public static final double angleKP = 100.0; + public static final double angleKI = 0.0; + public static final double angleKD = 0.0; + + /* Drive Motor PID Values */ + public static final double driveKP = 0.0012; + public static final double driveKI = 0.0; + public static final double driveKD = 0.0; + public static final double driveKF = 0.0; + + /* Drive Motor Characterization Values From SYSID */ + public static final double driveKS = 0.185; + public static final double driveKV = 1.004 / 6.536; + public static final double driveKA = 0.0; + + /* Swerve Profiling Values */ + /** Meters per Second */ + public static final double maxSpeed = 3.0; + public static final double autoMaxSpeed = 3.0; + /** Radians per Second */ + public static final double maxAngularVelocity = 4.0; + + /* Neutral Modes */ + public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast; + public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake; + + public static final double odometryFrequency = 100.0; + + /* Teleop limits */ + public static final double forwardLimit = 10.0; + public static final double forwardTiltLimit = 1000.0; + public static final double leftTiltLimit = 1000.0; + public static final double rightTiltLimit = 1000.0; + public static final double backTiltLimit = 1000.0; + public static final double skidLimit = 1000.0; + + /* Module Specific Constants */ + + // @anchor:moduleConstants + // @formatter:off + public static final ModuleConstants[] modulesConstants = new ModuleConstants[] { + // Front Left Module + new ModuleConstantsBuilder() + .driveMotorId(2) + .angleMotorId(1) + .canCoderId(1) + .angleOffset(Rotation2d.fromRotations(0.008789)) + .finish(), + // Front Right Module + new ModuleConstantsBuilder() + .driveMotorId(9) + .angleMotorId(8) + .canCoderId(2) + .angleOffset(Rotation2d.fromRotations(-0.301758)) + .finish(), + // Back Left Module + new ModuleConstantsBuilder() + .driveMotorId(0) + .angleMotorId(19) + .canCoderId(4) + .angleOffset(Rotation2d.fromRotations(-0.451172)) + .finish(), + // Back Right Module + new ModuleConstantsBuilder() + .driveMotorId(11) + .angleMotorId(10) + .canCoderId(3) + .angleOffset(Rotation2d.fromRotations(0.321777)) + .finish(), + }; + // @formatter:on } + /** Vision Constants */ + public static final class Vision { + public static final AprilTagFieldLayout fieldLayout = + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark); + // @formatter:off + public static final CameraConstants[] cameraConstants = new CameraConstants[] { + new CameraConstantsBuilder() + .name("cam0") + .height(800) + .width(1280) + .horizontalFieldOfView(80) + .simFps(20) + .simLatency(0.3) + .simLatencyStdDev(0.02) + .calibrationErrorMean(0.8) + .calibrationErrorStdDev(0.08) + .robotToCamera(new Transform3d(new Translation3d(Units.inchesToMeters(11), + -Units.inchesToMeters(12), Units.inchesToMeters(10)), + new Rotation3d(Math.PI, 0, 0))) + .translationError(0.02) + .finish(), + }; + // @formatter:on + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b22af27..0e2484d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,15 +14,15 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.util.PhoenixSignals; /** * Runs tasks on Roborio in this file. */ public class Robot extends LoggedRobot { private RobotContainer robotContainer; - private Command autoChooser; /** * Robnot Run type @@ -37,6 +37,7 @@ public static enum RobotRunType { } public RobotRunType robotRunType = RobotRunType.kReal; + private Timer gcTimer = new Timer(); // private Ultrasonic ultrasonic = new Ultrasonic(); /** @@ -63,8 +64,6 @@ public void robotInit() { break; } - - if (isReal()) { Logger.addDataReceiver(new WPILOGWriter("/media/sda1")); // Log to a USB stick Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables @@ -95,27 +94,21 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(robotRunType); - } - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *

- * This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ + gcTimer.start(); + } @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, - // running already-scheduled commands, removing finished or interrupted commands, and - // running - // subsystem periodic() methods. This must be called from the robot's periodic block in - // order for - // anything in the Command-based framework to work. + PhoenixSignals.refreshAll(); + CommandScheduler.getInstance().run(); + + robotContainer.periodic(); + + if (gcTimer.advanceIfElapsed(5)) { + System.gc(); + } } @Override @@ -124,48 +117,24 @@ public void disabledInit() {} @Override public void disabledPeriodic() {} - /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - */ @Override - public void autonomousInit() { - robotContainer.getAutonomousCommand().schedule(); - autoChooser = robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (autoChooser != null) { - autoChooser.schedule(); - } - } + public void autonomousInit() {} - /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() {} @Override - public void teleopInit() { - if (autoChooser != null) { - autoChooser.cancel(); - } - } + public void teleopInit() {} - /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - // vision.update(); - } + public void teleopPeriodic() {} @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } + public void testInit() {} - /** This function is called periodically during test mode. */ @Override public void testPeriodic() {} - private static final String environmentVariable = "AKIT_LOG_PATH"; private static final String advantageScopeFileName = "akit-log-path.txt"; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2756f2f..9a3c3c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,17 +1,28 @@ package frc.robot; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController; -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.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; +import org.ironmaple.simulation.SimulatedArena; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Robot.RobotRunType; -import frc.robot.subsystems.drive.Drivetrain; -import frc.robot.subsystems.drive.DrivetrainIO; -import frc.robot.subsystems.drive.DrivetrainReal; +import frc.robot.subsystems.swerve.Swerve; +import frc.robot.subsystems.swerve.SwerveIOEmpty; +import frc.robot.subsystems.swerve.SwerveReal; +import frc.robot.subsystems.swerve.SwerveSim; +import frc.robot.subsystems.swerve.gyro.GyroIOEmpty; +import frc.robot.subsystems.swerve.gyro.GyroNavX2; +import frc.robot.subsystems.swerve.mod.SwerveModuleIOEmpty; +import frc.robot.subsystems.swerve.mod.SwerveModuleReal; +import frc.robot.subsystems.swerve.util.TeleopControls; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIOEmpty; +import frc.robot.subsystems.vision.VisionReal; +import frc.robot.subsystems.vision.VisionSim; +import frc.robot.util.DeviceDebug; +import frc.robot.viz.RobotViz; + /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -19,60 +30,61 @@ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including * subsystems, commands, and button mappings) should be declared here. */ -public class RobotContainer { - /* Controllers */ - private final CommandXboxController driver = new CommandXboxController(Constants.driverID); - private final CommandXboxController operator = new CommandXboxController(Constants.operatorID); +@NullMarked +public final class RobotContainer { - // Initialize AutoChooser Sendable - private final SendableChooser autoChooser = new SendableChooser<>(); + /* Controllers */ + public final CommandXboxController driver = + new CommandXboxController(Constants.DriverControls.controllerId); /* Subsystems */ - private Drivetrain drivetrain; + private final Swerve swerve; + private final Vision vision; + + private final SwerveSim sim; + private final RobotViz viz; /** - * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer(RobotRunType runtimeType) { - SmartDashboard.putData("Choose Auto: ", autoChooser); - autoChooser.setDefaultOption("Wait 1 Second", "wait"); switch (runtimeType) { case kReal: - drivetrain = new Drivetrain(new DrivetrainReal()); + sim = null; + swerve = new Swerve(SwerveReal::new, GyroNavX2::new, SwerveModuleReal::new); + vision = new Vision(swerve.state, new VisionReal()); break; case kSimulation: - // drivetrain = new Drivetrain(new DrivetrainSim() {}); + sim = new SwerveSim(new Pose2d(2.0, 2.0, Rotation2d.kZero)); + swerve = new Swerve(sim::simProvider, sim::gyroProvider, sim::moduleProvider); + vision = new Vision(swerve.state, new VisionSim(sim)); break; default: - drivetrain = new Drivetrain(new DrivetrainIO() {}); + sim = null; + swerve = new Swerve(SwerveIOEmpty::new, GyroIOEmpty::new, SwerveModuleIOEmpty::new); + vision = new Vision(swerve.state, new VisionIOEmpty()); } - // Configure the button bindings - configureButtonBindings(); - } + viz = new RobotViz(sim, swerve); - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses - * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a - * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() {} + DeviceDebug.initialize(); - /** - * Gets the user's selected autonomous command. - * - * @return Returns autonomous command selected. - */ - public Command getAutonomousCommand() { - Command autocommand; - String stuff = autoChooser.getSelected(); - switch (stuff) { - case "wait": - autocommand = new WaitCommand(1.0); - break; - default: - autocommand = new InstantCommand(); + swerve.setDefaultCommand(swerve.driveUserRelative(TeleopControls.teleopControls( + () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX()))); + + driver.y().onTrue(swerve.setFieldRelativeOffset()); + + driver.a().whileTrue(swerve.wheelRadiusCharacterization()).onFalse(swerve.emergencyStop()); + driver.b().whileTrue(swerve.feedforwardCharacterization()).onFalse(swerve.emergencyStop()); + } + + /** Runs once per 0.02 seconds after subsystems and commands. */ + public void periodic() { + if (sim != null) { + SimulatedArena.getInstance().simulationPeriodic(); + Logger.recordOutput("FieldSimulation/Algae", + SimulatedArena.getInstance().getGamePiecesArrayByType("Algae")); + Logger.recordOutput("FieldSimulation/Coral", + SimulatedArena.getInstance().getGamePiecesArrayByType("Coral")); } - return autocommand; + viz.periodic(); } } diff --git a/src/main/java/frc/robot/autos/.keep b/src/main/java/frc/robot/autos/.keep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/commands/.keep b/src/main/java/frc/robot/commands/.keep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/modules/.keep b/src/main/java/frc/robot/modules/.keep deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java deleted file mode 100644 index 88e6569..0000000 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.subsystems.drive; - -import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** - * Drivetrain subsystem. - */ - -public class Drivetrain extends SubsystemBase { - private DrivetrainIO io; - private DrivetrainIOInputsAutoLogged inputs = new DrivetrainIOInputsAutoLogged(); - - /** - * Create Wrist Intake Subsystem - */ - public Drivetrain(DrivetrainIO io) { - this.io = io; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Drivetrain", inputs); - } -} - diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java deleted file mode 100644 index 842e1aa..0000000 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainIO.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.subsystems.drive; - -import org.littletonrobotics.junction.AutoLog; -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * DrivetrainIO interface - */ -public interface DrivetrainIO { - /** - * Drivetrain IO - */ - @AutoLog - public static class DrivetrainIOInputs { - public Rotation2d gyroYaw = new Rotation2d(); - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(DrivetrainIOInputs inputs) {} - - /** Run the motor at the specified voltage. */ - public default void setDriveVoltage(double lvolts, double rvolts) {} - - -} diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java deleted file mode 100644 index f8f17d1..0000000 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.subsystems.drive; - -import edu.wpi.first.math.geometry.Rotation2d; - -/** - * DrivetrainReal - */ -public class DrivetrainReal implements DrivetrainIO { - - - /** - * Drivetrain Real - */ - public DrivetrainReal() {} - - @Override - public void updateInputs(DrivetrainIOInputs inputs) { - inputs.gyroYaw = Rotation2d.fromDegrees(0); - } - - /** - * Drive Voltage - */ - public void setDriveVoltage(double lvolts, double rvolts) {} - -} diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java new file mode 100644 index 0000000..3b1ff21 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -0,0 +1,418 @@ +package frc.robot.subsystems.swerve; + +import java.util.Arrays; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.BiFunction; +import java.util.function.Function; +import java.util.function.Supplier; +import java.util.stream.IntStream; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.gyro.GyroIO; +import frc.robot.subsystems.swerve.gyro.GyroInputsAutoLogged; +import frc.robot.subsystems.swerve.mod.SwerveModule; +import frc.robot.subsystems.swerve.mod.SwerveModuleIO; +import frc.robot.subsystems.swerve.util.MoveToPoseBuilder; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; +import frc.robot.subsystems.swerve.util.SwerveRateLimiter; +import frc.robot.subsystems.swerve.util.SwerveState; +import frc.robot.subsystems.swerve.util.TuningCommands; +import frc.robot.util.AllianceFlipUtil; + +/** + * Primary swerve drivetrain subsystem. + * + *

+ * This subsystem owns and coordinates all components required to control and estimate the state of + * a swerve drive, including: + *

    + *
  • Swerve modules and their IO implementations
    • + *
  • Gyro integration
    • + *
  • High-frequency odometry sampling via {@link PhoenixOdometryThread}
    • + *
  • Pose estimation and vision fusion via {@link SwerveState}
    • + *
  • Acceleration, tilt, and skid limiting via {@link SwerveRateLimiter}
    • + *
+ * + *

Threading model

Odometry-related sensor signals are updated on a dedicated background + * thread. Access to these signals and derived state is synchronized using a shared + * {@code odometryLock} to ensure consistency across the estimator and modules. + * + *

Pose estimation

Wheel encoder and gyro data are integrated at high rate to produce + * odometry updates, which are then fused with delayed vision measurements inside + * {@link SwerveState}. The resulting pose estimate is the authoritative source of robot position + * for autonomous and field-relative driving. + * + *

Driving model

All drive commands ultimately resolve to robot-relative + * {@link ChassisSpeeds}. These speeds are passed through a {@link SwerveRateLimiter} before being + * discretized and converted to per-module states. + * + *

+ * This class exposes convenience commands for robot-relative, field-relative, and user-relative + * driving, as well as trajectory-style pose targeting and characterization routines. + */ +@NullMarked +public final class Swerve extends SubsystemBase { + + private final Lock odometryLock = new ReentrantLock(); + private final PhoenixOdometryThread odometryThread; + private final SwerveModule[] modules; + private final GyroIO gyro; + private final GyroInputsAutoLogged gyroInputs = new GyroInputsAutoLogged(); + private final SwerveIO io; + private final SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged(); + + private final SwerveRateLimiter limiter = new SwerveRateLimiter(); + + public final SwerveState state; + + /** + * Constructs the swerve subsystem and initializes all hardware interfaces, estimator state, and + * background odometry processing. + * + *

+ * The provided factories are invoked with a shared {@link PhoenixOdometryThread} instance to + * allow sensors and motors to register high-frequency signals for synchronized sampling. + * + * @param swerveIo factory for creating the drivetrain-level IO implementation + * @param gyroIo factory for creating the gyro IO implementation + * @param moduleIoFn factory for creating per-module IO implementations + */ + public Swerve(Function swerveIo, + Function gyroIo, + BiFunction moduleIoFn) { + super("Swerve"); + this.odometryThread = new PhoenixOdometryThread(this.odometryLock); + this.gyro = gyroIo.apply(this.odometryThread); + this.modules = IntStream.range(0, Constants.Swerve.modulesConstants.length) + .mapToObj(i -> new SwerveModule(i, moduleIoFn.apply(i, this.odometryThread))) + .toArray(SwerveModule[]::new); + this.io = swerveIo.apply(odometryThread); + this.odometryThread.start(); + this.odometryLock.lock(); + SwerveModulePosition[] initPositions = new SwerveModulePosition[modules.length]; + try { + Arrays.stream(modules).map(mod -> { + mod.updateInputs(); + return mod.getPosition(); + }).toArray(_i -> initPositions); + } finally { + this.odometryLock.unlock(); + } + this.state = new SwerveState(initPositions, this.gyroInputs.yaw); + } + + @Override + public void periodic() { + this.odometryLock.lock(); + + for (int i = 0; i < modules.length; i++) { + this.modules[i].updateInputs(); + } + + this.gyro.updateInputs(this.gyroInputs); + Logger.processInputs("Swerve/Gyro", this.gyroInputs); + + this.io.updateInputs(this.inputs); + Logger.processInputs("Swerve/Timestamps", this.inputs); + + this.odometryLock.unlock(); + + for (int i = 0; i < modules.length; i++) { + this.modules[i].periodic(); + } + + SwerveModuleState[] wheelStates = new SwerveModuleState[modules.length]; + for (int j = 0; j < modules.length; j++) { + wheelStates[j] = modules[j].getState(); + } + ChassisSpeeds currentSpeeds = + Constants.Swerve.swerveKinematics.toChassisSpeeds(wheelStates); + double[] sampleTimestamps = this.inputs.timestamps; + double gyroPerSample = currentSpeeds.omegaRadiansPerSecond / sampleTimestamps.length; + SwerveModulePosition[] wheelPositions = new SwerveModulePosition[modules.length]; + for (int i = 0; i < sampleTimestamps.length; i++) { + for (int j = 0; j < modules.length; j++) { + wheelPositions[j] = modules[j].getOdometryPosition(i); + } + state.addOdometryObservation(wheelPositions, + gyroInputs.connected ? Rotation2d.fromRadians(gyroInputs.yawRads[i]) + : state.getGlobalPoseEstimate().getRotation() + .plus(Rotation2d.fromRadians((gyroPerSample + 1) * i)), + sampleTimestamps[i]); + } + limiter.update(currentSpeeds); + state.updateSpeeds(currentSpeeds); + + Logger.recordOutput("Swerve/GlobalPoseEstimate", state.getGlobalPoseEstimate()); + } + + /** + * Drives the robot using robot-relative chassis speeds. + * + *

+ * Supplied speeds are passed through the {@link SwerveRateLimiter} before being applied to the + * drivetrain. + * + * @param driveSpeeds supplier of robot-relative chassis speeds + * @return a command that drives the robot while scheduled + */ + public Command driveRobotRelative(Supplier driveSpeeds) { + return this.run(() -> { + ChassisSpeeds speeds = driveSpeeds.get(); + speeds = limiter.limit(speeds); + setModuleStates(speeds); + }); + } + + /** + * Drives the robot using a user-defined field reference heading. + * + *

+ * The supplied field-relative speeds are transformed into robot-relative speeds using the + * user-controlled heading offset. + * + * @param driveSpeeds supplier of field-relative chassis speeds + * @return a command that drives the robot while scheduled + */ + public Command driveUserRelative(Supplier driveSpeeds) { + return driveRobotRelative(() -> ChassisSpeeds.fromFieldRelativeSpeeds(driveSpeeds.get(), + getUserRelativeHeading())); + } + + /** + * Drives the robot using the estimated global field heading. + * + *

+ * The supplied field-relative speeds are transformed into robot-relative speeds using the + * current pose estimate from {@link SwerveState}. + * + * @param driveSpeeds supplier of field-relative chassis speeds + * @return a command that drives the robot while scheduled + */ + public Command driveFieldRelative(Supplier driveSpeeds) { + return driveRobotRelative(() -> ChassisSpeeds.fromFieldRelativeSpeeds(driveSpeeds.get(), + state.getGlobalPoseEstimate().getRotation())); + } + + /** + * Immediately sets the robot's pose to a new value, updating both the odometry estimator and + * the underlying simulation state (if any). + * + *

+ * This is useful when you need to forcibly override the robot's pose, for example during + * testing or at the start of autonomous in simulation. + * + *

+ * If you only want to update the odometry/estimator without affecting any simulation state, use + * {@link SwerveState#resetPose(Pose2d)} instead. + * + * @param newPose a supplier that provides the new robot pose in field coordinates + * @return a command that applies the pose override once when scheduled + */ + public Command overridePose(Supplier newPose) { + return Commands.runOnce(() -> { + Pose2d newPose_ = newPose.get(); + io.resetPose(newPose_); + state.resetPose(newPose_); + }); + } + + /** + * Creates a command builder for driving the robot to a target global pose. + * + *

+ * The generated command uses a holonomic controller and the current pose estimate to compute + * chassis speeds, which are rate-limited and applied to the drivetrain. + * + * @return a {@link MoveToPoseBuilder} for configuring pose targets + */ + public MoveToPoseBuilder moveToPose() { + return new MoveToPoseBuilder(this, (speeds) -> { + speeds = limiter.limit(speeds); + setModuleStates(speeds); + }); + } + + /** + * Creates a SysId routine for drivetrain feedforward characterization. + * + *

+ * This routine is used to identify kS and kV parameters for the swerve drive by applying + * controlled voltage steps and measuring resulting motion. + * + * @return a command that runs feedforward characterization + */ + public Command feedforwardCharacterization() { + return TuningCommands.feedforwardCharacterization(this, this::runCharacterization, + this::getFFCharacterizationVelocity); + } + + /** + * Creates a SysId routine for wheel radius characterization. + * + *

+ * This routine estimates the effective wheel radius by correlating commanded motion with + * measured yaw change. + * + * @return a command that runs wheel radius characterization + */ + public Command wheelRadiusCharacterization() { + return TuningCommands.wheelRadiusCharacterization(this, this::setModuleStates, + this::getWheelRadiusCharacterizationPositions, () -> this.gyroInputs.yaw); + } + + /** + * Sets a user-defined field-relative heading offset. + * + *

+ * This offset is used by user-relative driving modes to define "forward" independently of the + * robot's pose estimate. + * + * @param knownHeading supplier of the desired field heading + * @return a one-shot command that applies the offset + */ + public Command setFieldRelativeOffset(Supplier knownHeading) { + return Commands.runOnce( + () -> fieldOffset = gyroInputs.yaw.getRotations() - knownHeading.get().getRotations()); + } + + /** + * Sets a user-defined field-relative heading offset. + * + *

+ * This offset is used by user-relative driving modes to define "forward" independently of the + * robot's pose estimate. + * + * @return a one-shot command that sets the offset such that the current direction is "forward" + */ + public Command setFieldRelativeOffset() { + return setFieldRelativeOffset(() -> Rotation2d.kZero); + } + + /** + * Sets a user-defined field-relative heading offset. + * + *

+ * This offset is used by user-relative driving modes to define "forward" independently of the + * robot's pose estimate. + * + * @return a one-shot command that sets the offset such that it agrees with the estimated pose + * (+180 degrees when on red alliance) + */ + public Command resetFieldRelativeOffsetBasedOnPose() { + return setFieldRelativeOffset(() -> state.getGlobalPoseEstimate().getRotation() + .plus(AllianceFlipUtil.shouldFlip() ? Rotation2d.kZero : Rotation2d.k180deg)); + } + + /** + * Creates a command that smoothly brings the drivetrain to a complete stop. + * + *

+ * This command commands zero desired chassis speeds and allows the {@link SwerveRateLimiter} to + * decelerate the robot within configured acceleration, tilt, and skid constraints rather than + * stopping abruptly. + * + *

+ * The command completes once the rate-limited translational speed falls below a small + * threshold, indicating the robot is effectively stationary. After completion, a final + * zero-speed command is issued to ensure all modules are explicitly commanded to stop. + * + *

+ * This is intended for use when transitioning between driving modes, autonomous steps, or + * before actions that require the robot to be fully settled. + * + * @return a command that decelerates and stops the drivetrain + */ + public Command stop() { + return this.driveRobotRelative(ChassisSpeeds::new).until(() -> { + var speeds = limiter.limit(new ChassisSpeeds()); + return Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) < 0.1; + }).andThen(this.emergencyStop()); + } + + /** + * Creates a command that immediately commands zero chassis speeds to the drivetrain. + * + *

+ * This method bypasses all rate limiting and deceleration constraints and directly commands the + * swerve modules to stop in a single control cycle. As a result, it may cause abrupt + * deceleration, increased state estimation error, and/or loss of traction depending on robot + * speed and surface conditions. + * + *

+ * In most situations, {@link #stop()} should be preferred, as it brings the robot to + * rest in a controlled manner using the {@link SwerveRateLimiter}. + * + *

+ * This command is intended only for exceptional circumstances such as fault handling, disable + * transitions, or safety-critical interruptions where immediate cessation of motion is + * required. + * + * @return a command that immediately commands zero chassis speeds + */ + public Command emergencyStop() { + return this.runOnce(() -> setModuleStates(new ChassisSpeeds())); + } + + private void runCharacterization(double output) { + for (SwerveModule module : modules) { + module.runCharacterization(output); + } + } + + private double getFFCharacterizationVelocity() { + double output = 0.0; + for (SwerveModule module : modules) { + output += module.getFFCharacterizationVelocity() / modules.length; + } + return output; + } + + private double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[modules.length]; + for (int i = 0; i < modules.length; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; + } + + private double fieldOffset = 0.0; + + /** + * Returns the current user-relative heading used for driving. + * + *

+ * This heading is derived from the gyro yaw and a manually controlled field offset, independent + * of the pose estimator. + * + * @return user-relative field heading + */ + public Rotation2d getUserRelativeHeading() { + return Rotation2d.fromRotations(gyroInputs.yaw.getRotations() - fieldOffset); + } + + private void setModuleStates(ChassisSpeeds chassisSpeeds) { + ChassisSpeeds targetSpeeds = ChassisSpeeds.discretize(chassisSpeeds, 0.02); + SwerveModuleState[] swerveModuleStates = + Constants.Swerve.swerveKinematics.toSwerveModuleStates(targetSpeeds); + setModuleStates(swerveModuleStates); + } + + private void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed); + for (int i = 0; i < modules.length; i++) { + modules[i].setDesiredState(desiredStates[i]); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java new file mode 100644 index 0000000..18853dd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveIO.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.swerve; + +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; +import frc.robot.util.GenerateEmptyIO; + +/** IO for swerve timestamps */ +@GenerateEmptyIO(PhoenixOdometryThread.class) +@NullMarked +public interface SwerveIO { + + /** Inputs for swerve timestamps */ + @AutoLog + public static class SwerveInputs { + public double[] timestamps = new double[0]; + } + + /** Update inputs */ + public void updateInputs(SwerveInputs inputs); + + /** Set the ground truth pose. Only useful in sim. */ + public void resetPose(Pose2d pose); + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java new file mode 100644 index 0000000..a97c481 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveReal.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.swerve; + +import java.util.Queue; +import org.jspecify.annotations.NullMarked; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; + +/** Real Swerve Implementation */ +@NullMarked +public final class SwerveReal implements SwerveIO { + + private final Queue timestampQueue; + + /** Real Swerve Implementation */ + public SwerveReal(PhoenixOdometryThread odometryThread) { + this.timestampQueue = odometryThread.makeTimestampQueue(); + } + + @Override + public void updateInputs(SwerveInputs inputs) { + inputs.timestamps = this.timestampQueue.stream().mapToDouble(x -> x).toArray(); + this.timestampQueue.clear(); + } + + @Override + public void resetPose(Pose2d pose) {} + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java new file mode 100644 index 0000000..036bcf0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSim.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems.swerve; + +import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.Pounds; +import static edu.wpi.first.units.Units.Volts; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.jspecify.annotations.NullMarked; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.gyro.GyroIO; +import frc.robot.subsystems.swerve.gyro.GyroSim; +import frc.robot.subsystems.swerve.mod.SwerveModuleIO; +import frc.robot.subsystems.swerve.mod.SwerveModuleSim; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; + +/** Simulation implementation for swerve */ +@NullMarked +public final class SwerveSim implements SwerveIO { + + public final SwerveDriveSimulation mapleSim; + + /** Simulation implementation for swerve */ + public SwerveSim(Pose2d initialPose) { + this.mapleSim = new SwerveDriveSimulation( + DriveTrainSimulationConfig.Default().withGyro(COTS.ofNav2X()) + .withRobotMass(Pounds.of(150)) + .withCustomModuleTranslations(Constants.Swerve.swerveTranslations) + .withBumperSize(Constants.Swerve.bumperFront.times(2), + Constants.Swerve.bumperRight.times(2)) + .withSwerveModule(new SwerveModuleSimulationConfig(DCMotor.getKrakenX60(1), + DCMotor.getKrakenX60(1), Constants.Swerve.driveGearRatio, + Constants.Swerve.angleGearRatio, Volts.of(0.15), Volts.of(0.35), + Constants.Swerve.wheelRadius, KilogramSquareMeters.of(0.02), 1.2)), + initialPose); + SimulatedArena.getInstance().addDriveTrainSimulation(this.mapleSim); + } + + + /** Supplier passed into Swerve constructor */ + public SwerveSim simProvider(PhoenixOdometryThread odometryThread) { + return this; + } + + /** Supplier passed into Swerve constructor */ + public GyroIO gyroProvider(PhoenixOdometryThread odometryThread) { + return new GyroSim(this.mapleSim.getGyroSimulation()); + } + + /** Supplier passed into Swerve constructor */ + public SwerveModuleIO moduleProvider(int index, PhoenixOdometryThread odometryThread) { + return new SwerveModuleSim(index, this.mapleSim.getModules()[index]); + } + + @Override + public void updateInputs(SwerveInputs inputs) { + inputs.timestamps = new double[] {Timer.getTimestamp()}; + } + + + @Override + public void resetPose(Pose2d pose) { + mapleSim.setSimulationWorldPose(pose); + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java new file mode 100644 index 0000000..1e8c935 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIO.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.swerve.gyro; + +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; +import frc.robot.util.GenerateEmptyIO; + +/** IO for gyro */ +@GenerateEmptyIO({PhoenixOdometryThread.class}) +@NullMarked +public interface GyroIO { + + /** Inputs for gyro */ + @AutoLog + public static class GyroInputs { + public boolean connected; + public Rotation2d yaw = Rotation2d.kZero; + public double yawVelocityRadPerSec; + public Rotation2d pitch = Rotation2d.kZero; + public double pitchVelocityRadPerSec; + public Rotation2d roll = Rotation2d.kZero; + public double rollVelocityRadPerSec; + + public double[] yawRads = new double[0]; + } + + /** Update inputs */ + public void updateInputs(GyroInputs inputs); + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroNavX2.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroNavX2.java new file mode 100644 index 0000000..bca65c0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroNavX2.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.swerve.gyro; + +import java.util.Queue; +import org.jspecify.annotations.NullMarked; +import com.studica.frc.AHRS; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; + +/** NavX2 implementation for Gyro */ +@NullMarked +public class GyroNavX2 implements GyroIO { + + private AHRS gyro = new AHRS(Constants.Swerve.navXID, (int) Constants.Swerve.odometryFrequency); + + private final Queue yawQueue; + + /** NavX2 implementation for Gyro */ + public GyroNavX2(PhoenixOdometryThread odometryThread) { + this.yawQueue = odometryThread.registerSignal(() -> Units.degreesToRadians(gyro.getYaw())); + } + + @Override + public void updateInputs(GyroInputs inputs) { + inputs.connected = gyro.isConnected(); + + double invert = Constants.Swerve.invertGyro ? -1.0 : 1.0; + + inputs.yaw = Rotation2d.fromDegrees(invert * gyro.getYaw()); + inputs.yawVelocityRadPerSec = invert * Units.degreesToRadians(gyro.getRawGyroZ()); + + inputs.pitch = Rotation2d.fromDegrees(invert * gyro.getPitch()); + inputs.pitchVelocityRadPerSec = invert * Units.degreesToRadians(gyro.getRawGyroX()); + + inputs.roll = Rotation2d.fromDegrees(invert * gyro.getRoll()); + inputs.rollVelocityRadPerSec = invert * Units.degreesToRadians(gyro.getRawGyroY()); + + inputs.yawRads = yawQueue.stream().mapToDouble(x -> invert * x).toArray(); + yawQueue.clear(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroSim.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroSim.java new file mode 100644 index 0000000..5d3f62c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroSim.java @@ -0,0 +1,26 @@ +package frc.robot.subsystems.swerve.gyro; + +import static edu.wpi.first.units.Units.RadiansPerSecond; +import org.ironmaple.simulation.drivesims.GyroSimulation; +import org.jspecify.annotations.NullMarked; + +/** Simulation implementation for gyro */ +@NullMarked +public class GyroSim implements GyroIO { + + private final GyroSimulation gyro; + + /** Simulation implementation for gyro */ + public GyroSim(GyroSimulation gyro) { + this.gyro = gyro; + } + + @Override + public void updateInputs(GyroInputs inputs) { + inputs.yaw = gyro.getGyroReading(); + inputs.yawVelocityRadPerSec = gyro.getMeasuredAngularVelocity().in(RadiansPerSecond); + inputs.yawRads = new double[] {inputs.yaw.getRadians()}; + inputs.connected = true; + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/mod/ModuleConstants.java b/src/main/java/frc/robot/subsystems/swerve/mod/ModuleConstants.java new file mode 100644 index 0000000..4a79045 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/mod/ModuleConstants.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.swerve.mod; + +import org.jspecify.annotations.NullMarked; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.util.typestate.RequiredField; +import frc.robot.util.typestate.TypeStateBuilder; + +/** Per-module constants */ +@NullMarked +public class ModuleConstants { + + /** CAN ID for the drive motor */ + public final int driveMotorId; + + /** CAN ID for the angle motor */ + public final int angleMotorId; + + /** CAN ID for the CANCoder */ + public final int canCoderId; + + /** Reported angle when wheel is straight */ + public final Rotation2d angleOffset; + + /** Per-module constants */ + @TypeStateBuilder("ModuleConstantsBuilder") + public ModuleConstants(@RequiredField int driveMotorId, @RequiredField int angleMotorId, + @RequiredField int canCoderId, @RequiredField Rotation2d angleOffset) { + this.driveMotorId = driveMotorId; + this.angleMotorId = angleMotorId; + this.canCoderId = canCoderId; + this.angleOffset = angleOffset; + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModule.java new file mode 100644 index 0000000..357c6e7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModule.java @@ -0,0 +1,110 @@ +package frc.robot.subsystems.swerve.mod; + +import static edu.wpi.first.units.Units.Meters; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; + +/** Swerve Module */ +@NullMarked +public class SwerveModule { + + private final String inputsName; + private final SwerveModuleIO io; + private final SwerveModuleInputsAutoLogged inputs = new SwerveModuleInputsAutoLogged(); + + /** + * Swerve Module + * + * @param moduleId index into the motor constants + * @param io IO implementation + */ + public SwerveModule(int moduleId, SwerveModuleIO io) { + this.inputsName = "Swerve/Module" + moduleId; + this.io = io; + } + + /** + * Update inputs for a Swerve Module. + */ + public void updateInputs() { + this.io.updateInputs(this.inputs); + Logger.processInputs(this.inputsName, this.inputs); + } + + /** + * Set the desired state of the Swerve Module + * + * @param desiredState The desired {@link SwerveModuleState} for the module + */ + public void setDesiredState(SwerveModuleState desiredState) { + desiredState.optimize(getState().angle); + this.io.runAnglePosition(desiredState.angle); + this.io.runDriveVelocity( + desiredState.speedMetersPerSecond / Constants.Swerve.wheelRadius.in(Meters), 0); + } + + /** Get the current Swerve Module State */ + public SwerveModuleState getState() { + return new SwerveModuleState( + inputs.driveVelocityRadPerSec * Constants.Swerve.wheelRadius.in(Meters), + inputs.anglePosition); + } + + /** Get the current Swerve Module Position */ + public SwerveModulePosition getPosition() { + return new SwerveModulePosition( + inputs.drivePositionRad * Constants.Swerve.wheelRadius.in(Meters), + inputs.anglePosition); + } + + /** Run motor at given voltage with angle motor pointing forward */ + public void runCharacterization(double output) { + io.runDriveOpenLoop(output); + io.runAnglePosition(Rotation2d.kZero); + } + + /** Returns the module position in radians. */ + public double getWheelRadiusCharacterizationPosition() { + return inputs.drivePositionRad; + } + + /** Returns the module velocity in rotations/sec (Phoenix native units). */ + public double getFFCharacterizationVelocity() { + return Units.radiansToRotations(inputs.driveVelocityRadPerSec); + } + + /** Set drive motor brake mode */ + public void setBrakeMode(boolean enabled) { + this.io.setDriveBrakeMode(enabled); + } + + /** Set angle motor brake mode */ + public void setFreeSpin(boolean enabled) { + this.io.setAngleBrakeMode(!enabled); + } + + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[0]; + + /** Periodic function (doesn't update inputs) */ + public void periodic() { + int sampleCount = inputs.odometryDrivePositionsRad.length; + odometryPositions = new SwerveModulePosition[sampleCount]; + for (int i = 0; i < sampleCount; i++) { + double positionMeters = + inputs.odometryDrivePositionsRad[i] * Constants.Swerve.wheelRadius.in(Meters); + Rotation2d angle = inputs.odometryAnglePositions[i]; + odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); + } + } + + /** Get swerve module position at timestamp index i */ + public SwerveModulePosition getOdometryPosition(int i) { + return odometryPositions[i]; + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleIO.java b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleIO.java new file mode 100644 index 0000000..e97b9d2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleIO.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.swerve.mod; + +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; +import frc.robot.util.GenerateEmptyIO; + +/** IO for swerve module */ +@GenerateEmptyIO({int.class, PhoenixOdometryThread.class}) +@NullMarked +public interface SwerveModuleIO { + + /** Inputs for swerve module */ + @AutoLog + public static class SwerveModuleInputs { + public boolean driveConnected; + public double drivePositionRad; + public double driveVelocityRadPerSec; + public double driveAppliedVolts; + public double driveSupplyCurrentAmps; + public double driveStatorCurrentAmps; + + public boolean angleConnected; + public Rotation2d anglePosition = Rotation2d.kZero; + public double angleVelocityRadPerSec; + public double angleAppliedVolts; + public double angleSupplyCurrentAmps; + public double angleStatorCurrentAmps; + + public boolean absoluteAngleConnected; + public Rotation2d angleAbsolutePosition = Rotation2d.kZero; + + public double[] odometryDrivePositionsRad = new double[0]; + public double[] odometryDriveVelocityRadsPerSec = new double[0]; + public Rotation2d[] odometryAnglePositions = new Rotation2d[0]; + } + + /** Update inputs */ + public void updateInputs(SwerveModuleInputs inputs); + + /** Set voltage on drive motor */ + public void runDriveOpenLoop(double output); + + /** Set voltage on angle motor */ + public void runAngleOpenLoop(double output); + + /** Set velocity setpoint */ + public void runDriveVelocity(double velocityRadPerSec, double feedforward); + + /** Set angle setpoint */ + public void runAnglePosition(Rotation2d rotation); + + /** Set PID constants for drive motor */ + public void setDrivePID(double kP, double kI, double kD, double kS, double kV, double kA); + + /** Set PID constants for angle motor */ + public void setAnglePID(double kP, double kI, double kD); + + /** Set drive brake mode */ + public void setDriveBrakeMode(boolean enabled); + + /** Set angle brake mode */ + public void setAngleBrakeMode(boolean enabled); + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleReal.java b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleReal.java new file mode 100644 index 0000000..5dfe773 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleReal.java @@ -0,0 +1,301 @@ +package frc.robot.subsystems.swerve.mod; + +import java.util.Queue; +import java.util.concurrent.Executor; +import java.util.concurrent.Executors; +import org.jspecify.annotations.NullMarked; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.util.PhoenixOdometryThread; +import frc.robot.util.DeviceDebug; +import frc.robot.util.PhoenixSignals; + +/** Real swerve module implementation (assumes two TalonFXs) */ +@NullMarked +public class SwerveModuleReal implements SwerveModuleIO { + + // Drive Motor and Signals + private final TalonFX driveMotor; + private final TalonFXConfiguration driveConfig = new TalonFXConfiguration(); + + private final StatusSignal drivePosition; + private final Queue drivePositionQueue; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveSupplyCurrentAmps; + private final StatusSignal driveStatorCurrentAmps; + + // Angle Motor and Signals + private final TalonFX angleMotor; + private final TalonFXConfiguration angleConfig = new TalonFXConfiguration(); + + private final StatusSignal anglePosition; + private final Queue anglePositionQueue; + private final StatusSignal angleVelocity; + private final StatusSignal angleAppliedVolts; + private final StatusSignal angleSupplyCurrentAmps; + private final StatusSignal angleStatorCurrentAmps; + + // Absolute Encoder Signals + private final CANcoder absoluteEncoder; + private final CANcoderConfiguration absoluteConfig = new CANcoderConfiguration(); + private final StatusSignal absolutePosition; + + // Absolute Offset + private final Rotation2d angleOffset; + + /** Real swerve module implementation */ + public SwerveModuleReal(int index, PhoenixOdometryThread odometryThread) { + boolean isCanivore = Constants.Swerve.isCanviore; + + String loop = isCanivore ? "*" : ""; + driveMotor = new TalonFX(Constants.Swerve.modulesConstants[index].driveMotorId, loop); + angleMotor = new TalonFX(Constants.Swerve.modulesConstants[index].angleMotorId, loop); + absoluteEncoder = new CANcoder(Constants.Swerve.modulesConstants[index].canCoderId, loop); + angleOffset = Constants.Swerve.modulesConstants[index].angleOffset; + + // Create drive motor signals + drivePosition = driveMotor.getPosition(); + drivePositionQueue = odometryThread.registerSignal(driveMotor.getPosition().clone()); + driveVelocity = driveMotor.getVelocity(); + driveAppliedVolts = driveMotor.getMotorVoltage(); + driveSupplyCurrentAmps = driveMotor.getSupplyCurrent(); + driveStatorCurrentAmps = driveMotor.getStatorCurrent(); + + // Create angle motor signals + anglePosition = angleMotor.getPosition(); + anglePositionQueue = odometryThread.registerSignal(angleMotor.getPosition().clone()); + angleVelocity = angleMotor.getVelocity(); + angleAppliedVolts = angleMotor.getMotorVoltage(); + angleSupplyCurrentAmps = angleMotor.getSupplyCurrent(); + angleStatorCurrentAmps = angleMotor.getStatorCurrent(); + + // Create absolute encoder signals + absolutePosition = absoluteEncoder.getAbsolutePosition(); + + configDriveMotor(); + configAngleMotor(); + configAngleEncoder(); + + // Configure periodic frames + PhoenixSignals.tryUntilOk(5, + () -> BaseStatusSignal.setUpdateFrequencyForAll(Constants.Swerve.odometryFrequency, + drivePosition, anglePosition)); + PhoenixSignals.tryUntilOk(5, + () -> BaseStatusSignal.setUpdateFrequencyForAll(50.0, driveVelocity, driveAppliedVolts, + driveSupplyCurrentAmps, driveStatorCurrentAmps, angleVelocity, angleAppliedVolts, + angleSupplyCurrentAmps, angleStatorCurrentAmps)); + PhoenixSignals.tryUntilOk(5, () -> ParentDevice.optimizeBusUtilizationForAll(driveMotor, + angleMotor, absoluteEncoder)); + + // Register signals for refresh + PhoenixSignals.registerSignals(isCanivore, drivePosition, driveVelocity, driveAppliedVolts, + driveSupplyCurrentAmps, driveStatorCurrentAmps, anglePosition, angleVelocity, + angleAppliedVolts, angleSupplyCurrentAmps, angleStatorCurrentAmps, absolutePosition); + + DeviceDebug.register("SwerveModule_" + index + "_Drive", driveMotor); + DeviceDebug.register("SwerveModule_" + index + "_Angle", angleMotor); + DeviceDebug.register("SwerveModule_" + index + "_Encoder", absoluteEncoder); + } + + private void configDriveMotor() { + /* Drive Motor Config */ + /* Motor Inverts and Neutral Mode */ + driveConfig.MotorOutput.Inverted = Constants.Swerve.driveMotorInvert; + driveConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode; + + /* Gear Ratio Config */ + driveConfig.Feedback.SensorToMechanismRatio = Constants.Swerve.driveGearRatio; + + /* Current Limiting */ + driveConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.driveEnableCurrentLimit; + driveConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveCurrentLimit; + driveConfig.CurrentLimits.SupplyCurrentLowerLimit = Constants.Swerve.driveCurrentLowerLimit; + driveConfig.CurrentLimits.SupplyCurrentLowerTime = + Constants.Swerve.driveCurrentLowerTimeThreshold; + + /* PID Config */ + driveConfig.Slot0.kP = Constants.Swerve.driveKP; + driveConfig.Slot0.kI = Constants.Swerve.driveKI; + driveConfig.Slot0.kD = Constants.Swerve.driveKD; + driveConfig.Slot0.kS = Constants.Swerve.driveKS; + driveConfig.Slot0.kV = Constants.Swerve.driveKV; + driveConfig.Slot0.kA = Constants.Swerve.driveKA; + + /* Open and Closed Loop Ramping */ + driveConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; + driveConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = Constants.Swerve.openLoopRamp; + + driveConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = Constants.Swerve.closedLoopRamp; + driveConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = Constants.Swerve.closedLoopRamp; + + PhoenixSignals.tryUntilOk(5, () -> driveMotor.getConfigurator().apply(driveConfig, 0.25)); + PhoenixSignals.tryUntilOk(5, () -> driveMotor.getConfigurator().setPosition(0.0, 0.25)); + } + + private void configAngleMotor() { + /* Angle Motor Config */ + /* Motor Inverts and Neutral Mode */ + angleConfig.MotorOutput.Inverted = Constants.Swerve.angleMotorInvert; + angleConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode; + + /* Gear Ratio and Wrapping Config */ + angleConfig.Feedback.FeedbackRemoteSensorID = absoluteEncoder.getDeviceID(); + angleConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; + angleConfig.Feedback.SensorToMechanismRatio = 1.0; + angleConfig.Feedback.RotorToSensorRatio = Constants.Swerve.angleGearRatio; + angleConfig.ClosedLoopGeneral.ContinuousWrap = true; + + /* Current Limiting */ + angleConfig.CurrentLimits.SupplyCurrentLimitEnable = + Constants.Swerve.angleEnableCurrentLimit; + angleConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.angleCurrentLimit; + angleConfig.CurrentLimits.SupplyCurrentLowerLimit = Constants.Swerve.angleCurrentLowerLimit; + angleConfig.CurrentLimits.SupplyCurrentLowerTime = + Constants.Swerve.angleCurrentLowerTimeThreshold; + + /* PID Config */ + angleConfig.Slot0.kP = Constants.Swerve.angleKP; + angleConfig.Slot0.kI = Constants.Swerve.angleKI; + angleConfig.Slot0.kD = Constants.Swerve.angleKD; + + PhoenixSignals.tryUntilOk(5, () -> angleMotor.getConfigurator().apply(angleConfig)); + } + + private void configAngleEncoder() { + /* Angle Encoder Config */ + absoluteConfig.MagnetSensor.SensorDirection = Constants.Swerve.cancoderInvert; + absoluteConfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 0.5; + absoluteConfig.MagnetSensor.MagnetOffset = -angleOffset.getRotations(); + + PhoenixSignals.tryUntilOk(5, + () -> absoluteEncoder.getConfigurator().apply(absoluteConfig, 0.25)); + } + + @Override + public void updateInputs(SwerveModuleInputs inputs) { + // Drive Motor inputs + inputs.driveConnected = BaseStatusSignal.isAllGood(drivePosition, driveVelocity, + driveAppliedVolts, driveSupplyCurrentAmps, driveStatorCurrentAmps); + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()); + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveSupplyCurrentAmps = driveSupplyCurrentAmps.getValueAsDouble(); + inputs.driveStatorCurrentAmps = driveStatorCurrentAmps.getValueAsDouble(); + + // Angle Motor inputs + inputs.angleConnected = BaseStatusSignal.isAllGood(anglePosition, angleVelocity, + angleAppliedVolts, angleSupplyCurrentAmps, angleStatorCurrentAmps); + inputs.anglePosition = Rotation2d.fromRotations(anglePosition.getValueAsDouble()); + inputs.angleVelocityRadPerSec = Units.rotationsToRadians(angleVelocity.getValueAsDouble()); + inputs.angleAppliedVolts = angleAppliedVolts.getValueAsDouble(); + inputs.angleSupplyCurrentAmps = angleSupplyCurrentAmps.getValueAsDouble(); + inputs.angleStatorCurrentAmps = angleStatorCurrentAmps.getValueAsDouble(); + + // Encoder inputs + inputs.absoluteAngleConnected = BaseStatusSignal.isAllGood(absolutePosition); + inputs.angleAbsolutePosition = + Rotation2d.fromRotations(absolutePosition.getValueAsDouble()); + + // Odometry inputs + inputs.odometryDrivePositionsRad = + drivePositionQueue.stream().mapToDouble(Units::rotationsToRadians).toArray(); + inputs.odometryAnglePositions = + anglePositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new); + drivePositionQueue.clear(); + anglePositionQueue.clear(); + } + + private final VoltageOut driveVoltage = new VoltageOut(0.0); + + @Override + public void runDriveOpenLoop(double output) { + driveMotor.setControl(driveVoltage.withOutput(output)); + } + + private final VoltageOut angleVoltage = new VoltageOut(0.0); + + @Override + public void runAngleOpenLoop(double output) { + angleMotor.setControl(angleVoltage.withOutput(output)); + } + + private final VelocityVoltage driveVelocityVoltage = new VelocityVoltage(0.0); + + @Override + public void runDriveVelocity(double velocityRadPerSec, double feedforward) { + driveMotor.setControl( + driveVelocityVoltage.withVelocity(velocityRadPerSec).withFeedForward(feedforward)); + } + + private final PositionVoltage anglePositionVoltage = new PositionVoltage(0.0); + + @Override + public void runAnglePosition(Rotation2d rotation) { + angleMotor.setControl(anglePositionVoltage.withPosition(rotation.getRotations())); + } + + @Override + public void setDrivePID(double kP, double kI, double kD, double kS, double kV, double kA) { + driveConfig.Slot0.kP = kP; + driveConfig.Slot0.kI = kI; + driveConfig.Slot0.kD = kD; + driveConfig.Slot0.kS = kS; + driveConfig.Slot0.kV = kV; + driveConfig.Slot0.kA = kA; + PhoenixSignals.tryUntilOk(5, () -> driveMotor.getConfigurator().apply(driveConfig, 0.25)); + } + + @Override + public void setAnglePID(double kP, double kI, double kD) { + angleConfig.Slot0.kP = kP; + angleConfig.Slot0.kI = kI; + angleConfig.Slot0.kD = kD; + PhoenixSignals.tryUntilOk(5, () -> angleMotor.getConfigurator().apply(angleConfig, 0.25)); + } + + private static final Executor brakeModeExecutor = Executors.newFixedThreadPool(8); + + @Override + public void setDriveBrakeMode(boolean enabled) { + brakeModeExecutor.execute(() -> { + synchronized (driveConfig) { + driveConfig.MotorOutput.NeutralMode = + enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast; + PhoenixSignals.tryUntilOk(5, + () -> driveMotor.getConfigurator().apply(driveConfig, 0.25)); + } + }); + } + + @Override + public void setAngleBrakeMode(boolean enabled) { + brakeModeExecutor.execute(() -> { + synchronized (angleConfig) { + angleConfig.MotorOutput.NeutralMode = + enabled ? NeutralModeValue.Brake : NeutralModeValue.Coast; + PhoenixSignals.tryUntilOk(5, + () -> angleMotor.getConfigurator().apply(angleConfig, 0.25)); + } + }); + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleSim.java b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleSim.java new file mode 100644 index 0000000..5763a8c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/mod/SwerveModuleSim.java @@ -0,0 +1,142 @@ +package frc.robot.subsystems.swerve.mod; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.units.Units; +import frc.robot.Constants; + +/** Simulation implementation for Swerve Module */ +@NullMarked +public class SwerveModuleSim implements SwerveModuleIO { + + private final SwerveModuleSimulation moduleSimulation; + private final SimulatedMotorController.GenericMotorController driveMotor; + private final SimulatedMotorController.GenericMotorController turnMotor; + + private final int id; + + private boolean driveClosedLoop = false; + private boolean turnClosedLoop = false; + private final PIDController driveController; + private final PIDController turnController; + private double driveFFVolts = 0.0; + private double driveAppliedVolts = 0.0; + private double turnAppliedVolts = 0.0; + + private double kV = 1.0; + private double targetVelocity = 0.0; + + /** Simulation implementation for Swerve Module */ + public SwerveModuleSim(int index, SwerveModuleSimulation modSim) { + this.id = index; + this.moduleSimulation = modSim; + this.driveMotor = moduleSimulation.useGenericMotorControllerForDrive() + .withCurrentLimit(Amps.of(Constants.Swerve.driveCurrentLimit)); + this.turnMotor = + moduleSimulation.useGenericControllerForSteer().withCurrentLimit(Amps.of(20)); + + this.driveController = new PIDController(0.5, 0.0, 0.0); + this.turnController = new PIDController(8.0, 0.0, 0.0); + + // Enable wrapping for turn PID + turnController.enableContinuousInput(-Math.PI, Math.PI); + } + + @Override + public void updateInputs(SwerveModuleInputs inputs) { + if (driveClosedLoop) { + if (Math.abs(driveController.getSetpoint()) < 0.01) { + driveAppliedVolts = 0.0; + driveController.reset(); + } else { + driveAppliedVolts = driveFFVolts + + driveController.calculate( + moduleSimulation.getDriveWheelFinalSpeed().in(Units.RadiansPerSecond)) + + kV * targetVelocity; + } + } else { + driveController.reset(); + } + if (turnClosedLoop) { + turnAppliedVolts = + turnController.calculate(moduleSimulation.getSteerAbsoluteFacing().getRadians()); + } else { + turnController.reset(); + } + + Logger.recordOutput("driveVoltage" + id, driveAppliedVolts); + Logger.recordOutput("angleVoltage" + id, turnAppliedVolts); + + driveMotor.requestVoltage(Units.Volts.of(driveAppliedVolts)); + turnMotor.requestVoltage(Units.Volts.of(turnAppliedVolts)); + + inputs.driveConnected = true; + inputs.angleConnected = true; + inputs.absoluteAngleConnected = true; + + inputs.drivePositionRad = moduleSimulation.getDriveWheelFinalPosition().in(Radians); + inputs.driveVelocityRadPerSec = + moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond); + inputs.angleAbsolutePosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.anglePosition = inputs.angleAbsolutePosition; + inputs.angleVelocityRadPerSec = + moduleSimulation.getSteerAbsoluteEncoderSpeed().in(RadiansPerSecond); + + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryDriveVelocityRadsPerSec = new double[] {inputs.driveVelocityRadPerSec}; + inputs.odometryAnglePositions = new Rotation2d[] {inputs.anglePosition}; + } + + @Override + public void runDriveOpenLoop(double output) { + driveClosedLoop = false; + driveAppliedVolts = output; + } + + @Override + public void runAngleOpenLoop(double output) { + turnClosedLoop = false; + turnAppliedVolts = output; + } + + @Override + public void runDriveVelocity(double velocityRadPerSec, double feedforward) { + driveClosedLoop = true; + driveController.setSetpoint(velocityRadPerSec); + targetVelocity = velocityRadPerSec; + } + + @Override + public void runAnglePosition(Rotation2d rotation) { + turnClosedLoop = true; + turnController.setSetpoint(rotation.getRadians()); + } + + @Override + public void setDrivePID(double kP, double kI, double kD, double kS, double kV, double kA) { + // Changing PID not handled in sim + } + + @Override + public void setAnglePID(double kP, double kI, double kD) { + // Changing PID not handled in sim + } + + @Override + public void setDriveBrakeMode(boolean enabled) { + // Brake mode not handled in sim + } + + @Override + public void setAngleBrakeMode(boolean enabled) { + // Brake mode not handled in sim + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/util/MoveToPose.java b/src/main/java/frc/robot/subsystems/swerve/util/MoveToPose.java new file mode 100644 index 0000000..53adf77 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/util/MoveToPose.java @@ -0,0 +1,191 @@ +package frc.robot.subsystems.swerve.util; + +import java.util.function.Consumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.jspecify.annotations.NullMarked; +import org.jspecify.annotations.Nullable; +import choreo.auto.AutoRoutine; +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; +import edu.wpi.first.wpilibj.event.EventLoop; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.Swerve; +import frc.robot.util.AllianceFlipUtil; +import frc.robot.util.typestate.AltMethod; +import frc.robot.util.typestate.InitField; +import frc.robot.util.typestate.OptionalField; +import frc.robot.util.typestate.RequiredField; +import frc.robot.util.typestate.TypeStateBuilder; + +/** + * Command that drives a swerve drivetrain to a specified field-relative pose using a + * {@link HolonomicDriveController}. + * + *

+ * This command continuously computes robot-relative chassis speeds that reduce translational and + * rotational error to a target {@link Pose2d}. The command finishes once the robot is within the + * specified translation and rotation tolerances. + * + *

+ * The target pose may be supplied dynamically (e.g., from vision or an autonomous routine). An + * optional alliance-based pose flip can be applied automatically for red alliance operation. + * + *

+ * Output chassis speeds are robot-relative and are clamped to a configurable maximum translational + * speed. + * + *

+ * This command is typically created via {@code Swerve.moveToPose(...)} and should not be + * instantiated directly. + */ +@NullMarked +public class MoveToPose extends Command { + + private final AutoRoutine autoRoutine; + private final EventLoop eventLoop; + + private final Swerve swerve; + private final Consumer robotRelativeConsumer; + private final Supplier pose2dSupplier; + private final DoubleSupplier maxSpeedSupplier; + private final boolean flipForRed; + private final double translationTolerance; + private final double rotationTolerance; + + private boolean isActive = false; + private boolean isCompleted = false; + + /** + * Constructs a command that drives the robot to a target pose. + * + *

+ * Note: This constructor is intended for use by generated builders. Prefer + * {@code Swerve.moveToPose(...)} when creating this command. + * + * @param swerve drivetrain subsystem providing pose estimation + * @param robotRelativeConsumer consumer that accepts robot-relative chassis speeds + * @param target supplier providing the desired field-relative target pose + * @param autoRoutine optional autonomous routine used to scope triggers + * @param maxSpeed supplier providing the maximum allowed translational speed (m/s) + * @param flipForRed whether to mirror the target pose for red alliance + * @param translationTolerance allowable positional error (meters) + * @param rotationTolerance allowable angular error (radians) + */ + @TypeStateBuilder("MoveToPoseBuilder") + public MoveToPose(@InitField Swerve swerve, + @InitField Consumer robotRelativeConsumer, + @RequiredField(alt = @AltMethod(type = Pose2d.class, parameter_name = "targetConst", + value = "() -> targetConst")) Supplier target, + @OptionalField("null") @Nullable AutoRoutine autoRoutine, + @OptionalField(value = "() -> frc.robot.Constants.Swerve.autoMaxSpeed", + alt = @AltMethod(type = double.class, parameter_name = "maxSpeedConst", + value = "() -> maxSpeedConst")) DoubleSupplier maxSpeed, + @OptionalField("true") boolean flipForRed, + @OptionalField("0.5") double translationTolerance, + @OptionalField("edu.wpi.first.math.util.Units.degreesToRadians(5)") double rotationTolerance) { + this.autoRoutine = autoRoutine; + if (autoRoutine == null) { + this.eventLoop = CommandScheduler.getInstance().getDefaultButtonLoop(); + } else { + this.eventLoop = autoRoutine.loop(); + } + this.swerve = swerve; + this.robotRelativeConsumer = robotRelativeConsumer; + this.pose2dSupplier = target; + this.maxSpeedSupplier = maxSpeed; + this.flipForRed = flipForRed; + this.translationTolerance = translationTolerance; + this.rotationTolerance = rotationTolerance; + } + + /** + * Returns a trigger that is {@code true} while this command is actively running. + * + *

+ * If associated with an {@link AutoRoutine}, the trigger is also gated by the routine's active + * state. + * + * @return trigger indicating whether the command is currently active + */ + public Trigger active() { + if (autoRoutine != null) { + return new Trigger(eventLoop, + () -> this.isActive && autoRoutine.active().getAsBoolean()); + } + return new Trigger(eventLoop, () -> this.isActive); + } + + /** + * Returns a trigger that becomes {@code true} once the command has completed successfully. + * + *

+ * The trigger remains {@code true} until the command is restarted. + * + * @return trigger indicating completion of the command + */ + public Trigger done() { + return new Trigger(eventLoop, () -> this.isCompleted); + } + + @Override + public void initialize() { + isActive = true; + isCompleted = false; + } + + private static final HolonomicDriveController holonomicDriveController = + new HolonomicDriveController(new PIDController(Constants.SwerveTransformPID.translationP, + Constants.SwerveTransformPID.translationI, Constants.SwerveTransformPID.translationD), + new PIDController(Constants.SwerveTransformPID.translationP, + Constants.SwerveTransformPID.translationI, + Constants.SwerveTransformPID.translationD), + new ProfiledPIDController(Constants.SwerveTransformPID.rotationP, + Constants.SwerveTransformPID.rotationI, Constants.SwerveTransformPID.rotationD, + new Constraints(Constants.SwerveTransformPID.maxAngularVelocity, + Constants.SwerveTransformPID.maxAngularAcceleration))); + + private Pose2d target = Pose2d.kZero; + + @Override + public void execute() { + target = this.pose2dSupplier.get(); + if (flipForRed) { + target = AllianceFlipUtil.apply(target); + } + ChassisSpeeds ctrlEffort = holonomicDriveController + .calculate(swerve.state.getGlobalPoseEstimate(), target, 0, target.getRotation()); + double speed = Math.hypot(ctrlEffort.vxMetersPerSecond, ctrlEffort.vyMetersPerSecond); + double maxSpeed = this.maxSpeedSupplier.getAsDouble(); + if (speed > maxSpeed) { + double mul = maxSpeed / speed; + ctrlEffort.vxMetersPerSecond *= mul; + ctrlEffort.vyMetersPerSecond *= mul; + } + this.robotRelativeConsumer.accept(ctrlEffort); + } + + @Override + public void end(boolean interrupted) { + isActive = false; + isCompleted = !interrupted; + } + + @Override + public boolean isFinished() { + Pose2d poseError = Pose2d.kZero.plus(target.minus(swerve.state.getGlobalPoseEstimate())); + final var eTranslate = poseError.getTranslation(); + final var eRotate = poseError.getRotation(); + return Math.abs(eTranslate.getX()) < translationTolerance + && Math.abs(eTranslate.getY()) < translationTolerance + && Math.abs(eRotate.getDegrees()) < rotationTolerance; + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/util/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/swerve/util/PhoenixOdometryThread.java new file mode 100644 index 0000000..7036005 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/util/PhoenixOdometryThread.java @@ -0,0 +1,233 @@ +package frc.robot.subsystems.swerve.util; + +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; +import java.util.function.DoubleSupplier; +import org.jspecify.annotations.NullMarked; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import edu.wpi.first.wpilibj.RobotController; +import frc.robot.Constants; + +/** + * Background thread responsible for sampling odometry-related signals at a higher frequency than + * the main robot loop. + * + *

+ * This thread is primarily intended to reduce latency and aliasing when collecting drivetrain + * sensor data (e.g., wheel encoders, gyro rates) used for odometry and state estimation. + * + *

+ * Two categories of signals may be registered: + *

    + *
  • Phoenix signals - {@link StatusSignal} instances that can be synchronized and + * timestamped by the CTRE Phoenix framework
    • + *
  • Generic signals - arbitrary {@link DoubleSupplier}s sampled at the same rate as + * Phoenix signals
    • + *
+ * + *

+ * Each registered signal is associated with a thread-safe {@link Queue} into which samples are + * pushed. Consumers are expected to poll these queues from the main loop while holding the provided + * odometry lock. + * + *

+ * If running on a CAN-FD network, Phoenix signals are synchronized using + * {@link BaseStatusSignal#waitForAll(double, BaseStatusSignal...)}, allowing all signals to be + * sampled at nearly the same timestamp. On non-CAN-FD networks, the thread sleeps for the + * configured odometry period and explicitly refreshes signals. + * + *

+ * The thread runs as a daemon and will terminate automatically when the JVM exits. + */ +@NullMarked +public class PhoenixOdometryThread extends Thread { + + /** Lock shared with odometry consumers to ensure consistent sampling */ + private final Lock odometryLock; + + /** Lock protecting signal registration and Phoenix signal refresh */ + private final Lock signalsLock = new ReentrantLock(); + + /** Registered Phoenix status signals */ + private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0]; + /** Registered non-Phoenix signal suppliers */ + private final List genericSignals = new ArrayList<>(); + /** Output queues corresponding to Phoenix signals */ + private final List> phoenixQueues = new ArrayList<>(); + /** Output queues corresponding to generic signals */ + private final List> genericQueues = new ArrayList<>(); + /** Queues receiving timestamps associated with each sample */ + private final List> timestampQueues = new ArrayList<>(); + + /** Whether the active CAN bus supports CAN-FD synchronization */ + private final boolean isCANFD = new CANBus("*").isNetworkFD(); + + /** + * Creates a new odometry sampling thread. + * + * @param odometryLock lock shared with consumers to ensure that sampled values and timestamps + * are read atomically + */ + public PhoenixOdometryThread(Lock odometryLock) { + this.odometryLock = odometryLock; + this.setName("PhoenixOdometryThread"); + this.setDaemon(true); + } + + /** + * Starts the thread if at least one signal has been registered. + * + *

+ * This prevents spawning an idle background thread when no signals are being sampled. + */ + @Override + public synchronized void start() { + // Only start the thread if stuff is actually registered. + if (phoenixQueues.size() > 0 || genericQueues.size() > 0) { + super.start(); + } + } + + /** + * Registers a Phoenix {@link StatusSignal} to be sampled by the thread. + * + *

+ * The returned queue will receive one value per odometry tick. + * + *

+ * This method is thread-safe and may be called before the thread starts. + * + * @param signal Phoenix status signal to sample + * @return queue containing sampled signal values + */ + public Queue registerSignal(StatusSignal signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + odometryLock.lock(); + try { + BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; + System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); + newSignals[phoenixSignals.length] = signal; + phoenixSignals = newSignals; + phoenixQueues.add(queue); + } finally { + signalsLock.unlock(); + odometryLock.unlock(); + } + return queue; + } + + /** + * Registers a generic value supplier to be sampled at the odometry rate. + * + *

+ * This is useful for signals not managed by Phoenix (e.g., WPILib sensors or computed values). + * + * @param signal supplier producing the value to sample + * @return queue containing sampled values + */ + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(20); + signalsLock.lock(); + odometryLock.lock(); + try { + genericSignals.add(signal); + genericQueues.add(queue); + } finally { + signalsLock.unlock(); + odometryLock.unlock(); + } + return queue; + } + + /** + * Creates a queue that receives timestamps corresponding to each sampling cycle. + * + *

+ * The timestamp represents FPGA time in seconds, adjusted for average Phoenix signal latency + * when applicable. + * + * @return queue of sample timestamps (seconds) + */ + public Queue makeTimestampQueue() { + Queue queue = new ArrayBlockingQueue<>(20); + odometryLock.lock(); + try { + timestampQueues.add(queue); + } finally { + odometryLock.unlock(); + } + return queue; + } + + /** + * Main sampling loop. + * + *

+ * The thread runs indefinitely, sampling all registered signals at + * {@code Constants.Swerve.odometryFrequency}. + * + *

+ * Sampling behavior differs depending on CAN capabilities: + *

    + *
  • CAN-FD: waits for all Phoenix signals to update together
    • + *
  • Non-CAN-FD: sleeps for the target period and manually refreshes
    • + *
+ * + *

+ * All sampled values and timestamps are enqueued atomically under the odometry lock to ensure + * consistency for consumers. + */ + @Override + public void run() { + while (true) { + signalsLock.lock(); + try { + if (isCANFD && phoenixSignals.length > 0) { + BaseStatusSignal.waitForAll(2.0 / Constants.Swerve.odometryFrequency, + phoenixSignals); + } else { + Thread.sleep((long) (1000.0 / Constants.Swerve.odometryFrequency)); + if (phoenixSignals.length > 0) { + BaseStatusSignal.refreshAll(phoenixSignals); + } + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + + odometryLock.lock(); + try { + double timestamp = RobotController.getFPGATime() / 1e6; + double totalLatency = 0.0; + for (BaseStatusSignal signal : phoenixSignals) { + totalLatency += signal.getTimestamp().getLatency(); + } + if (phoenixSignals.length > 0) { + timestamp -= totalLatency / phoenixSignals.length; + } + + for (int i = 0; i < phoenixSignals.length; i++) { + phoenixQueues.get(i).offer(phoenixSignals[i].getValueAsDouble()); + } + for (int i = 0; i < genericSignals.size(); i++) { + genericQueues.get(i).offer(genericSignals.get(i).getAsDouble()); + } + for (int i = 0; i < timestampQueues.size(); i++) { + timestampQueues.get(i).offer(timestamp); + } + } finally { + odometryLock.unlock(); + } + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/util/SwerveArcOdometry.java b/src/main/java/frc/robot/subsystems/swerve/util/SwerveArcOdometry.java new file mode 100644 index 0000000..c3e85e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/util/SwerveArcOdometry.java @@ -0,0 +1,190 @@ +package frc.robot.subsystems.swerve.util; + +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.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; + +/** + * Swerve drive odometry implementation that integrates wheel motion using circular arc geometry + * rather than straight-line approximations. + * + *

+ * Inspired by Team 1690, this odometry model treats each swerve module's motion between updates as + * an arc defined by the change in wheel angle and distance traveled. This more accurately + * represents real swerve motion, especially during simultaneous translation and rotation, where + * straight-line assumptions can introduce measurable integration error. + * + *

+ * Compared to {@link SwerveDriveOdometry}, which assumes each module travels in a straight line + * over a timestep, this class: + *

    + *
  • Computes per-module displacement along a circular arc
    • + *
  • Transforms those displacements into the field frame
    • + *
  • Averages the resulting module displacements to update the robot pose
    • + *
+ * + *

+ * The robot heading is still sourced directly from the gyro, while translation is derived purely + * from module motion. + * + *

+ * This class is intended as a drop-in replacement for {@link SwerveDriveOdometry} when improved + * accuracy is desired at the cost of slightly increased computation. + */ +public final class SwerveArcOdometry extends SwerveDriveOdometry { + private final int numberOfModules; + private final Translation2d[] robotRelativeModuleOffsets; + + private Pose2d robotPose = Pose2d.kZero; + private final SwerveModulePosition[] previousWheelPositions; + + /** + * Constructs a new arc-based swerve odometry instance. + * + * @param kinematics the swerve drive kinematics describing module locations + * @param gyroAngle the initial robot heading from the gyro + * @param modulePositions the initial positions of each swerve module + */ + public SwerveArcOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle, + SwerveModulePosition[] modulePositions) { + super(kinematics, gyroAngle, modulePositions); + numberOfModules = kinematics.getModules().length; + robotRelativeModuleOffsets = kinematics.getModules(); + + previousWheelPositions = new SwerveModulePosition[numberOfModules]; + for (int i = 0; i < numberOfModules; i++) { + previousWheelPositions[i] = new SwerveModulePosition(); + } + } + + /** + * Computes the 2D displacement of a swerve module by integrating its motion along a circular + * arc. + * + *

+ * The arc is defined by the change in module steering angle and the change in wheel distance + * between two samples. If the steering angle does not change, the motion is treated as a + * straight line for efficiency. + * + *

+ * This calculation assumes the module follows a constant-curvature path over the timestep, + * which more closely matches real swerve behavior during rotation. + * + * @param previousWheelPosition the module position at the previous update + * @param currentWheelPosition the module position at the current update + * @return the module's displacement in the robot-relative frame + */ + private static Translation2d getModuleDisplacement(SwerveModulePosition previousWheelPosition, + SwerveModulePosition currentWheelPosition) { + // First, calculate difference between previous and current angles and distances + double angleDifferenceRadians = + currentWheelPosition.angle.getRadians() - previousWheelPosition.angle.getRadians(); + double arcLength = + currentWheelPosition.distanceMeters - previousWheelPosition.distanceMeters; + + // *If angle difference is 0 then we can just use a straight line instead of an arc + if (angleDifferenceRadians == 0) { + return new Translation2d(arcLength, currentWheelPosition.angle); + } + + // Next, calculate radius. Positive = left turn, negative = right turn + double radius = (arcLength / angleDifferenceRadians); + + // Then, calculate the center point of the circle that the arc is a part of, using the + // previous + // angle. The previous module translation is (0, 0) because we don't care where it starts, + // only + // the displacement. It is also always perpendicular to the previous angle, with + // positive/negative radius indicating which side of the module that the circle center will + // be + // on + double circleCenterX = -radius * previousWheelPosition.angle.getSin(); + double circleCenterY = radius * previousWheelPosition.angle.getCos(); + + // Finally, calculate the current module translation on the arc and return it as module + // displacement + double displacementX = circleCenterX + radius * currentWheelPosition.angle.getSin(); + double displacementY = circleCenterY - radius * currentWheelPosition.angle.getCos(); + + return new Translation2d(displacementX, displacementY); + } + + @Override + public Pose2d update(Rotation2d currentGyroAngle, + SwerveModulePosition[] currentWheelPositions) { + // First, get the field relative module poses of the previous robot pose, and apply robot + // relative module offsets + Pose2d[] fieldRelativeModulePosesOfPreviousPose = new Pose2d[numberOfModules]; + for (int i = 0; i < numberOfModules; i++) { + fieldRelativeModulePosesOfPreviousPose[i] = robotPose + .transformBy(new Transform2d(robotRelativeModuleOffsets[i], Rotation2d.kZero)); + } + + // Also get the module displacements from the previous wheel positions to the current wheel + // positions + Translation2d[] moduleDisplacements = new Translation2d[numberOfModules]; + for (int i = 0; i < numberOfModules; i++) { + moduleDisplacements[i] = + getModuleDisplacement(previousWheelPositions[i], currentWheelPositions[i]); + } + + // Next, add the module displacements to the field relative module poses + Translation2d[] fieldRelativeModuleDisplacements = new Translation2d[numberOfModules]; + for (int i = 0; i < numberOfModules; i++) { + fieldRelativeModuleDisplacements[i] = fieldRelativeModulePosesOfPreviousPose[i] + .transformBy(new Transform2d(moduleDisplacements[i], Rotation2d.kZero)) + .getTranslation(); + } + + // Finally, average the module displacements and return the new pose + Translation2d sumOfFieldRelativeModuleDisplacements = new Translation2d(); + for (int i = 0; i < numberOfModules; i++) { + sumOfFieldRelativeModuleDisplacements = + sumOfFieldRelativeModuleDisplacements.plus(fieldRelativeModuleDisplacements[i]); + } + double updatedPoseX = sumOfFieldRelativeModuleDisplacements.getX() / numberOfModules; + double updatedPoseY = sumOfFieldRelativeModuleDisplacements.getY() / numberOfModules; + var updatedPose = new Pose2d(updatedPoseX, updatedPoseY, currentGyroAngle); + + // After calculations, but before the next loop, update the previous pose & wheel positions + // to + // the current ones + robotPose = updatedPose; + for (int i = 0; i < numberOfModules; i++) { + previousWheelPositions[i] = currentWheelPositions[i]; + } + + return updatedPose; + } + + @Override + public void resetPosition(Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions, + Pose2d poseMeters) { + robotPose = new Pose2d(poseMeters.getTranslation(), gyroAngle); + } + + @Override + public void resetPose(Pose2d poseMeters) { + robotPose = poseMeters; + } + + @Override + public void resetTranslation(Translation2d translation) { + robotPose = new Pose2d(translation, robotPose.getRotation()); + } + + @Override + public void resetRotation(Rotation2d rotation) { + robotPose = new Pose2d(robotPose.getTranslation(), rotation); + } + + @Override + public Pose2d getPoseMeters() { + return robotPose; + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/util/SwerveRateLimiter.java b/src/main/java/frc/robot/subsystems/swerve/util/SwerveRateLimiter.java new file mode 100644 index 0000000..ba4d6a5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/util/SwerveRateLimiter.java @@ -0,0 +1,225 @@ +package frc.robot.subsystems.swerve.util; + +import java.util.EnumSet; +import org.ejml.data.DMatrix3; +import org.ejml.dense.fixed.CommonOps_DDF3; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.networktables.NetworkTableEvent.Kind; +import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.Constants; + +/** + * Applies rate limiting to robot-relative swerve chassis commands to ensure physically achievable, + * stable, and predictable motion. + * + *

+ * This limiter constrains translational acceleration using a multi-stage process inspired by Team + * 1690's swerve control methodology: + *

    + *
  • Limits forward acceleration based on current speed and remaining headroom to maximum + * velocity
    • + *
  • Clamps directional acceleration to prevent excessive robot tilt
    • + *
  • Caps overall acceleration magnitude to avoid wheel slip and skidding
    • + *
+ * + *

+ * The limiter operates in discrete time (20 ms control loop) and should be called once per + * cycle. The current robot velocity must be provided via {@link #update(ChassisSpeeds)} before + * calling {@link #limit(ChassisSpeeds)}. + * + *

+ * All limits are exposed via NetworkTables under {@code /SwerveRateLimiter/*} and may be tuned at + * runtime. Diagnostic values are published to AdvantageKit logs for visualization and debugging. + * + *

+ * Rotational velocity ({@code omegaRadiansPerSecond}) is passed through unchanged and is not + * subject to rate limiting. + * + *

+ * Usage pattern: + * + * 

{@code
+ * limiter.update(currentRobotRelativeSpeeds);
+ * ChassisSpeeds safeSpeeds = limiter.limit(desiredSpeeds);
+ * }
+ * + *

+ * Based on concepts presented by FRC Team 1690: + * + * https://www.youtube.com/watch?v=vUtVXz7ebEE + */ +@NullMarked +public class SwerveRateLimiter { + + private double forwardLimit = Constants.Swerve.forwardLimit; + private double forwardTiltLimit = Constants.Swerve.forwardTiltLimit; + private double leftTiltLimit = Constants.Swerve.leftTiltLimit; + private double rightTiltLimit = Constants.Swerve.rightTiltLimit; + private double backTiltLimit = Constants.Swerve.backTiltLimit; + private double skidLimit = Constants.Swerve.skidLimit; + + /** + * Creates a new {@code SwerveRateLimiter} and publishes all acceleration limits to + * NetworkTables for live tuning. + * + *

+ * Any updates received via NetworkTables will immediately modify the corresponding limit used + * by the rate limiter. + */ + public SwerveRateLimiter() { + final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); + try { + for (var item : SwerveRateLimiter.class.getDeclaredFields()) { + if (item.getType().equals(double.class)) { + var topic = ntInstance.getDoubleTopic("/SwerveRateLimiter/" + item.getName()); + var publisher = topic.publish(); + var value = (double) item.get(this); + publisher.accept(value); + ntInstance.addListener(topic, EnumSet.of(Kind.kValueAll), (ev) -> { + try { + var newValue = ev.valueData.value.getDouble(); + item.set(this, newValue); + } catch (IllegalArgumentException | IllegalAccessException e) { + e.printStackTrace(); + } + }); + } + } + } catch (IllegalArgumentException | IllegalAccessException e) { + e.printStackTrace(); + } + } + + /** + * Updates the current robot-relative chassis velocity used as the basis for acceleration + * limiting. + * + *

+ * This method should be called once per control loop using velocity data from odometry or state + * estimation before calling {@link #limit(ChassisSpeeds)}. + * + * @param robotRelative the current robot-relative chassis speeds + */ + public void update(ChassisSpeeds robotRelative) { + currentVel.a1 = robotRelative.vxMetersPerSecond; + currentVel.a2 = robotRelative.vyMetersPerSecond; + currentVel.a3 = robotRelative.omegaRadiansPerSecond; + } + + private final DMatrix3 currentVel = new DMatrix3(); + private final DMatrix3 wantedVel = new DMatrix3(); + private final DMatrix3 wantedAcc = new DMatrix3(); + + private void publish(String name, DMatrix3 v) { + Logger.recordOutput("SwerveRateLimiter/" + name, new Twist2d(v.a1, v.a2, v.a3)); + } + + private void publish(String name, double v) { + Logger.recordOutput("SwerveRateLimiter/" + name, v); + } + + /** + * Limits a desired robot-relative chassis velocity based on physical acceleration, tilt, and + * skid constraints, producing a safe and achievable {@link ChassisSpeeds}. + * + *

+ * The limiting process operates in discrete time (20 ms) and applies several sequential + * constraints: + *

    + *
  1. Physical acceleration limit: Caps acceleration in the direction of current motion + * based on available traction and remaining headroom to max speed.
    2. + *
  2. Tilt prevention: Restricts directional acceleration components to prevent + * excessive forward, backward, or lateral tilting.
    3. + *
  3. Skid prevention: Limits the overall translational acceleration magnitude to avoid + * wheel slip.
    4. + *
+ * + *

+ * The resulting acceleration is integrated over one control loop period and added to the + * current velocity to produce the final commanded chassis speeds. + * + *

+ * Rotational velocity ({@code omegaRadiansPerSecond}) is passed through unchanged and is not + * subject to acceleration limiting. + * + * @param wantedSpeedsRobotRelative the desired robot-relative chassis speeds (vx, vy, omega) + * @return a new {@link ChassisSpeeds} representing the limited, physically achievable + * robot-relative velocities for the next control step + */ + public ChassisSpeeds limit(ChassisSpeeds wantedSpeedsRobotRelative) { + double currentSpeed = Math.hypot(currentVel.a1, currentVel.a2); + double wantedSpeed = Math.hypot(wantedSpeedsRobotRelative.vxMetersPerSecond, + wantedSpeedsRobotRelative.vyMetersPerSecond); + + publish("currentSpeed", currentSpeed); + publish("wantedSpeed", wantedSpeed); + + wantedVel.a1 = wantedSpeedsRobotRelative.vxMetersPerSecond; + wantedVel.a2 = wantedSpeedsRobotRelative.vyMetersPerSecond; + wantedVel.a3 = wantedSpeedsRobotRelative.omegaRadiansPerSecond; + publish("wantedVel", wantedVel); + publish("currentVel", currentVel); + + CommonOps_DDF3.subtract(wantedVel, currentVel, wantedAcc); + CommonOps_DDF3.divide(wantedAcc, 0.02); + publish("wantedAccStep0", wantedAcc); + + // Step 1: Robot cannot accelerate indefinitely. Ensure accelerations are achievable + // (sub-max to ensure correctness even under degradation). + double subphysicalAccelerationLimit = + Math.max(1.0 - (currentSpeed / Constants.Swerve.maxSpeed), 0.0); + publish("subphysicalAccelerationLimit", subphysicalAccelerationLimit); + double maxForwardAccel = forwardLimit * subphysicalAccelerationLimit; + publish("maxForwardAccel", maxForwardAccel); + + // get acceleration in direction of current velocity + double wantedAccMagnitude = wantedAcc.a1 * currentVel.a1 / currentSpeed + + wantedAcc.a2 * currentVel.a2 / currentSpeed; + publish("wantedAccMagnitudeStep1", wantedAccMagnitude); + if (wantedAccMagnitude > maxForwardAccel) { + double mul = maxForwardAccel / wantedAccMagnitude; + wantedAcc.a1 *= mul; + wantedAcc.a2 *= mul; + } + publish("wantedAccStep1", wantedAcc); + + // Step 2: Robot may accelerate too fast and result in tilting. Limit directional + // acceleration to prevent this. + if (wantedAcc.a1 > forwardTiltLimit) { + wantedAcc.a1 = forwardLimit; + } + if (wantedAcc.a1 < -backTiltLimit) { + wantedAcc.a1 = -backTiltLimit; + } + if (wantedAcc.a2 > leftTiltLimit) { + wantedAcc.a2 = leftTiltLimit; + } + if (wantedAcc.a2 < -rightTiltLimit) { + wantedAcc.a2 = -rightTiltLimit; + } + publish("wantedAccStep2", wantedAcc); + + // Step 3: Robot accelerating in a different direction may result in skidding. Limit + // magnitude of acceleration to prevent this. + wantedAccMagnitude = Math.hypot(wantedAcc.a1, wantedAcc.a2); + publish("wantedAccMagnitudeStep3", wantedAccMagnitude); + if (wantedAccMagnitude > skidLimit) { + double multiplier = skidLimit / wantedAccMagnitude; + wantedAcc.a1 *= multiplier; + wantedAcc.a2 *= multiplier; + } + publish("wantedAccStep3", wantedAcc); + + CommonOps_DDF3.scale(0.02, wantedAcc); + CommonOps_DDF3.add(currentVel, wantedAcc, wantedVel); + + publish("resultant", wantedVel); + publish("resultantSpeed", Math.hypot(wantedVel.a1, wantedVel.a2)); + + return new ChassisSpeeds(wantedVel.a1, wantedVel.a2, wantedVel.a3); + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/util/SwerveState.java b/src/main/java/frc/robot/subsystems/swerve/util/SwerveState.java new file mode 100644 index 0000000..d3a94cb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/util/SwerveState.java @@ -0,0 +1,320 @@ +package frc.robot.subsystems.swerve.util; + +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Seconds; +import java.util.List; +import java.util.Optional; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.Logger; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.PoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.util.Units; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.Swerve; +import frc.robot.subsystems.vision.CameraConstants; + +/** + * Maintains and updates the robot's estimated global pose for a swerve drive by fusing wheel + * odometry, gyro measurements, and delayed vision updates. + * + *

+ * This class tracks two related poses: + *

    + *
  • Odometry pose - derived purely from wheel encoder deltas and gyro yaw
    • + *
  • Estimated pose - a filtered pose that incorporates vision corrections
    • + *
+ * + *

+ * A time-interpolated pose buffer is maintained to allow vision measurements with latency to be + * applied retroactively at the correct timestamp. + * + *

+ * This implementation resembles a lightweight pose estimator rather than a full Kalman filter, + * using configurable standard deviations to weight vision updates against odometry uncertainty. + */ +@NullMarked +public class SwerveState { + + /** Whether the pose estimator has been initialized from vision */ + private boolean initted = false; + + private final PoseEstimator visionAdjustedOdometry; + + private final TimeInterpolatableBuffer rotationBuffer = + TimeInterpolatableBuffer.createBuffer(1.5); + + /** + * Creates a new swerve state estimator. + * + * @param wheelPositions the initial swerve module positions used to seed odometry + */ + public SwerveState(SwerveModulePosition[] wheelPositions, Rotation2d gyroYaw) { + SwerveDriveOdometry swerveOdometry = + new SwerveArcOdometry(Constants.Swerve.swerveKinematics, gyroYaw, wheelPositions); + visionAdjustedOdometry = new PoseEstimator<>(Constants.Swerve.swerveKinematics, + swerveOdometry, VecBuilder.fill(0.1, 0.1, 0.1), VecBuilder.fill(0.9, 0.9, 0.9)); + } + + /** + * Resets the internal pose estimate to a known field pose. + * + *

+ * This method forces the underlying swerve odometry to the specified pose, effectively + * redefining the robot's position on the field. It should be used when the robot pose is known + * with high confidence, such as: + *

    + *
  • At the start of autonomous
    • + *
  • After a field-aligned reset
    • + *
  • Following a trusted vision-based localization event
    • + *
+ * + *

+ * This method updates only the pose estimator / odometry state owned by {@code SwerveState}. It + * does not update any associated simulation state or drivetrain model. + * + *

+ * Most code should prefer {@link Swerve#overridePose} when resetting the robot pose, as that + * method ensures both the estimator and any simulated drivetrain pose remain consistent. + * + *

+ * Future odometry and vision updates will be applied relative to this new pose. + * + * @param pose the desired robot pose in field coordinates + */ + public void resetPose(Pose2d pose) { + this.visionAdjustedOdometry.resetPose(pose); + rotationBuffer.clear(); + rotationBuffer.getInternalBuffer().clear(); + } + + private Optional sampleRotationAt(double timestampSeconds) { + if (rotationBuffer.getInternalBuffer().isEmpty()) { + return Optional.empty(); + } + + double oldestOdometryTimestamp = rotationBuffer.getInternalBuffer().firstKey(); + double newestOdometryTimestamp = rotationBuffer.getInternalBuffer().lastKey(); + if (oldestOdometryTimestamp > timestampSeconds) { + return Optional.empty(); + } + timestampSeconds = + MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp); + + return rotationBuffer.getSample(timestampSeconds); + } + + /** + * Updates odometry and pose estimates using swerve module encoders and an optional gyro + * measurement. + * + * @param wheelPositions current swerve module positions + * @param gyroYaw current robot yaw, if available + * @param timestamp measurement timestamp in seconds + */ + public void addOdometryObservation(SwerveModulePosition[] wheelPositions, Rotation2d gyroYaw, + double timestamp) { + rotationBuffer.addSample(timestamp, gyroYaw); + visionAdjustedOdometry.update(gyroYaw, wheelPositions); + } + + private ChassisSpeeds currentSpeeds; + + /** + * Updates the robot's current chassis speeds. + * + *

+ * This method records the most recent robot-relative {@link ChassisSpeeds} command or measured + * velocity. The stored speeds may be used by future pose estimation or vision fusion logic to + * account for motion during sensor latency or to improve prediction accuracy. + * + *

+ * This method does not directly affect the current pose estimate. + * + * @param speeds the current robot-relative chassis speeds + */ + public void updateSpeeds(ChassisSpeeds speeds) { + currentSpeeds = speeds; + } + + /** + * Forcibly initializes the pose estimator using a known robot pose. + * + *

+ * This method sets the internal odometry/estimator state directly and marks the estimator as + * initialized, bypassing the normal vision-based initialization path. It is intended for + * situations where a trusted global pose is already known (for example, at the start of + * autonomous) or when a vision needs to be tested on a non-compliant field (for example, the + * practice field at district events). + * + *

+ * After this method is called, subsequent vision updates will be treated as corrections rather + * than initialization measurements. + * + * @param pose the known robot pose in field coordinates to initialize the estimator with + */ + public void overrideInit(Pose2d pose) { + visionAdjustedOdometry.resetPose(pose); + initted = true; + } + + /** + * Adds a vision measurement using an externally computed camera pose. + * + * @param cameraPose estimated camera pose in field coordinates + * @param robotToCamera transform from robot to camera frame + * @param translationStdDev translation measurement standard deviation (meters) + * @param rotationStdDev rotation measurement standard deviation (radians) + * @param timestamp measurement timestamp in seconds + */ + public void addVisionObservation(Pose3d cameraPose, Transform3d robotToCamera, + double translationStdDev, double rotationStdDev, double timestamp) { + Pose2d robotPose = cameraPose.plus(robotToCamera.inverse()).toPose2d(); + Pose2d before = visionAdjustedOdometry.getEstimatedPosition(); + visionAdjustedOdometry.addVisionMeasurement(robotPose, timestamp, + VecBuilder.fill(translationStdDev, translationStdDev, rotationStdDev)); + Pose2d after = visionAdjustedOdometry.getEstimatedPosition(); + double correction = after.getTranslation().getDistance(before.getTranslation()); + Logger.recordOutput("State/Correction", correction); + Logger.recordOutput("State/VisionRobotPose", robotPose); + } + + /** + * Adds a vision measurement from PhotonVision. + * + *

+ * If the estimator has not yet been initialized, a valid multi-tag estimate will be used to + * seed the global pose. + * + * @param camera camera configuration constants + * @param pipelineResult latest PhotonVision pipeline result + */ + public boolean addVisionObservation(CameraConstants camera, + PhotonPipelineResult pipelineResult) { + var multiTag = pipelineResult.getMultiTagResult(); + if (!initted) { + multiTag.ifPresent(multiTag_ -> { + Transform3d best = multiTag_.estimatedPose.best; + Pose3d cameraPose = + new Pose3d().plus(best).relativeTo(Constants.Vision.fieldLayout.getOrigin()); + Pose3d robotPose = cameraPose.plus(camera.robotToCamera.inverse()); + visionAdjustedOdometry.resetPose(robotPose.toPose2d()); + initted = true; + }); + return initted; + } else { + double translationSpeed = + Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); + double rotationSpeed = Math.abs(currentSpeeds.omegaRadiansPerSecond); + double velocityStdDev = camera.simLatencyStdDev.in(Seconds); + double velocityTranslationError = translationSpeed * velocityStdDev; + double velocityRotationError = rotationSpeed * velocityStdDev; + Logger.recordOutput("State/velocityTranslationError", velocityTranslationError); + Logger.recordOutput("State/velocityRotationError", velocityRotationError); + if (multiTag.isPresent()) { + // Multi Tag + Transform3d best = multiTag.get().estimatedPose.best; + Pose3d cameraPose = + new Pose3d().plus(best).relativeTo(Constants.Vision.fieldLayout.getOrigin()); + double stdDevMultiplier = stdDevMultiplier(pipelineResult.targets, cameraPose); + double translationStdDev = + stdDevMultiplier * velocityTranslationError + camera.translationError; + double rotationStdDev = + stdDevMultiplier * velocityRotationError + camera.rotationError; + Logger.recordOutput("State/stdDevMultipler", stdDevMultiplier); + Logger.recordOutput("State/stdDevTranslation", translationStdDev); + Logger.recordOutput("State/stdDevRotation", rotationStdDev); + addVisionObservation(cameraPose, camera.robotToCamera, translationStdDev, + rotationStdDev, pipelineResult.getTimestampSeconds()); + return true; + } else if (rotationSpeed < Units.degreesToRadians(3)) { + // Single Tag + PhotonTrackedTarget target = pipelineResult.getBestTarget(); + if (target == null) { + return false; + } + Optional yawSample = + sampleRotationAt(pipelineResult.getTimestampSeconds()); + if (!yawSample.isPresent()) { + return false; + } + Optional maybePose = + Constants.Vision.fieldLayout.getTagPose(target.getFiducialId()); + if (!maybePose.isPresent()) { + return false; + } + double distance = target.getBestCameraToTarget().getTranslation().getNorm(); + Rotation3d targetInCameraFrame = new Rotation3d(Radians.of(0.0), + Degrees.of(-target.getPitch()), Degrees.of(-target.getYaw())); + Rotation3d cameraRotationInWorldFrame = + camera.robotToCamera.getRotation().rotateBy(new Rotation3d(yawSample.get())); + Translation3d debugTranslation = + new Pose3d(getGlobalPoseEstimate()).plus(camera.robotToCamera).getTranslation(); + Logger.recordOutput("State/singleTagCameraRotationInWorldFrame", + new Pose3d(debugTranslation, cameraRotationInWorldFrame)); + Rotation3d targetRotationInWorldFrame = + targetInCameraFrame.plus(cameraRotationInWorldFrame); + Logger.recordOutput("State/singleTagTargetRotationInWorldFrame", + new Pose3d(debugTranslation, targetRotationInWorldFrame)); + Translation3d cameraToTargetInWorldFrame = + new Translation3d(distance, targetRotationInWorldFrame); + Logger.recordOutput("State/singleTagTargetVector", new Translation3d[] { + debugTranslation, debugTranslation.plus(cameraToTargetInWorldFrame)}); + Translation2d cameraPosition = maybePose.get().getTranslation() + .minus(cameraToTargetInWorldFrame).toTranslation2d(); + Pose3d cameraPose = new Pose3d(cameraPosition.getX(), cameraPosition.getY(), + camera.robotToCamera.getZ(), cameraRotationInWorldFrame); + Logger.recordOutput("State/singleTagCameraPose", cameraPose); + double stdDevMultiplier = stdDevMultiplier(pipelineResult.targets, cameraPose); + double translationStdDev = + stdDevMultiplier * camera.singleTagError + velocityTranslationError; + Logger.recordOutput("State/stdDevMultipler", stdDevMultiplier); + Logger.recordOutput("State/stdDevTranslation", translationStdDev); + addVisionObservation(cameraPose, camera.robotToCamera, translationStdDev, + Double.POSITIVE_INFINITY, pipelineResult.getTimestampSeconds()); + return true; + } + } + return false; + } + + private static double stdDevMultiplier(List targets, Pose3d cameraPose) { + double totalDistance = 0.0; + int count = 0; + for (var tag : targets) { + var maybeTagPose = Constants.Vision.fieldLayout.getTagPose(tag.getFiducialId()); + if (maybeTagPose.isPresent()) { + var tagPose = maybeTagPose.get(); + totalDistance += tagPose.getTranslation().getDistance(cameraPose.getTranslation()); + count++; + } + } + double avgDistance = totalDistance / count; + double stddev = Math.pow(avgDistance, 2.0) / count; + return stddev; + } + + /** + * Returns the current best estimate of the robot's global field pose. + * + * @return estimated robot pose in field coordinates + */ + public Pose2d getGlobalPoseEstimate() { + return visionAdjustedOdometry.getEstimatedPosition(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/util/TeleopControls.java b/src/main/java/frc/robot/subsystems/swerve/util/TeleopControls.java new file mode 100644 index 0000000..cc42e61 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/util/TeleopControls.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems.swerve.util; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.jspecify.annotations.NullMarked; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import frc.robot.Constants; + +/** + * Control scheme utilities for teleoperated swerve driving. + * + *

+ * This class converts raw driver input signals (typically joystick axes) into {@link ChassisSpeeds} + * suitable for commanding a swerve drivetrain. Input processing includes deadbanding, non-linear + * shaping, and scaling to configured maximum translational and rotational speeds. + * + *

+ * The resulting chassis speeds are intended to be consumed by higher-level drive commands + * (robot-relative or field-relative) rather than applied directly to hardware. + */ +@NullMarked +public class TeleopControls { + + /** + * Creates a supplier that converts driver inputs into desired chassis speeds. + * + *

+ * The returned {@link Supplier} performs the following processing each time it is evaluated: + *

    + *
  • Applies a configurable deadband to all input axes
    • + *
  • Applies non-linear shaping to translational inputs for finer low-speed control
    • + *
  • Scales translational and rotational inputs to configured maximum speeds
    • + *
+ * + *

+ * The produced {@link ChassisSpeeds} are expressed in a pseudo-field-relative frame, + * where: + *

    + *
  • Forward input corresponds to positive X velocity
    • + *
  • Rightward input corresponds to positive Y velocity
    • + *
  • Counterclockwise input corresponds to positive angular velocity
    • + *
+ * + *

+ * This method does not perform true field-relative conversion; callers are expected to apply + * any required frame transformations using the robot's current heading. + * + * @param forward supplier providing the forward/backward driver input + * @param right supplier providing the left/right driver input + * @param turnCCW supplier providing the counterclockwise rotation input + * @return a supplier that generates processed {@link ChassisSpeeds} for teleop control + */ + public static Supplier teleopControls(DoubleSupplier forward, + DoubleSupplier right, DoubleSupplier turnCCW) { + return () -> { + double xaxis = right.getAsDouble(); + double yaxis = forward.getAsDouble(); + double raxis = turnCCW.getAsDouble(); + yaxis = MathUtil.applyDeadband(yaxis, Constants.DriverControls.stickDeadband); + xaxis = MathUtil.applyDeadband(xaxis, Constants.DriverControls.stickDeadband); + xaxis *= xaxis * Math.signum(xaxis); + yaxis *= yaxis * Math.signum(yaxis); + raxis = (Math.abs(raxis) < Constants.DriverControls.stickDeadband) ? 0 : raxis; + return new ChassisSpeeds(yaxis * Constants.DriverControls.driverTranslationalMaxSpeed, + xaxis * Constants.DriverControls.driverTranslationalMaxSpeed, + raxis * Constants.DriverControls.driverRotationalMaxSpeed); + }; + } + +} diff --git a/src/main/java/frc/robot/subsystems/swerve/util/TuningCommands.java b/src/main/java/frc/robot/subsystems/swerve/util/TuningCommands.java new file mode 100644 index 0000000..0dcdbd1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/util/TuningCommands.java @@ -0,0 +1,222 @@ +package frc.robot.subsystems.swerve.util; + +import java.text.DecimalFormat; +import java.text.NumberFormat; +import java.util.LinkedList; +import java.util.List; +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.Swerve; + +/** + * Collection of command factories used for drivetrain tuning and characterization. + * + *

+ * This class provides reusable command sequences for empirically determining drivetrain parameters + * such as feedforward gains and effective wheel radius. These commands are intended to be run + * manually during calibration and development, not during normal match operation. + * + *

+ * This class is non-instantiable and only contains static factory methods. + */ +public final class TuningCommands { + + private TuningCommands() {} + + private static final double ffStartDelay = 2.0; + private static final double ffRampRate = 0.1; + private static final double wheelRadiusMaxVelocity = 0.25; + private static final double wheelRadiusRampRate = 0.05; + + /** + * Creates a command to characterize drivetrain feedforward constants. + * + *

+ * This routine performs a quasi-static ramp test by slowly increasing applied voltage while + * measuring the resulting drivetrain velocity. The collected data is fit using linear + * regression to estimate: + * + *

    + *
  • kS - static friction voltage
    • + *
  • kV - velocity-proportional voltage
    • + *
+ * + *

+ * The characterization process consists of: + *

    + *
  1. Orient all modules to face forward
    2. + *
  2. Holding zero output to allow modules to fully orient
    3. + *
  3. Linearly ramping voltage at a fixed rate
    4. + *
  4. Recording velocity and voltage samples each loop
    5. + *
+ * + *

+ * When the command is cancelled, the collected samples are fit and the resulting constants are + * printed to the console and logged. + * + *

+ * This routine should be run on flat carpet with minimal disturbances. + * + * @param swerve the swerve subsystem being characterized + * @param runCharacterization consumer that applies a raw voltage command to the drivetrain + * @param getFFCharacterizationVelocity supplier that returns the current drivetrain velocity + * for sampling + * @return a command that performs feedforward characterization + */ + public static Command feedforwardCharacterization(Swerve swerve, + DoubleConsumer runCharacterization, DoubleSupplier getFFCharacterizationVelocity) { + List velocitySamples = new LinkedList<>(); + List voltageSamples = new LinkedList<>(); + Timer timer = new Timer(); + + return Commands.sequence( + // Reset data + Commands.runOnce(() -> { + velocitySamples.clear(); + voltageSamples.clear(); + }), + + // Allow modules to orient + Commands.run(() -> runCharacterization.accept(0.0), swerve).withTimeout(ffStartDelay), + + // Start timer + Commands.runOnce(timer::restart), + + // Accelerate and gather data + Commands.run(() -> { + double voltage = timer.get() * ffRampRate; + runCharacterization.accept(voltage); + velocitySamples.add(getFFCharacterizationVelocity.getAsDouble()); + voltageSamples.add(voltage); + }, swerve) + + // When cancelled, calculate and print results + .finallyDo(() -> { + int n = velocitySamples.size(); + double sumX = 0.0; + double sumY = 0.0; + double sumXY = 0.0; + double sumX2 = 0.0; + for (int i = 0; i < n; i++) { + sumX += velocitySamples.get(i); + sumY += voltageSamples.get(i); + sumXY += velocitySamples.get(i) * voltageSamples.get(i); + sumX2 += velocitySamples.get(i) * velocitySamples.get(i); + } + double kS = (sumY * sumX2 - sumX * sumXY) / (n * sumX2 - sumX * sumX); + double kV = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX); + + NumberFormat formatter = new DecimalFormat("#0.00000"); + System.out.println("********** Drive FF Characterization Results **********"); + System.out.println("\tkS: " + formatter.format(kS)); + System.out.println("\tkV: " + formatter.format(kV)); + Logger.recordOutput("Sysid/FF/kS", kS); + Logger.recordOutput("Sysid/FF/kV", kV); + })); + } + + /** + * Creates a command to characterize the effective wheel radius of the drivetrain. + * + *

+ * This routine estimates wheel radius by rotating the robot in place and comparing the + * integrated gyro rotation against the average wheel travel. The wheel radius is computed using + * the relationship: + * + * 

+     * wheelRadius = (robotRotation * driveBaseRadius) / wheelTravel
+     *
+ * + *

+ * The command runs two sequences in parallel: + *

    + *
  • A drive sequence that smoothly accelerates rotational speed
    • + *
  • A measurement sequence that integrates gyro rotation and wheel deltas
    • + *
+ * + *

+ * Intermediate values and the computed wheel radius are logged continuously for validation and + * offline analysis. + * + *

+ * This routine assumes: + *

    + *
  • Accurate gyro measurements
    • + *
  • Consistent wheel traction during rotation
    • + *
  • Correct drivetrain geometry constants
    • + *
+ * + * @param swerve the swerve subsystem being characterized + * @param setModuleStates consumer used to command rotational chassis speeds + * @param getWheelRadiusCharacterizationPositions supplier of wheel position measurements + * @param gyroYaw supplier of the current robot yaw angle + * @return a command that performs wheel radius characterization + */ + public static Command wheelRadiusCharacterization(Swerve swerve, + Consumer setModuleStates, + Supplier getWheelRadiusCharacterizationPositions, Supplier gyroYaw) { + SlewRateLimiter limiter = new SlewRateLimiter(wheelRadiusRampRate); + WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); + + final double driveBaseRadius = + Math.hypot(Constants.Swerve.trackWidth / 2.0, Constants.Swerve.wheelBase / 2.0); + + return Commands.parallel( + // Drive control sequence + Commands.sequence( + // Reset acceleration limiter + Commands.runOnce(() -> limiter.reset(0.0)), + + // Turn in place, accelerating up to full speed + Commands.run(() -> { + double speed = limiter.calculate(wheelRadiusMaxVelocity); + setModuleStates.accept(new ChassisSpeeds(0.0, 0.0, speed)); + }, swerve)), + + // Measurement sequence + Commands.sequence( + // Wait for modules to fully orient before starting measurement + Commands.waitSeconds(1.0), + + // Record starting measurement + Commands.runOnce(() -> { + state.positions = getWheelRadiusCharacterizationPositions.get(); + state.lastAngle = gyroYaw.get(); + state.gyroDelta = 0.0; + }), + + // Update gyro delta + Commands.run(() -> { + var rotation = gyroYaw.get(); + state.gyroDelta += Math.abs(rotation.minus(state.lastAngle).getRadians()); + state.lastAngle = rotation; + + double[] positions = getWheelRadiusCharacterizationPositions.get(); + double wheelDelta = 0.0; + for (int i = 0; i < 4; i++) { + wheelDelta += Math.abs(positions[i] - state.positions[i]) / 4.0; + } + double wheelRadius = (state.gyroDelta * driveBaseRadius) / wheelDelta; + + Logger.recordOutput("Drive/WheelDelta", wheelDelta); + Logger.recordOutput("Drive/WheelRadius", Units.metersToInches(wheelRadius)); + }))); + } + + private static class WheelRadiusCharacterizationState { + double[] positions = new double[4]; + Rotation2d lastAngle = Rotation2d.kZero; + double gyroDelta = 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/vision/CameraConstants.java b/src/main/java/frc/robot/subsystems/vision/CameraConstants.java new file mode 100644 index 0000000..d309c3f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/CameraConstants.java @@ -0,0 +1,64 @@ +package frc.robot.subsystems.vision; + +import org.jspecify.annotations.NullMarked; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.measure.Frequency; +import edu.wpi.first.units.measure.Time; +import frc.robot.util.typestate.AltMethod; +import frc.robot.util.typestate.OptionalField; +import frc.robot.util.typestate.RequiredField; +import frc.robot.util.typestate.TypeStateBuilder; + +/** Constants for a camera */ +@NullMarked +public class CameraConstants { + + public final String name; + public final int height; + public final int width; + public final Rotation2d horizontalFieldOfView; + public final Frequency simFps; + public final Time simLatency; + public final Time simLatencyStdDev; + public final double calibrationErrorMean; + public final double calibrationErrorStdDev; + public final Transform3d robotToCamera; + public final double translationError; + public final double rotationError; + public final double singleTagError; + + /** Constants for a camera */ + @TypeStateBuilder("CameraConstantsBuilder") + public CameraConstants(@RequiredField String name, @RequiredField int height, + @RequiredField int width, + @RequiredField(alt = @AltMethod(type = double.class, parameter_name = "degrees", + value = "edu.wpi.first.math.geometry.Rotation2d.fromDegrees(degrees)")) Rotation2d horizontalFieldOfView, + @RequiredField(alt = @AltMethod(type = double.class, parameter_name = "hz", + value = "edu.wpi.first.units.Units.Hertz.of(hz)")) Frequency simFps, + @RequiredField(alt = @AltMethod(type = double.class, parameter_name = "seconds", + value = "edu.wpi.first.units.Units.Seconds.of(seconds)")) Time simLatency, + @RequiredField(alt = @AltMethod(type = double.class, parameter_name = "seconds", + value = "edu.wpi.first.units.Units.Seconds.of(seconds)")) Time simLatencyStdDev, + @RequiredField double calibrationErrorMean, @RequiredField double calibrationErrorStdDev, + @RequiredField Transform3d robotToCamera, @OptionalField("0.3") double translationError, + @OptionalField("edu.wpi.first.math.util.Units.degreesToRadians(3)") double rotationError, + @OptionalField("0.2") double singleTagError) { + this.name = name; + this.height = height; + this.width = width; + this.horizontalFieldOfView = horizontalFieldOfView; + this.simFps = simFps; + this.simLatency = simLatency; + this.simLatencyStdDev = simLatencyStdDev; + this.calibrationErrorMean = calibrationErrorMean; + this.calibrationErrorStdDev = calibrationErrorStdDev; + this.robotToCamera = robotToCamera; + this.translationError = translationError; + this.rotationError = rotationError; + this.singleTagError = singleTagError; + } + + + +} diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..b7fbd1c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -0,0 +1,128 @@ +package frc.robot.subsystems.vision; + +import java.util.ArrayList; +import java.util.List; +import java.util.stream.IntStream; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.Logger; +import org.photonvision.targeting.PhotonPipelineResult; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.util.SwerveState; +import frc.robot.util.Tuples.Tuple2; + +/** + * Vision subsystem responsible for processing camera-based pose observations and contributing + * vision measurements to the drivetrain pose estimator. + * + *

+ * This subsystem acts as a bridge between one or more vision pipelines (e.g. PhotonVision) and the + * {@link SwerveState} pose estimator. + * + *

Timestamp handling

Vision measurements are frequently delayed relative to the control + * loop. This subsystem ensures measurements are applied in chronological order so that the pose + * estimator can correctly replay history using its internal pose buffer. + * + *

Multiple cameras

The subsystem supports an arbitrary number of cameras, each with its + * own calibration and transform defined in {@link Constants.Vision#cameraConstants}. All cameras + * contribute measurements into a single shared pose estimate. + * + *

Visualization

For debugging and analysis, the subsystem publishes 3D visualization data + * showing the relationship between the robot, cameras, and observed AprilTags. This data is + * intended for tools such as AdvantageScope and is not used for control. + */ +@NullMarked +public class Vision extends SubsystemBase { + + private final VisionIO io; + private final VisionIO.CameraInputs[] cameraInputs; + private final String[] cameraInputKeys; + private final SwerveState state; + private final Translation3d[][] cameraViz; + private final String[] cameraVizKeys; + private final boolean[] cameraContributed; + private final String[] cameraContributedKeys; + + /** + * Creates the vision subsystem. + * + * @param state shared swerve pose estimator to receive vision updates + * @param io vision IO implementation responsible for acquiring camera results + */ + public Vision(SwerveState state, VisionIO io) { + super("Vision"); + this.io = io; + this.state = state; + this.cameraInputs = IntStream.range(0, Constants.Vision.cameraConstants.length) + .mapToObj((_x) -> new VisionIO.CameraInputs()).toArray(VisionIO.CameraInputs[]::new); + this.cameraInputKeys = IntStream.range(0, Constants.Vision.cameraConstants.length) + .mapToObj((i) -> "Vision/Camera_" + i).toArray(String[]::new); + this.cameraViz = IntStream.range(0, Constants.Vision.cameraConstants.length) + .mapToObj((_x) -> new Translation3d[0]).toArray(Translation3d[][]::new); + this.cameraVizKeys = IntStream.range(0, Constants.Vision.cameraConstants.length) + .mapToObj((i) -> "Vision/AprilTagViz_" + i).toArray(String[]::new); + this.cameraContributed = new boolean[Constants.Vision.cameraConstants.length]; + this.cameraContributedKeys = IntStream.range(0, Constants.Vision.cameraConstants.length) + .mapToObj((i) -> "Vision/Contributed_" + i).toArray(String[]::new); + } + + @Override + public void periodic() { + io.updateInputs(cameraInputs); + for (int i = 0; i < cameraInputs.length; i++) { + Logger.processInputs(cameraInputKeys[i], cameraInputs[i]); + } + + // Get 2-tuples of (camera idx, result) + List> results = new ArrayList<>(); + for (int i = 0; i < cameraInputs.length; i++) { + + for (int j = 0; j < cameraInputs[i].results.length; j++) { + var result = cameraInputs[i].results[j]; + results.add(new Tuple2<>(i, result)); + } + } + + // Sort by timestamp (earlier pipeline results come first) + results.sort( + (a, b) -> Double.compare(a._1().getTimestampSeconds(), b._1().getTimestampSeconds())); + + for (int i = 0; i < Constants.Vision.cameraConstants.length; i++) { + cameraViz[i] = new Translation3d[0]; + } + + for (var result : results) { + cameraContributed[result._0()] = state + .addVisionObservation(Constants.Vision.cameraConstants[result._0()], result._1()); + for (int i = 0; i < result._1().targets.size(); i++) { + var cameraPose = new Pose3d(state.getGlobalPoseEstimate()) + .plus(Constants.Vision.cameraConstants[result._0()].robotToCamera); + var target = result._1().targets.get(i); + Constants.Vision.fieldLayout.getTagPose(target.fiducialId).ifPresent(pose -> { + cameraViz[result._0()] = addTwo(cameraViz[result._0()], + cameraPose.getTranslation(), pose.getTranslation()); + }); + } + } + + for (int i = 0; i < Constants.Vision.cameraConstants.length; i++) { + Logger.recordOutput(cameraContributedKeys[i], cameraContributed[i]); + if (cameraViz[i].length == 0) { + continue; + } + Logger.recordOutput(cameraVizKeys[i], cameraViz[i]); + } + } + + private Translation3d[] addTwo(Translation3d[] translations, Translation3d newTranslation1, + Translation3d newTranslation2) { + Translation3d[] res = new Translation3d[translations.length + 2]; + System.arraycopy(translations, 0, res, 0, translations.length); + res[translations.length] = newTranslation1; + res[translations.length + 1] = newTranslation2; + return res; + } + +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java new file mode 100644 index 0000000..6699df1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -0,0 +1,92 @@ +package frc.robot.subsystems.vision; + +import java.util.Optional; +import org.jspecify.annotations.NullMarked; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.struct.PhotonPipelineResultSerde; +import org.photonvision.targeting.PhotonPipelineResult; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.numbers.N8; +import frc.robot.util.GenerateEmptyIO; + +/** IO Class for Vision */ +@GenerateEmptyIO +@NullMarked +public interface VisionIO { + + /** Inputs for an individual camera */ + public static class CameraInputs implements LoggableInputs, Cloneable { + public PhotonPipelineResult[] results = new PhotonPipelineResult[0]; + public Optional> cameraMatrix = Optional.empty(); + public Optional> distCoeffs = Optional.empty(); + + private static final PhotonPipelineResultSerde serde = new PhotonPipelineResultSerde(); + + @Override + public void toLog(LogTable table) { + table.put("length", results.length); + for (int i = 0; i < results.length; i++) { + LogTable subtable = table.getSubtable("[" + i + "]"); + var result = results[i]; + if (result == null) { + var rawBytes = new byte[0]; + subtable.put("rawBytes", rawBytes); + } else { + // https://github.com/PhotonVision/photonvision/blob/78b82e3a96d04f79e0b70746dd975c10c8ff1294/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java#L166 + var rawBytes = new Packet(1024); + serde.pack(rawBytes, result); + subtable.put("rawBytes", rawBytes.getWrittenDataCopy()); + } + } + if (cameraMatrix.isPresent()) { + table.put("cameraMatrix", cameraMatrix.get().getData()); + } else { + table.put("cameraMatrix", new double[0]); + } + if (distCoeffs.isPresent()) { + table.put("distCoeffs", distCoeffs.get().getData()); + } else { + table.put("distCoeffs", new double[0]); + } + } + + @Override + public void fromLog(LogTable table) { + int length = table.get("length", 0); + results = new PhotonPipelineResult[length]; + for (int i = 0; i < length; i++) { + LogTable subtable = table.getSubtable("[" + i + "]"); + var rawBytes = subtable.get("rawBytes", new byte[0]); + PhotonPipelineResult result; + if (rawBytes.length > 0) { + Packet p = new Packet(rawBytes); + result = serde.unpack(p); + } else { + result = null; + } + results[i] = result; + } + + double[] data = table.get("cameraMatrix", new double[0]); + if (data.length == 0) { + this.cameraMatrix = Optional.empty(); + } else { + this.cameraMatrix = Optional.of(new Matrix<>(N3.instance, N3.instance, data)); + } + data = table.get("distCoeffs", new double[0]); + if (data.length == 0) { + this.distCoeffs = Optional.empty(); + } else { + this.distCoeffs = Optional.of(new Matrix<>(N8.instance, N1.instance, data)); + } + } + + } + + public void updateInputs(CameraInputs[] inputs); + +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionReal.java b/src/main/java/frc/robot/subsystems/vision/VisionReal.java new file mode 100644 index 0000000..f2c56dc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionReal.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.vision; + +import java.util.Arrays; +import org.jspecify.annotations.NullMarked; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; +import frc.robot.Constants; + +/** PhotonVision-attached implementation */ +@NullMarked +public class VisionReal implements VisionIO { + + protected final PhotonCamera[] cameras; + + /** PhotonVision-attached implementation */ + public VisionReal() { + cameras = Arrays.stream(Constants.Vision.cameraConstants) + .map((consts) -> new PhotonCamera(consts.name)).toArray(PhotonCamera[]::new); + } + + @Override + public void updateInputs(CameraInputs[] inputs) { + for (int i = 0; i < cameras.length; i++) { + inputs[i].results = + cameras[i].getAllUnreadResults().toArray(PhotonPipelineResult[]::new); + inputs[i].cameraMatrix = cameras[i].getCameraMatrix(); + inputs[i].distCoeffs = cameras[i].getDistCoeffs(); + } + } + +} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionSim.java new file mode 100644 index 0000000..d2c77fe --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSim.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.vision; + +import static edu.wpi.first.units.Units.Hertz; +import static edu.wpi.first.units.Units.Milliseconds; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.jspecify.annotations.NullMarked; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.SwerveSim; + +/** Simulation of vision using built-in PhotonVision simulator. */ +@NullMarked +public class VisionSim extends VisionReal { + + private final SwerveDriveSimulation sim; + private final VisionSystemSim visionSim; + + /** Simulation of vision using built-in PhotonVision simulator. */ + public VisionSim(SwerveSim sim) { + this.sim = sim.mapleSim; + this.visionSim = new VisionSystemSim("main"); + + visionSim.addAprilTags(Constants.Vision.fieldLayout); + + var constants = Constants.Vision.cameraConstants; + for (int i = 0; i < constants.length; i++) { + SimCameraProperties props = new SimCameraProperties(); + props.setCalibration(constants[i].width, constants[i].height, + constants[i].horizontalFieldOfView); + props.setCalibError(constants[i].calibrationErrorMean, + constants[i].calibrationErrorStdDev); + props.setFPS(constants[i].simFps.in(Hertz)); + props.setAvgLatencyMs(constants[i].simLatency.in(Milliseconds)); + props.setLatencyStdDevMs(constants[i].simLatencyStdDev.in(Milliseconds)); + PhotonCameraSim cameraSim = new PhotonCameraSim(this.cameras[i], props); + visionSim.addCamera(cameraSim, constants[i].robotToCamera); + } + } + + @Override + public void updateInputs(CameraInputs[] inputs) { + visionSim.update(sim.getSimulatedDriveTrainPose()); + super.updateInputs(inputs); + } + +} diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..7e3af81 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,48 @@ +package frc.robot.util; + +import org.jspecify.annotations.NullMarked; +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.util.Units; +import edu.wpi.first.wpilibj.DriverStation; + +/** Utilities for flipping based on alliance */ +@NullMarked +public class AllianceFlipUtil { + public static double fieldWidth = Units.feetToMeters(26.0) + Units.inchesToMeters(5.0); + public static double fieldLength = Units.feetToMeters(57.0) + Units.inchesToMeters(6.875); + + /** Possibly flip */ + public static double applyX(double x) { + return shouldFlip() ? fieldLength - x : x; + } + + /** Possibly flip */ + public static double applyY(double y) { + return shouldFlip() ? fieldWidth - y : y; + } + + /** Possibly flip */ + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + /** Possibly flip */ + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + /** Possibly flip */ + public static Pose2d apply(Pose2d pose) { + return shouldFlip() ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + /** Possibly flip */ + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } + +} diff --git a/src/main/java/frc/robot/util/DeviceDebug.java b/src/main/java/frc/robot/util/DeviceDebug.java new file mode 100644 index 0000000..3434be9 --- /dev/null +++ b/src/main/java/frc/robot/util/DeviceDebug.java @@ -0,0 +1,145 @@ +package frc.robot.util; + +import java.util.EnumSet; +import java.util.HashMap; +import java.util.Map; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.networktables.NetworkTableEvent; +import edu.wpi.first.networktables.NetworkTableInstance; + +/** + * Runtime debugging utility for exposing low-level device controls via NetworkTables. + * + *

+ * This class allows selected CAN devices to be interactively inspected and reconfigured at runtime + * through NetworkTables (e.g. from Glass, Shuffleboard, or AdvantageScope). It is intended strictly + * for debugging and tuning and should not be relied upon for normal robot operation. + * + *

Design overview

+ *
    + *
  • Devices are explicitly registered by name before debugging is enabled.
    • + *
  • No NetworkTables listeners or control hooks are installed until debugging is explicitly + * enabled via {@code /Debug/Enable}.
    • + *
  • Once enabled, this class publishes per-device debug topics and installs listeners to apply + * configuration changes live.
    • + *
+ * + *

Safety model

+ * + *

+ * Debug functionality is gated behind an explicit NetworkTables boolean ({@code /Debug/Enable}) to + * prevent accidental configuration changes during normal operation. Once enabled, the enable + * listener is removed and debugging remains active for the lifetime of the program. + * + *

+ * This class is not thread-safe and is expected to be initialized and used during robot + * startup before devices are actively commanded. + * + *

+ * Support for additional device types and parameters (e.g. CANcoders, current limits, inversion, + * soft limits) may be added in the future. + */ +public final class DeviceDebug { + + private DeviceDebug() {} + + private static final Map talonFXs = new HashMap<>(); + private static final Map cancoders = new HashMap<>(); + + private static int enableListener; + + /** + * Initializes the debug system and installs the global enable listener. + * + *

+ * This method publishes {@code /Debug/Enable} (default {@code false}) and waits for it to be + * set {@code true}. When enabled, all registered devices will have their debug topics created + * and listeners installed. + * + *

+ * This method should be called exactly once during robot initialization. + */ + public static void initialize() { + NetworkTableInstance instance = NetworkTableInstance.getDefault(); + var enableTopic = instance.getBooleanTopic("/Debug/Enable"); + enableTopic.publish().accept(false); + enableListener = instance.addListener(enableTopic.subscribe(false), + EnumSet.of(NetworkTableEvent.Kind.kValueAll), (ev) -> { + if (ev.valueData.value.getBoolean()) { + setupDebug(instance); + } + }); + } + + private static void setupDebug(NetworkTableInstance instance) { + instance.removeListener(enableListener); + for (var kv : talonFXs.entrySet()) { + String key = "/Debug/TalonFX/" + kv.getKey(); + + var brakeModeTopic = instance.getBooleanTopic(key + "/Brake"); + TalonFX talon = kv.getValue(); + boolean isBrake = getBrakeMode(talon); + var publisher = brakeModeTopic.publish(); + publisher.accept(isBrake); + instance.addListener(brakeModeTopic.subscribe(isBrake), + EnumSet.of(NetworkTableEvent.Kind.kValueRemote), (ev) -> { + setBrakeMode(talon, ev.valueData.value.getBoolean()); + boolean isBrake_ = getBrakeMode(talon); + publisher.accept(isBrake_); + }); + } + } + + /** + * Registers a TalonFX for runtime debugging. + * + *

+ * The provided name is used as part of the NetworkTables path: + * {@code /Debug/TalonFX/<name>}. + * + *

+ * Registration must occur before debugging is enabled. + * + * @param name human-readable identifier for the device + * @param talon TalonFX instance to expose for debugging + */ + public static void register(String name, TalonFX talon) { + talonFXs.put(name, talon); + } + + /** + * Registers a CANcoder for future runtime debugging. + * + *

+ * This method currently records the device but does not yet expose any debug controls. It + * exists to allow future expansion without changing registration semantics. + * + *

+ * Registration must occur before debugging is enabled. + * + * @param name human-readable identifier for the device + * @param cancoder CANcoder instance to register + */ + public static void register(String name, CANcoder cancoder) { + cancoders.put(name, cancoder); + } + + private static boolean getBrakeMode(TalonFX talon) { + MotorOutputConfigs config = new MotorOutputConfigs(); + var configurator = talon.getConfigurator(); + configurator.refresh(config); + return config.NeutralMode.value == NeutralModeValue.Brake.value; + } + + private static void setBrakeMode(TalonFX talon, boolean enableBrake) { + MotorOutputConfigs config = new MotorOutputConfigs(); + var configurator = talon.getConfigurator(); + configurator.refresh(config); + config.NeutralMode = enableBrake ? NeutralModeValue.Brake : NeutralModeValue.Coast; + PhoenixSignals.tryUntilOk(5, () -> configurator.apply(config)); + } + +} diff --git a/src/main/java/frc/robot/util/GenerateEmptyIO.java b/src/main/java/frc/robot/util/GenerateEmptyIO.java new file mode 100644 index 0000000..38367c3 --- /dev/null +++ b/src/main/java/frc/robot/util/GenerateEmptyIO.java @@ -0,0 +1,16 @@ +package frc.robot.util; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** Generate Empty IO implementation using RobotUtils */ +@Retention(RetentionPolicy.CLASS) +@Target(ElementType.TYPE) +public @interface GenerateEmptyIO { + + /** Parameter types for constructor */ + public Class[] value() default {}; + +} diff --git a/src/main/java/frc/robot/util/PhoenixSignals.java b/src/main/java/frc/robot/util/PhoenixSignals.java new file mode 100644 index 0000000..a77793a --- /dev/null +++ b/src/main/java/frc/robot/util/PhoenixSignals.java @@ -0,0 +1,109 @@ +package frc.robot.util; + +import java.util.function.Supplier; +import org.jspecify.annotations.NullMarked; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; + +/** + * Utility class that centralizes registration and synchronized refreshing of CTRE Phoenix + * {@link BaseStatusSignal}s. + * + *

+ * Grouping signals and refreshing them together ensures that all sampled values share the same + * synchronization fence, minimizing timestamp skew between related measurements. + * + *

+ * Signals are maintained in separate groups depending on whether they are sourced from a CANivore + * bus or the roboRIO CAN bus, as Phoenix requires refresh calls to be performed per bus. + * + *

+ * This class is purely static and is not intended to be instantiated. + */ +@NullMarked +public class PhoenixSignals { + + private PhoenixSignals() {} + + /** + * Repeatedly executes a Phoenix command until it succeeds or the maximum number of attempts is + * reached. + * + *

+ * This is useful for Phoenix configuration calls that may transiently fail during startup or + * bus contention. + * + * @param maxAttempts maximum number of attempts before giving up + * @param command supplier that executes the Phoenix command and returns a {@link StatusCode} + */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error.isOK()) { + break; + } + System.err.println("Phoenix Error: " + error.toString()); + } + } + + /** Signals registered on the CANivore bus for synchronized refresh. */ + private static BaseStatusSignal[] canivoreSignals = new BaseStatusSignal[0]; + /** Signals registered on the roboRIO CAN bus for synchronized refresh. */ + private static BaseStatusSignal[] rioSignals = new BaseStatusSignal[0]; + + /** + * Registers one or more Phoenix signals for synchronized refreshing. + * + *

+ * All registered signals will be refreshed together when {@link #refreshAll()} is called, + * ensuring consistent timestamps across related measurements. + * + *

+ * Signals must be registered to the correct bus (CANivore vs roboRIO). Mixing buses in a single + * refresh call is not supported by Phoenix. + * + *

+ * This method should typically be called during subsystem construction or robot initialization, + * before periodic refreshes occur. + * + * @param canivore {@code true} if the signals are on the CANivore bus, {@code false} if they + * are on the roboRIO CAN bus + * @param signals one or more {@link BaseStatusSignal}s to register + */ + public static void registerSignals(boolean canivore, BaseStatusSignal... signals) { + if (canivore) { + BaseStatusSignal[] newSignals = + new BaseStatusSignal[canivoreSignals.length + signals.length]; + System.arraycopy(canivoreSignals, 0, newSignals, 0, canivoreSignals.length); + System.arraycopy(signals, 0, newSignals, canivoreSignals.length, signals.length); + canivoreSignals = newSignals; + } else { + BaseStatusSignal[] newSignals = + new BaseStatusSignal[rioSignals.length + signals.length]; + System.arraycopy(rioSignals, 0, newSignals, 0, rioSignals.length); + System.arraycopy(signals, 0, newSignals, rioSignals.length, signals.length); + rioSignals = newSignals; + } + } + + /** + * Refreshes all registered Phoenix signals. + * + *

+ * This method should be called once per control loop to update all registered signals in a + * synchronized manner. + * + *

+ * Signals on the CANivore and roboRIO buses are refreshed separately, but each group is + * internally synchronized. + */ + public static void refreshAll() { + if (canivoreSignals.length > 0) { + BaseStatusSignal.refreshAll(canivoreSignals); + } + if (rioSignals.length > 0) { + BaseStatusSignal.refreshAll(rioSignals); + } + } + +} diff --git a/src/main/java/frc/robot/util/SparkSignals.java b/src/main/java/frc/robot/util/SparkSignals.java new file mode 100644 index 0000000..9ef371f --- /dev/null +++ b/src/main/java/frc/robot/util/SparkSignals.java @@ -0,0 +1,158 @@ +package frc.robot.util; + +import java.util.function.Consumer; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; +import java.util.function.Function; +import java.util.function.Supplier; +import org.jspecify.annotations.NullMarked; +import com.revrobotics.REVLibError; +import com.revrobotics.spark.SparkBase; + +/** + * Utility methods for safely interacting with REV Spark motor controllers in the presence of + * transient or persistent communication errors. + * + *

+ * REV API calls report errors via {@link REVLibError} rather than exceptions. These helpers + * centralize common error-handling patterns and ensure that invalid or stale values are not + * inadvertently consumed by higher-level logic. + * + *

+ * All utility methods update a shared {@link #sparkStickyFault} flag whenever a non-{@code kOk} + * error is detected. This flag may be monitored elsewhere in the robot code to trigger diagnostics, + * alerts, or safe fallback behavior. + * + *

+ * This class is purely static and is not intended to be instantiated. + */ +@NullMarked +public class SparkSignals { + + private SparkSignals() {} + + /** + * Sticky indicator that is set whenever a Spark-related error is detected. + * + *

+ * Once set, this flag remains {@code true} until explicitly cleared by user code. It can be + * used to surface non-fatal communication issues that may otherwise go unnoticed during + * operation. + */ + public static boolean sparkStickyFault = false; + + /** + * Retrieves and processes a value from a Spark controller only if the underlying call completed + * successfully. + * + *

+ * If the Spark reports an error, the value is discarded and the consumer is not invoked. + * + * @param spark the Spark device associated with the value + * @param supplier supplier that retrieves the value from the Spark + * @param consumer consumer that processes the value if it is valid + */ + public static void ifOk(SparkBase spark, DoubleSupplier supplier, DoubleConsumer consumer) { + double value = supplier.getAsDouble(); + if (spark.getLastError() == REVLibError.kOk) { + consumer.accept(value); + } else { + sparkStickyFault = true; + } + } + + /** + * Retrieves multiple values from a Spark controller and processes them only if all values are + * valid. + * + *

+ * If any supplier produces an error, processing is aborted and the consumer is not invoked. + * + * @param spark the Spark device associated with the values + * @param suppliers array of suppliers retrieving Spark values + * @param consumer consumer that processes the values if all are valid + */ + public static void ifOk(SparkBase spark, DoubleSupplier[] suppliers, + Consumer consumer) { + double[] values = new double[suppliers.length]; + for (int i = 0; i < suppliers.length; i++) { + values[i] = suppliers[i].getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) { + sparkStickyFault = true; + return; + } + } + consumer.accept(values); + } + + /** + * Retrieves a value from a Spark controller, returning a default if the value is invalid. + * + *

+ * If an error occurs, the default value is returned and the sticky fault flag is set. + * + * @param spark the Spark device associated with the value + * @param supplier supplier that retrieves the value + * @param defaultValue value to return if an error is detected + * @return the retrieved value if valid, otherwise {@code defaultValue} + */ + public static double ifOkOrDefault(SparkBase spark, DoubleSupplier supplier, + double defaultValue) { + double value = supplier.getAsDouble(); + if (spark.getLastError() == REVLibError.kOk) { + return value; + } else { + sparkStickyFault = true; + return defaultValue; + } + } + + /** + * Retrieves multiple values from a Spark controller and applies a transformation only if all + * values are valid. + * + *

+ * If any value is invalid, the transformation is skipped and a default value is returned. + * + * @param spark the Spark device associated with the values + * @param suppliers array of suppliers retrieving Spark values + * @param transformer function that computes a result from the retrieved values + * @param defaultValue value to return if any supplier reports an error + * @return transformed value if all inputs are valid, otherwise {@code defaultValue} + */ + public static double ifOkOrDefault(SparkBase spark, DoubleSupplier[] suppliers, + Function transformer, double defaultValue) { + double[] values = new double[suppliers.length]; + for (int i = 0; i < suppliers.length; i++) { + values[i] = suppliers[i].getAsDouble(); + if (spark.getLastError() != REVLibError.kOk) { + sparkStickyFault = true; + return defaultValue; + } + } + return transformer.apply(values); + } + + /** + * Repeatedly executes a Spark command until it succeeds or the maximum number of attempts is + * reached. + * + *

+ * This is commonly used for configuration calls that may fail during initialization due to + * transient CAN issues. + * + * @param maxAttempts maximum number of attempts before giving up + * @param command supplier that executes the Spark command and returns a {@link REVLibError} + */ + public static void tryUntilOk(int maxAttempts, Supplier command) { + for (int i = 0; i < maxAttempts; i++) { + var error = command.get(); + if (error == REVLibError.kOk) { + break; + } else { + sparkStickyFault = true; + } + } + } + +} diff --git a/src/main/java/frc/robot/util/Tuples.java b/src/main/java/frc/robot/util/Tuples.java new file mode 100644 index 0000000..4cc53e6 --- /dev/null +++ b/src/main/java/frc/robot/util/Tuples.java @@ -0,0 +1,565 @@ +package frc.robot.util; + +/** + * Defines various tuple types for Java. + */ +public final class Tuples { + private Tuples() {} + + + /** + * A type that has a 1st element. + */ + public static interface IValue1 { + + /** + * Get the 1st element. + */ + T _0(); + } + + + /** + * A tuple of 1 elements. + */ + public static final class Tuple1 implements IValue1 { + private final T1 v0; + + /** + * Create a new 1-tuple. + */ + public Tuple1(T1 v0) { + this.v0 = v0; + } + + @Override + public T1 _0() { + return this.v0; + } + + } + + + /** + * A type that has a 2nd element. + */ + public static interface IValue2 { + + /** + * Get the 2nd element. + */ + T _1(); + } + + + /** + * A tuple of 2 elements. + */ + public static final class Tuple2 implements IValue1, IValue2 { + private final T1 v0; + private final T2 v1; + + /** + * Create a new 2-tuple. + */ + public Tuple2(T1 v0, T2 v1) { + this.v0 = v0; + this.v1 = v1; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + } + + + /** + * A type that has a 3rd element. + */ + public static interface IValue3 { + + /** + * Get the 3rd element. + */ + T _2(); + } + + + /** + * A tuple of 3 elements. + */ + public static final class Tuple3 implements IValue1, IValue2, IValue3 { + private final T1 v0; + private final T2 v1; + private final T3 v2; + + /** + * Create a new 3-tuple. + */ + public Tuple3(T1 v0, T2 v1, T3 v2) { + this.v0 = v0; + this.v1 = v1; + this.v2 = v2; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + @Override + public T3 _2() { + return this.v2; + } + + } + + + /** + * A type that has a 4th element. + */ + public static interface IValue4 { + + /** + * Get the 4th element. + */ + T _3(); + } + + + /** + * A tuple of 4 elements. + */ + public static final class Tuple4 + implements IValue1, IValue2, IValue3, IValue4 { + private final T1 v0; + private final T2 v1; + private final T3 v2; + private final T4 v3; + + /** + * Create a new 4-tuple. + */ + public Tuple4(T1 v0, T2 v1, T3 v2, T4 v3) { + this.v0 = v0; + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + @Override + public T3 _2() { + return this.v2; + } + + @Override + public T4 _3() { + return this.v3; + } + + } + + + /** + * A type that has a 5th element. + */ + public static interface IValue5 { + + /** + * Get the 5th element. + */ + T _4(); + } + + + /** + * A tuple of 5 elements. + */ + public static final class Tuple5 + implements IValue1, IValue2, IValue3, IValue4, IValue5 { + private final T1 v0; + private final T2 v1; + private final T3 v2; + private final T4 v3; + private final T5 v4; + + /** + * Create a new 5-tuple. + */ + public Tuple5(T1 v0, T2 v1, T3 v2, T4 v3, T5 v4) { + this.v0 = v0; + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + this.v4 = v4; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + @Override + public T3 _2() { + return this.v2; + } + + @Override + public T4 _3() { + return this.v3; + } + + @Override + public T5 _4() { + return this.v4; + } + + } + + + /** + * A type that has a 6th element. + */ + public static interface IValue6 { + + /** + * Get the 6th element. + */ + T _5(); + } + + + /** + * A tuple of 6 elements. + */ + public static final class Tuple6 + implements IValue1, IValue2, IValue3, IValue4, IValue5, IValue6 { + private final T1 v0; + private final T2 v1; + private final T3 v2; + private final T4 v3; + private final T5 v4; + private final T6 v5; + + /** + * Create a new 6-tuple. + */ + public Tuple6(T1 v0, T2 v1, T3 v2, T4 v3, T5 v4, T6 v5) { + this.v0 = v0; + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + this.v4 = v4; + this.v5 = v5; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + @Override + public T3 _2() { + return this.v2; + } + + @Override + public T4 _3() { + return this.v3; + } + + @Override + public T5 _4() { + return this.v4; + } + + @Override + public T6 _5() { + return this.v5; + } + + } + + + /** + * A type that has a 7th element. + */ + public static interface IValue7 { + + /** + * Get the 7th element. + */ + T _6(); + } + + + /** + * A tuple of 7 elements. + */ + public static final class Tuple7 implements IValue1, + IValue2, IValue3, IValue4, IValue5, IValue6, IValue7 { + private final T1 v0; + private final T2 v1; + private final T3 v2; + private final T4 v3; + private final T5 v4; + private final T6 v5; + private final T7 v6; + + /** + * Create a new 7-tuple. + */ + public Tuple7(T1 v0, T2 v1, T3 v2, T4 v3, T5 v4, T6 v5, T7 v6) { + this.v0 = v0; + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + this.v4 = v4; + this.v5 = v5; + this.v6 = v6; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + @Override + public T3 _2() { + return this.v2; + } + + @Override + public T4 _3() { + return this.v3; + } + + @Override + public T5 _4() { + return this.v4; + } + + @Override + public T6 _5() { + return this.v5; + } + + @Override + public T7 _6() { + return this.v6; + } + + } + + + /** + * A type that has a 8th element. + */ + public static interface IValue8 { + + /** + * Get the 8th element. + */ + T _7(); + } + + + /** + * A tuple of 8 elements. + */ + public static final class Tuple8 implements IValue1, + IValue2, IValue3, IValue4, IValue5, IValue6, IValue7, IValue8 { + private final T1 v0; + private final T2 v1; + private final T3 v2; + private final T4 v3; + private final T5 v4; + private final T6 v5; + private final T7 v6; + private final T8 v7; + + /** + * Create a new 8-tuple. + */ + public Tuple8(T1 v0, T2 v1, T3 v2, T4 v3, T5 v4, T6 v5, T7 v6, T8 v7) { + this.v0 = v0; + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + this.v4 = v4; + this.v5 = v5; + this.v6 = v6; + this.v7 = v7; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + @Override + public T3 _2() { + return this.v2; + } + + @Override + public T4 _3() { + return this.v3; + } + + @Override + public T5 _4() { + return this.v4; + } + + @Override + public T6 _5() { + return this.v5; + } + + @Override + public T7 _6() { + return this.v6; + } + + @Override + public T8 _7() { + return this.v7; + } + + } + + + /** + * A type that has a 9th element. + */ + public static interface IValue9 { + + /** + * Get the 9th element. + */ + T _8(); + } + + + /** + * A tuple of 9 elements. + */ + public static final class Tuple9 + implements IValue1, IValue2, IValue3, IValue4, IValue5, IValue6, + IValue7, IValue8, IValue9 { + private final T1 v0; + private final T2 v1; + private final T3 v2; + private final T4 v3; + private final T5 v4; + private final T6 v5; + private final T7 v6; + private final T8 v7; + private final T9 v8; + + /** + * Create a new 9-tuple. + */ + public Tuple9(T1 v0, T2 v1, T3 v2, T4 v3, T5 v4, T6 v5, T7 v6, T8 v7, T9 v8) { + this.v0 = v0; + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + this.v4 = v4; + this.v5 = v5; + this.v6 = v6; + this.v7 = v7; + this.v8 = v8; + } + + @Override + public T1 _0() { + return this.v0; + } + + @Override + public T2 _1() { + return this.v1; + } + + @Override + public T3 _2() { + return this.v2; + } + + @Override + public T4 _3() { + return this.v3; + } + + @Override + public T5 _4() { + return this.v4; + } + + @Override + public T6 _5() { + return this.v5; + } + + @Override + public T7 _6() { + return this.v6; + } + + @Override + public T8 _7() { + return this.v7; + } + + @Override + public T9 _8() { + return this.v8; + } + + } + +} diff --git a/src/main/java/frc/robot/util/typestate/AltMethod.java b/src/main/java/frc/robot/util/typestate/AltMethod.java new file mode 100644 index 0000000..1b6da96 --- /dev/null +++ b/src/main/java/frc/robot/util/typestate/AltMethod.java @@ -0,0 +1,15 @@ +package frc.robot.util.typestate; + +/** Alternative builder step. */ +public @interface AltMethod { + + /** The alternative parameter type. */ + public Class type(); + + /** If specified, this parameter name is used instead of the constructor parameter name. */ + public String parameter_name() default ""; + + /** Stringified java expression converting the alternative type into the actual type. */ + public String value(); + +} diff --git a/src/main/java/frc/robot/util/typestate/InitField.java b/src/main/java/frc/robot/util/typestate/InitField.java new file mode 100644 index 0000000..a3121df --- /dev/null +++ b/src/main/java/frc/robot/util/typestate/InitField.java @@ -0,0 +1,12 @@ +package frc.robot.util.typestate; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** Marks this parameter as required to create the builder. */ +@Retention(RetentionPolicy.CLASS) +@Target(ElementType.PARAMETER) +public @interface InitField { +} diff --git a/src/main/java/frc/robot/util/typestate/OptionalField.java b/src/main/java/frc/robot/util/typestate/OptionalField.java new file mode 100644 index 0000000..170b5ed --- /dev/null +++ b/src/main/java/frc/robot/util/typestate/OptionalField.java @@ -0,0 +1,12 @@ +package frc.robot.util.typestate; + +/** Marks this parameter as optional. */ +public @interface OptionalField { + + /** Stringified java expression used when this parameter is not specified. */ + public String value(); + + /** Alternative build methods. */ + public AltMethod[] alt() default {}; + +} diff --git a/src/main/java/frc/robot/util/typestate/RequiredField.java b/src/main/java/frc/robot/util/typestate/RequiredField.java new file mode 100644 index 0000000..2bec093 --- /dev/null +++ b/src/main/java/frc/robot/util/typestate/RequiredField.java @@ -0,0 +1,8 @@ +package frc.robot.util.typestate; + +/** Field required to be specified before finishing. */ +public @interface RequiredField { + + /** Alternative build methods. */ + public AltMethod[] alt() default {}; +} diff --git a/src/main/java/frc/robot/util/typestate/TypeStateBuilder.java b/src/main/java/frc/robot/util/typestate/TypeStateBuilder.java new file mode 100644 index 0000000..56744eb --- /dev/null +++ b/src/main/java/frc/robot/util/typestate/TypeStateBuilder.java @@ -0,0 +1,19 @@ +package frc.robot.util.typestate; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** Generate TypeState Builder using RobotUtils */ +@Retention(RetentionPolicy.CLASS) +@Target(ElementType.CONSTRUCTOR) +public @interface TypeStateBuilder { + + /** + * If specified, the builder's class name is this. Otherwise, "Builder" is appended to the + * enclosing class's name. + */ + public String value() default ""; + +} diff --git a/src/main/java/frc/robot/viz/RobotViz.java b/src/main/java/frc/robot/viz/RobotViz.java new file mode 100644 index 0000000..e52f7ca --- /dev/null +++ b/src/main/java/frc/robot/viz/RobotViz.java @@ -0,0 +1,87 @@ +package frc.robot.viz; + +import java.util.Arrays; +import java.util.function.Supplier; +import org.jspecify.annotations.NullMarked; +import org.jspecify.annotations.Nullable; +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose3d; +import frc.robot.Constants; +import frc.robot.subsystems.swerve.Swerve; +import frc.robot.subsystems.swerve.SwerveSim; +import frc.robot.subsystems.vision.CameraConstants; + +/** + * Centralized visualization helper for publishing robot state to logging and visualization tools. + * + *

+ * Currently, this class publishes visualization data related to drivetrain state, including the + * robot's global pose and derived camera poses. + * + *

+ * Future intent: This class is intended to become the single location responsible for + * publishing visualization data for all robot subsystems, such as elevators, arms, + * intakes, and other articulated mechanisms. Each subsystem will contribute geometry or pose data + * that can be rendered together in a unified visualization view. + * + *

+ * This class supports both real and simulated operation: + *

    + *
  • When a {@link SwerveSim} instance is provided, visualization data is sourced from the + * simulator's ground-truth drivetrain pose.
    • + *
  • Otherwise, certain entries with keys indicating ground truth use estimates instead, often + * duplicating other entries (e.g. {@code ActualPose} will be equivalent to + * {@code GlobalEstPose}).
    • + *
+ * + *

+ * All outputs are published via {@link Logger} and are intended strictly for debugging, analysis, + * and visualization (e.g., AdvantageScope). No control or decision-making logic should depend on + * this class. + */ +@NullMarked +public class RobotViz { + + private final @Nullable Supplier robotPoseSupplier; + private final Supplier estPoseSupplier; + + /** + * Creates a new visualization helper. + * + * @param sim optional swerve simulator; when provided, simulated ground-truth drivetrain pose + * is used for visualization + * @param swerve live swerve subsystem providing pose estimates when not simulating + */ + public RobotViz(@Nullable SwerveSim sim, Swerve swerve) { + estPoseSupplier = () -> new Pose3d(swerve.state.getGlobalPoseEstimate()); + if (sim != null) { + robotPoseSupplier = () -> new Pose3d(sim.mapleSim.getSimulatedDriveTrainPose()); + } else { + robotPoseSupplier = estPoseSupplier; + } + } + + /** + * Publishes visualization data for the current control loop iteration. + * + *

+ * This method should be called periodically (e.g., from {@code robotPeriodic}). + * + *

+ * As additional subsystems are integrated, this method will be extended to publish poses, + * transforms, and geometry for other mechanisms relative to the robot frame. + */ + public void periodic() { + Pose3d robotPose = robotPoseSupplier.get(); + Pose3d estPose = estPoseSupplier.get(); + Logger.recordOutput("Viz/ActualPose", robotPose); + for (CameraConstants constants : Constants.Vision.cameraConstants) { + Logger.recordOutput("Viz/Cameras/" + constants.name + "/ActualPose", + robotPose.plus(constants.robotToCamera)); + } + Logger.recordOutput("Viz/GlobalEstPose", estPose); + Logger.recordOutput("Viz/CameraPoses", Arrays.stream(Constants.Vision.cameraConstants) + .map(consts -> robotPose.plus(consts.robotToCamera)).toArray(Pose3d[]::new)); + } + +} diff --git a/vendordeps/ChoreoLib-2025.0.3.json b/vendordeps/ChoreoLib-2025.0.3.json deleted file mode 100644 index 5a8cd54..0000000 --- a/vendordeps/ChoreoLib-2025.0.3.json +++ /dev/null @@ -1,44 +0,0 @@ -{ - "fileName": "ChoreoLib-2025.0.3.json", - "name": "ChoreoLib", - "version": "2025.0.3", - "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", - "frcYear": "2025", - "mavenUrls": [ - "https://lib.choreo.autos/dep", - "https://repo1.maven.org/maven2" - ], - "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", - "javaDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-java", - "version": "2025.0.3" - }, - { - "groupId": "com.google.code.gson", - "artifactId": "gson", - "version": "2.11.0" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "choreo", - "artifactId": "ChoreoLib-cpp", - "version": "2025.0.3", - "libName": "ChoreoLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file diff --git a/vendordeps/ChoreoLib2025.json b/vendordeps/ChoreoLib2025.json new file mode 100644 index 0000000..2c2d3c3 --- /dev/null +++ b/vendordeps/ChoreoLib2025.json @@ -0,0 +1,44 @@ +{ + "fileName": "ChoreoLib2025.json", + "name": "ChoreoLib", + "version": "2025.0.3", + "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", + "frcYear": "2025", + "mavenUrls": [ + "https://lib.choreo.autos/dep", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib2025.json", + "javaDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-java", + "version": "2025.0.3" + }, + { + "groupId": "com.google.code.gson", + "artifactId": "gson", + "version": "2.11.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "choreo", + "artifactId": "ChoreoLib-cpp", + "version": "2025.0.3", + "libName": "ChoreoLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} diff --git a/vendordeps/ReduxLib-2025.0.1.json b/vendordeps/ReduxLib-2025.0.1.json new file mode 100644 index 0000000..6cc750e --- /dev/null +++ b/vendordeps/ReduxLib-2025.0.1.json @@ -0,0 +1,72 @@ +{ + "fileName": "ReduxLib-2025.0.1.json", + "name": "ReduxLib", + "version": "2025.0.1", + "frcYear": "2025", + "uuid": "151ecca8-670b-4026-8160-cdd2679ef2bd", + "mavenUrls": [ + "https://maven.reduxrobotics.com/" + ], + "jsonUrl": "https://frcsdk.reduxrobotics.com/ReduxLib_2025.json", + "javaDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-java", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-cpp", + "version": "2025.0.1", + "libName": "ReduxLib", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + }, + { + "groupId": "com.reduxrobotics.frc", + "artifactId": "ReduxLib-driver", + "version": "2025.0.1", + "libName": "ReduxCore", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm32", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/Studica-2025.0.1.json b/vendordeps/Studica-2025.0.1.json new file mode 100644 index 0000000..5010be0 --- /dev/null +++ b/vendordeps/Studica-2025.0.1.json @@ -0,0 +1,71 @@ +{ + "fileName": "Studica-2025.0.1.json", + "name": "Studica", + "version": "2025.0.1", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2025", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2025/" + ], + "jsonUrl": "https://dev.studica.com/releases/2025/Studica-2025.0.1.json", + "cppDependencies": [ + { + "artifactId": "Studica-cpp", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "Studica", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + }, + { + "artifactId": "Studica-driver", + "binaryPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "groupId": "com.studica.frc", + "headerClassifier": "headers", + "libName": "StudicaDriver", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "version": "2025.0.1" + } + ], + "javaDependencies": [ + { + "artifactId": "Studica-java", + "groupId": "com.studica.frc", + "version": "2025.0.1" + } + ], + "jniDependencies": [ + { + "artifactId": "Studica-driver", + "groupId": "com.studica.frc", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ], + "version": "2025.0.1" + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..5c6484d --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.3.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.3.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.3.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.3.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.3.1" + } + ] +} \ No newline at end of file