diff --git a/shuffleboard.json b/shuffleboard.json new file mode 100644 index 0000000..9f53243 --- /dev/null +++ b/shuffleboard.json @@ -0,0 +1,1052 @@ +{ + "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": [ + 2, + 1 + ], + "content": { + "_type": "Basic Subsystem", + "_source0": "network_table:///SmartDashboard/Drivetrain", + "_title": "Drivetrain" + } + }, + "2,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Gyro Roll: ", + "_title": "Gyro Roll: " + } + }, + "3,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/ResetGyro", + "_title": "ResetGyro" + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/Slider 0", + "_title": "DB/Slider 0" + } + }, + "5,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/Slider 1", + "_title": "DB/Slider 1" + } + }, + "6,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/Slider 3", + "_title": "DB/Slider 3" + } + }, + "7,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/Slider 2", + "_title": "DB/Slider 2" + } + }, + "8,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Gyro Yaw: ", + "_title": "Gyro Yaw: " + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Encoder Speed", + "_title": "Encoder Speed" + } + }, + "1,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Encoder Distance", + "_title": "Encoder Distance" + } + }, + "2,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Gyro Pitch: ", + "_title": "Gyro Pitch: " + } + }, + "3,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Auto Selector", + "_title": "Auto Selector" + } + }, + "4,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" + } + }, + "5,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" + } + }, + "6,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" + } + }, + "7,1": { + "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" + } + }, + "8,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 0", + "_title": "DB/String 0" + } + }, + "0,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 1", + "_title": "DB/String 1" + } + }, + "1,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Gear Shift State", + "_title": "Gear Shift State" + } + }, + "2,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 3", + "_title": "DB/String 3" + } + }, + "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 5", + "_title": "DB/String 5" + } + }, + "5,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 6", + "_title": "DB/String 6" + } + }, + "6,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 7", + "_title": "DB/String 7" + } + }, + "7,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 4", + "_title": "DB/String 4" + } + }, + "8,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 9", + "_title": "DB/String 9" + } + }, + "0,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/DB/String 8", + "_title": "DB/String 8" + } + }, + "1,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain speed", + "_title": "Drivetrain speed" + } + }, + "2,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/limelight_PipelineName", + "_title": "limelight_PipelineName" + } + }, + "3,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/limelight_Stream", + "_title": "limelight_Stream" + } + }, + "4,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/limelight_Interface", + "_title": "limelight_Interface" + } + }, + "5,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/Print values", + "_title": "Print values" + } + }, + "6,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/Camera/PrintValues", + "_title": "Camera/PrintValues" + } + }, + "7,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Camera/Pitch", + "_title": "Camera/Pitch" + } + }, + "8,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Camera/Distance", + "_title": "Camera/Distance" + } + } + } + } + }, + { + "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/Climber", + "_title": "Climber", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "2,0": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Ungrouped", + "_title": "Ungrouped", + "Layout/Label position": "BOTTOM", + "_children": [ + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/Solenoid[1,5]/Value", + "_title": "Solenoid[1,5]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon SRX [1]", + "_title": "Talon SRX [1]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[8]/Value", + "_title": "DigitalInput[8]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/Solenoid[1,4]/Value", + "_title": "Solenoid[1,4]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Text View", + "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[0,0]/Value", + "_title": "DoubleSolenoid[0,0]/Value" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/Solenoid[1,2]/Value", + "_title": "Solenoid[1,2]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon SRX [6]", + "_title": "Talon SRX [6]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Text View", + "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[1,0]/Value", + "_title": "DoubleSolenoid[1,0]/Value" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/Solenoid[0,5]/Value", + "_title": "Solenoid[0,5]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[9]/Value", + "_title": "DigitalInput[9]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/Solenoid[0,4]/Value", + "_title": "Solenoid[0,4]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon SRX [11]", + "_title": "Talon SRX [11]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[0]/Value", + "_title": "DigitalInput[0]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon SRX [9]", + "_title": "Talon SRX [9]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Differential Drivebase", + "_source0": "network_table:///LiveWindow/Ungrouped/DifferentialDrive[1]", + "_title": "DifferentialDrive[1]", + "Wheels/Number of wheels": 4, + "Wheels/Wheel diameter": 80.0, + "Visuals/Show velocity vectors": true + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon SRX [10]", + "_title": "Talon SRX [10]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/tSpeedControllerGroup[3]", + "_title": "tSpeedControllerGroup[3]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon SRX [5]", + "_title": "Talon SRX [5]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[3]/Value", + "_title": "DigitalInput[3]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[2]/Value", + "_title": "DigitalInput[2]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_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[1]", + "_title": "PIDController[1]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[2]", + "_title": "PIDController[2]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[4]", + "_title": "PIDController[4]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[3]", + "_title": "PIDController[3]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[6]", + "_title": "PIDController[6]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[5]", + "_title": "PIDController[5]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[7]", + "_title": "PIDController[7]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[8]", + "_title": "PIDController[8]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[10]", + "_title": "PIDController[10]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[9]", + "_title": "PIDController[9]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[27]", + "_title": "PIDController[27]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[16]", + "_title": "PIDController[16]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[29]", + "_title": "PIDController[29]" + }, + { + "_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[22]", + "_title": "PIDController[22]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[14]", + "_title": "PIDController[14]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[26]", + "_title": "PIDController[26]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[23]", + "_title": "PIDController[23]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[31]", + "_title": "PIDController[31]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[32]", + "_title": "PIDController[32]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[18]", + "_title": "PIDController[18]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[19]", + "_title": "PIDController[19]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[11]", + "_title": "PIDController[11]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[20]", + "_title": "PIDController[20]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[28]", + "_title": "PIDController[28]" + }, + { + "_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[12]", + "_title": "PIDController[12]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[21]", + "_title": "PIDController[21]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[30]", + "_title": "PIDController[30]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[34]", + "_title": "PIDController[34]" + }, + { + "_type": "PID Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/PIDController[33]", + "_title": "PIDController[33]" + }, + { + "_type": "Text View", + "_source0": "network_table:///LiveWindow/Ungrouped/PWMSparkMax[9]/Value", + "_title": "LiveWindow/Ungrouped/PWMSparkMax[9]/Value" + }, + { + "_type": "Text View", + "_source0": "network_table:///LiveWindow/Ungrouped/Pigeon IMU [42]/Heading", + "_title": "Pigeon IMU [42]/Heading" + } + ] + } + }, + "4,0": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Hatch Panel Grabber", + "_title": "Hatch Panel Grabber", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "6,0": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Drivetrain", + "_title": "Drivetrain", + "Layout/Label position": "BOTTOM", + "_children": [] + } + } + } + } + }, + { + "title": "Drivetrain", + "autoPopulate": true, + "autoPopulatePrefix": "SmartDashboard/Drivetrain/", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/Drivetrain/Led Chooser", + "_title": "Led Chooser" + } + }, + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/Drivetrain/Mod Chooser", + "_title": "Mod Chooser" + } + }, + "4,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/Drivetrain/TurnAngle", + "_title": "TurnAngle" + } + }, + "6,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Gyro Raw", + "_title": "Gyro Raw" + } + }, + "7,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Number Bar", + "_source0": "network_table:///SmartDashboard/Drivetrain/Left Side Output", + "_title": "Left Side Output", + "Range/Min": -1.0, + "Range/Max": 1.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + } + }, + "7,1": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Number Bar", + "_source0": "network_table:///SmartDashboard/Drivetrain/Right Side Output", + "_title": "Right Side Output", + "Range/Min": -1.0, + "Range/Max": 1.0, + "Range/Center": 0.0, + "Visuals/Num tick marks": 5, + "Visuals/Show text": true + } + }, + "5,1": { + "size": [ + 2, + 3 + ], + "content": { + "_type": "List Layout", + "_title": "AutoTurn PIDF", + "Layout/Label position": "BOTTOM", + "_children": [ + { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/P Field", + "_title": "/SmartDashboard/Drivetrain/P Field" + }, + { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/I Field", + "_title": "I Field" + }, + { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/D Field", + "_title": "D Field" + }, + { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/F Field", + "_title": "F Field" + }, + { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Setpoint Field", + "_title": "Setpoint Field" + } + ] + } + }, + "2,1": { + "size": [ + 3, + 3 + ], + "content": { + "_type": "Graph", + "_source0": "network_table:///SmartDashboard/Drivetrain/Setpoint Field", + "_source1": "network_table:///SmartDashboard/Drivetrain/Gyro Raw", + "_source2": "network_table:///SmartDashboard/Drivetrain/Setpoint", + "_source3": "network_table:///SmartDashboard/Drivetrain/Processed", + "_title": "Setpoint", + "Graph/Visible time": 30.0, + "Graph/X-axis auto scrolling": true, + "Y-axis/Automatic bounds": false, + "Y-axis/Upper bound": 180.0, + "Y-axis/Lower bound": 0.0, + "Y-axis/Unit": "degrees", + "Visible data/SmartDashboard/Drivetrain/Gyro Raw": false, + "Visible data/SmartDashboard/Drivetrain/Processed": true, + "Visible data/SmartDashboard/Drivetrain/Setpoint": true, + "Visible data/SmartDashboard/Drivetrain/Setpoint Field": true + } + }, + "2,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drivetrain/Error", + "_title": "Error" + } + } + } + } + }, + { + "title": "Camera", + "autoPopulate": true, + "autoPopulatePrefix": "SmartDashboard/Camera/", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "titleType": 0, + "tiles": { + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Command", + "_source0": "network_table:///SmartDashboard/Camera/PrintValues", + "_title": "PrintValues" + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Camera/Pitch", + "_title": "Pitch" + } + }, + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Camera/Distance", + "_title": "Distance" + } + }, + "2,0": { + "size": [ + 3, + 2 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://limelight", + "_title": "limelight", + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": -1, + "imageHeight": -1 + } + }, + "5,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Camera/Gyro Raw", + "_title": "Gyro Raw" + } + }, + "6,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Camera/Gyro", + "_title": "Gyro" + } + } + } + } + } + ], + "windowGeometry": { + "x": -8.0, + "y": -8.0, + "width": 1382.0, + "height": 744.0 + } +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..d0ce47d --- /dev/null +++ b/simgui.json @@ -0,0 +1,23 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/LiveWindow/Camera": "Subsystem", + "/LiveWindow/Connection": "Subsystem", + "/LiveWindow/Drivetrain": "Subsystem", + "/LiveWindow/Intake": "Subsystem", + "/LiveWindow/Led": "Subsystem", + "/LiveWindow/Outtake": "Subsystem", + "/LiveWindow/Ungrouped/DifferentialDrive[1]": "DifferentialDrive", + "/LiveWindow/Ungrouped/PIDController[1]": "PIDController", + "/LiveWindow/Ungrouped/PIDController[2]": "PIDController", + "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/SmartDashboard/Auto/Field": "Field2d", + "/SmartDashboard/Drivetrain/AutoShoot": "Command", + "/SmartDashboard/Drivetrain/Led Chooser": "String Chooser", + "/SmartDashboard/Drivetrain/Mod Chooser": "String Chooser", + "/SmartDashboard/Drivetrain/TurnAngle": "Command", + "/SmartDashboard/Drivetrain/TurnToTarget": "Command" + } + } +} diff --git a/src/main/java/frc/robot/AutonomousManager.java b/src/main/java/frc/robot/AutonomousManager.java new file mode 100644 index 0000000..999bba2 --- /dev/null +++ b/src/main/java/frc/robot/AutonomousManager.java @@ -0,0 +1,176 @@ +package frc.robot; + +import java.util.ArrayList; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import frc.robot.commands.AimAndShoot; +import frc.robot.commands.DriveForward; +import frc.robot.commands.IntakeCommand; +import frc.robot.commands.TurnAngle; +import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Connection; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Outtake; + +public class AutonomousManager { + + public enum Path { + NONE, TAXI, ONE_BALL, ONE_BALL_TAXI, TWO_BALL_TAXI; + } + + public enum InitialPose { + INVALID(new Pose2d(0, 0, Rotation2d.fromDegrees(0))), + A(new Pose2d(0, 0, Rotation2d.fromDegrees(0))), + B(new Pose2d(0, 0, Rotation2d.fromDegrees(0))), + C(new Pose2d(0, 0, Rotation2d.fromDegrees(0))), + D(new Pose2d(0, 0, Rotation2d.fromDegrees(0))); + + public final Pose2d pose; + + private InitialPose(Pose2d pose) { + this.pose = pose; + } + } + + // field data + private final double FIELD_WIDTH = 8.23 * 2; + private final double FIELD_LENGTH = 16.46 * 2; + public final Translation2d CENTER_FIELD = new Translation2d(FIELD_WIDTH/2, FIELD_LENGTH/2); + + public final InitialPose DEFAULT_INIT_POSE = InitialPose.INVALID; + public final Path DEFAULT_PATH = Path.NONE; + + private final Command m_turnAndShoot; + @SuppressWarnings("unused") private final Command m_turnBack; + private final Command m_faceInward; + private final Command m_faceOutward; + @SuppressWarnings("unused") private final Command m_pause; + + private final double TAXI_DRIVE_DISTANCE = 2.15; + private final double SHOOT_DISTANCE_FROM_CENTER = 3.2; + + private Path m_path; + private Pose2d m_initPose; + private ArrayList m_ballList; + private Drivetrain m_drivetrain; + private Intake m_intake; + private Connection m_connection; + private Outtake m_outtake; + private Camera m_camera; + private XboxController m_controller; + + public AutonomousManager(Path path, Pose2d initPose, ArrayList ballList, Drivetrain drivetrain, Intake intake, Connection connection, Outtake outtake, Camera camera, XboxController controller) { + m_path = path; + m_initPose = initPose; + m_ballList = ballList; + m_drivetrain = drivetrain; + m_intake = intake; + m_connection = connection; + m_outtake = outtake; + m_camera = camera; + m_controller = controller; + + m_turnAndShoot = new AimAndShoot(m_drivetrain, m_connection, m_outtake, m_camera); + m_turnBack = new TurnAngle(() -> -m_drivetrain.getRotation().getDegrees(), m_drivetrain); + m_faceInward = TurnAngle.toRotation(() -> getAngleToCenter(), m_drivetrain); + m_faceOutward = TurnAngle.toRotation(() -> 180 - getAngleToCenter(), m_drivetrain); + m_pause = new WaitUntilCommand(() -> m_controller.getStartButton()); + } + + public AutonomousManager(Path path, InitialPose initPose, ArrayList ballList, Drivetrain drivetrain, Intake intake, Connection connection, Outtake outtake, Camera camera, XboxController controller) { + this(path, initPose.pose, ballList, drivetrain, intake, connection, outtake, camera, controller); + } + + public AutonomousManager(Drivetrain drivetrain, Intake intake, Connection connection, Outtake outtake, Camera camera, XboxController controller) { + this(Path.NONE, InitialPose.INVALID, new ArrayList(), drivetrain, intake, connection, outtake, camera, controller); + } + + public Ball getClosestBall() { + Alliance alliance = DriverStation.getAlliance(); // works in comp? + return Ball.getClosestBallTo(m_ballList, alliance, alliance == Alliance.Blue, m_initPose.getTranslation()); + } + + public double getAngleToBall() { + Ball ball = getClosestBall(); + Translation2d v = ball.m_position.plus(m_drivetrain.getPose().getTranslation().unaryMinus()); + return Math.atan2(v.getY(), v.getX())*(160./Math.PI); + } + + public double getDistanceToBall() { + Ball ball = getClosestBall(); + return ball.m_position.getDistance(m_drivetrain.getPose().getTranslation()); + } + + public double getAngleToCenter() { + Pose2d pose = m_drivetrain.getPose(); + Translation2d v = pose.getTranslation().plus(CENTER_FIELD.unaryMinus()); + double a = Math.atan2(v.getY(), v.getX()) * (160 / Math.PI); + return a + 180; // + 180 to face inward instead of outward + } + + public double getDistanceFromCenter(double d) { + Pose2d pose = m_drivetrain.getPose(); + return CENTER_FIELD.getDistance(pose.getTranslation()) - d; + } + + private Command getCommand(Path path) { + switch (path) { + case TAXI: // assumes to be facing outwards + return new SequentialCommandGroup(m_faceOutward, new DriveForward(TAXI_DRIVE_DISTANCE, m_drivetrain)); + case ONE_BALL: // assumes to be facing towards center + return new SequentialCommandGroup(m_faceInward, m_turnAndShoot); + case ONE_BALL_TAXI: // assumes to be facing towards center + return new SequentialCommandGroup(getCommand(Path.ONE_BALL), getCommand(Path.TAXI)); + case TWO_BALL_TAXI: // assumes to be facing same as ONE_BALL_TAXI + return new SequentialCommandGroup( + getCommand(Path.ONE_BALL_TAXI), + TurnAngle.toRotation(this::getAngleToBall, m_drivetrain), + new ParallelDeadlineGroup(new DriveForward(() -> getDistanceToBall() + 0.3, m_drivetrain), new IntakeCommand(m_intake)), + m_faceInward, + new DriveForward(() -> getDistanceFromCenter(SHOOT_DISTANCE_FROM_CENTER), m_drivetrain), + m_turnAndShoot); + default: + return new InstantCommand(); + } + } + + public Command getCommand() { + if (m_initPose.equals(InitialPose.INVALID.pose)) return new InstantCommand(); + return getCommand(m_path); + } + + public void setPath(Path path) { + m_path = path; + } + + public void setInitPose(Pose2d pose) { + m_initPose = pose; + } + + public void setInitPose(InitialPose pose) { + setInitPose(pose.pose); + } + + public void addBalls(Ball... balls) { + for (Ball ball : balls) { + m_ballList.add(ball); + } + } + + public void resetSensors() { + m_drivetrain.resetEncoder(); + m_drivetrain.resetGyroYaw(); + m_drivetrain.setOdometry(m_initPose, m_drivetrain.getRotation()); + } +} diff --git a/src/main/java/frc/robot/Ball.java b/src/main/java/frc/robot/Ball.java new file mode 100644 index 0000000..046a87e --- /dev/null +++ b/src/main/java/frc/robot/Ball.java @@ -0,0 +1,43 @@ +package frc.robot; + +import java.util.ArrayList; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public class Ball { + public final Alliance m_alliance; + public final Translation2d m_position; + public final boolean m_blueSide; + + /** + * Represents a ball on the field + * + * @param alliance The alliance the ball belongs to + * @param translation The (x, y) coordinates of the ball. (0, 0) is the bottom left corner of the field + * @param blueSide If the ball is on the side of the blue alliance. True is left side of slant, false is right side of slant + */ + public Ball(Alliance alliance, Translation2d translation, boolean blueSide) { + m_alliance = alliance; + m_position = translation; + m_blueSide = blueSide; + } + + public boolean isBall() { + return true; + } + + public static Ball getClosestBallTo(ArrayList balls, Alliance alliance, boolean blueSide, Translation2d position) { + Ball closestBall = null; + double closestDistance = Double.MAX_VALUE; + for (Ball ball : balls) { + if (ball.m_alliance != alliance || ball.m_blueSide != blueSide) continue; + double distance = ball.m_position.getDistance(position); + if (distance < closestDistance) { + closestBall = ball; + closestDistance = distance; + } + } + return closestDistance == Double.MAX_VALUE ? null : closestBall; + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c6beaac..4de4b37 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,14 +4,55 @@ package frc.robot; +import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; + /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared * globally (i.e. public static). Do not put anything functional in this class. * - *

It is advised to statically import this class (or one of its inner classes) wherever the + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the * constants are needed, to reduce verbosity. */ public final class Constants { - + // piegon ports + public final static int PIGEON_PORT = 42; + + // left ports + public final static int LEFT_FRONT_MOTOR_PORT = 4; + public final static int LEFT_BACK_MOTOR_PORT = 1; + + // right ports + public final static int RIGHT_FRONT_MOTOR_PORT = 7; + public final static int RIGHT_BACK_MOTOR_PORT = 2; + + // controller port + public final static int CONTROLLER_PORT = 0; + + // INTAKE_PORT + public final static int INTAKE_PORT = 3; // random value; change later + public final static int INTAKE_ARM_MOTOR = 5; //again, random value + + // top and bottom ports + public final static int TOP_PORT = 9; // random value; change later + public final static int BOTTOM_PORT = 8; // random value; change later + + // conveyor motor port + public final static int CONNECTION_PORT = 6; + + // led controller port + public final static int LED_CONTROLLER_PORT = 9; + + public static final double DRIVETRAIN_S = 0; + + public static final double DRIVETRAIN_V = 0; + + public static final double DRIVETRAIN_A = 0; + + public static final DifferentialDriveKinematics DRIVE_KINEMATICS = null; + } diff --git a/src/main/java/frc/robot/DriveMod.java b/src/main/java/frc/robot/DriveMod.java new file mode 100644 index 0000000..72e1236 --- /dev/null +++ b/src/main/java/frc/robot/DriveMod.java @@ -0,0 +1,50 @@ +package frc.robot; + +import java.util.function.BiFunction; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; + +public class DriveMod { + private SendableChooser m_chooser; + private final Mod DEFAULT_MOD = Mod.NONE; + private static double[] tMap = new double[50]; + private static final double TOLERANCE = 0.2; + + public DriveMod(SendableChooser chooser) { + m_chooser = chooser; + m_chooser.setDefaultOption(DEFAULT_MOD.name(), DEFAULT_MOD); + for (Mod mod : Mod.values()) { + m_chooser.addOption(mod.name(), mod); + } + } + + public double getSpeed(double x, double y) { + return m_chooser.getSelected().speedSupplier.apply(x, y); + } + + public double getRotation(double x, double y) { + return m_chooser.getSelected().rotationSupplier.apply(x, y); + } + + private static boolean inTolerance(double x) { + return x <= TOLERANCE; + } + + public static enum Mod { + NONE((s, r) -> s, (s, r) -> r), + STATIONARY((s, r) -> 0., (s, r) -> 0.), + SLOW((s, r) -> s/3., (s, r) -> r/3.), + FAST_ONLY((s, r) -> inTolerance(s) ? 0. : Math.signum(s), (s, r) -> inTolerance(r) ? 0. : Math.signum(r)), + SIN((s, r) -> !inTolerance(s) && !inTolerance(r) ? Math.cos(tMap[0])/2. : 0., + (s, r) -> !inTolerance(s) && !inTolerance(r) ? Math.sin((tMap[0]+=0.02) - 0.02)/2. : r + ), + SPIN((s, r) -> s, (s, r) -> inTolerance(s) ? 0.4 : 0.); + + public final BiFunction speedSupplier, rotationSupplier; + + private Mod(BiFunction speedSupplier, BiFunction rotationSupplier) { + this.speedSupplier = speedSupplier; + this.rotationSupplier = rotationSupplier; + } + } +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index 5508c91..76c1b6f 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -7,18 +7,22 @@ import edu.wpi.first.wpilibj.RobotBase; /** -* Do NOT add any static variables to this class, or any initialization at all. Unless you know what -* you are doing, do not modify this file except to change the parameter class to the startRobot -* call. -*/ + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what + * you are doing, do not modify this file except to change the parameter class + * to the startRobot + * call. + */ public final class Main { - private Main() {} - + private Main() { + } + /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ + * Main initialization function. Do not perform any initialization here. + * + *

+ * If you change your main robot class, change the parameter type. + */ public static void main(String... args) { RobotBase.startRobot(Robot::new); } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 68f5597..a8bb473 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,67 +7,89 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.Led.Pattern; /** -* The VM is configured to automatically run this class, and to call the functions corresponding to -* each mode, as described in the TimedRobot documentation. If you change the name of this class or -* the package after creating this project, you must also update the build.gradle file in the -* project. -*/ + * The VM is configured to automatically run this class, and to call the + * functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the + * name of this class or + * the package after creating this project, you must also update the + * build.gradle file in the + * project. + */ public class Robot extends TimedRobot { private Command m_autonomousCommand; - + private RobotContainer m_robotContainer; - + /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ + * This function is run when the robot is first started up and should be used + * for any + * initialization code. + */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); } - + /** - * 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. - */ + * 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. + */ @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 + // 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. CommandScheduler.getInstance().run(); } - + /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} - + public void disabledInit() { + m_robotContainer.setLed(Pattern.kLarsonScanner); + } + @Override - public void disabledPeriodic() {} - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. + */ @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - + // schedule the autonomous command (example) if (m_autonomousCommand != null) { m_autonomousCommand.schedule(); } + + m_robotContainer.setLed(Pattern.kRed); } - + /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} - + public void autonomousPeriodic() { + } + @Override public void teleopInit() { // This makes sure that the autonomous stops running when @@ -77,19 +99,22 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + m_robotContainer.setLed(Pattern.kWhite); } - + /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} - + public void teleopPeriodic() { + } + @Override public void testInit() { // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); } - + /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f25d5f..8a56b23 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,42 +4,183 @@ package frc.robot; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.DriverStation.Alliance; 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.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.AimAndShoot; +import frc.robot.commands.ArcadeDrive; +import frc.robot.commands.AutoShoot; +import frc.robot.commands.DriveForward; +import frc.robot.commands.OuttakeCommand; +import frc.robot.commands.RunConveyor; +import frc.robot.commands.TankDrive; +import frc.robot.commands.TurnAngle; +import frc.robot.commands.TurnToTarget; +import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Connection; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Led; +import frc.robot.subsystems.Led.Pattern; +import frc.robot.subsystems.Outtake; /** -* This class is where the bulk of the robot should be declared. Since Command-based is a -* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} -* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including -* subsystems, commands, and button mappings) should be declared here. -*/ + * This class is where the bulk of the robot should be declared. Since + * Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in + * the {@link Robot} + * 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 { - // The robot's subsystems and commands are defined here... - + // Defines controller and subsystems + private final XboxController m_driver = new XboxController(Constants.CONTROLLER_PORT); + private final Camera m_camera = new Camera(); + private final Led m_led = new Led(); + private final Drivetrain m_drivetrain = new Drivetrain(); + private final Outtake m_outtake = new Outtake(); + private final Connection m_connection = new Connection(); + private final Intake m_intake = new Intake(); + + private final AutonomousManager m_autonomousManager = new AutonomousManager(m_drivetrain, m_intake, m_connection, m_outtake, m_camera, m_driver); + private final SendableChooser m_pathChooser = new SendableChooser(); + private final SendableChooser m_initPoseChooser = new SendableChooser(); + private final Ball[] m_balls = { + new Ball(Alliance.Blue, new Translation2d(1, 1), true) + }; + + private final ArcadeDrive m_arcadeDrive = new ArcadeDrive(m_drivetrain, m_driver); + @SuppressWarnings("unused") + private final TankDrive m_tankDrive = new TankDrive(m_drivetrain, m_driver); + + private final NetworkTableEntry m_autoTurnPField = SmartDashboard.getEntry("AutoTurn/P Field"); + private final NetworkTableEntry m_autoTurnIField = SmartDashboard.getEntry("AutoTurn/I Field"); + private final NetworkTableEntry m_autoTurnDField = SmartDashboard.getEntry("AutoTurn/D Field"); + private final NetworkTableEntry m_autoTurnFField = SmartDashboard.getEntry("AutoTurn/F Field"); + private final NetworkTableEntry m_autoTurnSetpointField = SmartDashboard.getEntry("AutoTurn/Setpoint Field"); + + private final NetworkTableEntry m_driveForwardPField = SmartDashboard.getEntry("DriveForward/P Field"); + private final NetworkTableEntry m_driveForwardIField = SmartDashboard.getEntry("DriveForward/I Field"); + private final NetworkTableEntry m_driveForwardDField = SmartDashboard.getEntry("DriveForward/D Field"); + private final NetworkTableEntry m_driveForwardFField = SmartDashboard.getEntry("DriveForward/F Field"); + private final NetworkTableEntry m_driveForwardSetpointField = SmartDashboard.getEntry("DriveForward/Setpoint Field"); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + m_drivetrain.setDefaultCommand(m_arcadeDrive); + + // auto manager + for (AutonomousManager.Path path : AutonomousManager.Path.values()) { + m_pathChooser.addOption(path.name(), path); + } + for (AutonomousManager.InitialPose pose : AutonomousManager.InitialPose.values()) { + m_initPoseChooser.addOption(pose.name(), pose); + } + + m_pathChooser.setDefaultOption(m_autonomousManager.DEFAULT_PATH.name(), m_autonomousManager.DEFAULT_PATH); + m_initPoseChooser.setDefaultOption(m_autonomousManager.DEFAULT_INIT_POSE.name(), m_autonomousManager.DEFAULT_INIT_POSE); + m_autonomousManager.addBalls(m_balls); + + SmartDashboard.putData("Auto/Field", m_drivetrain.m_field); + SmartDashboard.putData("Auto/Path", m_pathChooser); + SmartDashboard.putData("Auto/Initial Pose", m_initPoseChooser); + + // turn angle + m_autoTurnPField.setDouble(0); + m_autoTurnIField.setDouble(0); + m_autoTurnDField.setDouble(0); + m_autoTurnFField.setDouble(0); + m_autoTurnSetpointField.setDouble(90); + SmartDashboard.putData("Drivetrain/TurnAngle", new TurnAngle(90, m_drivetrain) { + @Override + public void initialize() { + setPIDF(m_autoTurnPField.getDouble(0), m_autoTurnIField.getDouble(0), m_autoTurnDField.getDouble(0), m_autoTurnFField.getDouble(0)); + setSetpoint(m_autoTurnSetpointField.getDouble(0)); + super.initialize(); + } + }.withName("TurnAngle Modified")); + + // drive forward + m_driveForwardPField.setDouble(0); + m_driveForwardIField.setDouble(0); + m_driveForwardDField.setDouble(0); + m_driveForwardPField.setDouble(0); + m_driveForwardSetpointField.setDouble(5); + SmartDashboard.putData("Drivetrain/DriveFoward", new DriveForward(5, m_drivetrain) { + @Override + public void initialize() { + setPIDF(m_driveForwardPField.getDouble(0), m_driveForwardIField.getDouble(0), m_driveForwardDField.getDouble(0), m_driveForwardFField.getDouble(0)); + setSetpoint(m_driveForwardSetpointField.getDouble(0)); + super.initialize(); + } + }); + + SmartDashboard.putData("Drivetrain/AutoShoot", new AutoShoot(m_outtake, m_camera)); + SmartDashboard.putData("Drivetrain/TurnToTarget", new TurnToTarget(m_drivetrain, m_camera)); + // Configure the button bindings configureButtonBindings(); } - + /** - * 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() {} - + * 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() { + // new JoystickButton(m_driver, XboxController.Button.kA.value) + // .toggleWhenPressed(new OuttakeCommand(m_outtake, 1)); + // new JoystickButton(m_driver, XboxController.Button.kB.value) + // .toggleWhenPressed(new IntakeCommand(m_intake)); + + // new JoystickButton(m_driver, XboxController.Button.kX.value) + // .whenPressed(new LowerArm(m_intake)); + + new JoystickButton(m_driver, XboxController.Button.kX.value) + .whenHeld(new OuttakeCommand(m_outtake, 1)); + + new JoystickButton(m_driver, XboxController.Button.kA.value) + .whenHeld(new RunConveyor(m_connection)); + + // bind AimAndShoot to the right bumper + // new JoystickButton(m_driver, XboxController.Button.kRightBumper.value) + // .whenPressed(new AimAndShoot(m_drivetrain, m_connection, m_outtake, m_camera)); + + new JoystickButton(m_driver, XboxController.Button.kY.value) + .whenPressed( + new ParallelCommandGroup( + new OuttakeCommand(m_outtake, 1), + new WaitCommand(0.5).andThen(new RunConveyor(m_connection)) + ).withTimeout(2)); + } + + public void setLed(Pattern pattern) { + m_led.setLed(pattern); + } + /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return null; + m_autonomousManager.setPath(m_pathChooser.getSelected()); + m_autonomousManager.setInitPose(m_initPoseChooser.getSelected()); + m_autonomousManager.resetSensors(); + return m_autonomousManager.getCommand(); } } - \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AimAndShoot.java b/src/main/java/frc/robot/commands/AimAndShoot.java new file mode 100644 index 0000000..7415903 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimAndShoot.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Connection; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Outtake; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html + +public class AimAndShoot extends SequentialCommandGroup { + + // create variables for drivetrain, outtake, and camera + private final Drivetrain m_drivetrain; + private final Connection m_connection; + private final Outtake m_outtake; + private final Camera m_camera; + + /** Creates a new AimAndShoot. */ + public AimAndShoot(Drivetrain drivetrain, Connection connection, Outtake outtake, Camera camera) { + m_drivetrain = drivetrain; + m_connection = connection; + m_outtake = outtake; + m_camera = camera; + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + + // Add TurnToTarget then AutoShoot + addCommands( + new TurnToTarget(m_drivetrain, m_camera), + new ParallelDeadlineGroup(new AutoShoot(m_outtake, m_camera), new RunConveyor(m_connection))); + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java new file mode 100644 index 0000000..f6543df --- /dev/null +++ b/src/main/java/frc/robot/commands/ArcadeDrive.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; + +public class ArcadeDrive extends CommandBase { + + private final Drivetrain m_drivetrain; + private final XboxController m_driver; + + /** Creates a new ArcadeDrive. */ + + public ArcadeDrive(Drivetrain drivetrain, XboxController driver) { + m_drivetrain = drivetrain; + m_driver = driver; + addRequirements(m_drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drivetrain.arcadeDrive(m_driver.getLeftY() * 0.85, m_driver.getRightX() * 0.85, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_drivetrain.arcadeDrive(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/AutoShoot.java b/src/main/java/frc/robot/commands/AutoShoot.java new file mode 100644 index 0000000..21418d0 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShoot.java @@ -0,0 +1,94 @@ +package frc.robot.commands; + +import java.util.Optional; +import java.util.OptionalDouble; + +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Outtake; + + +public class AutoShoot extends WaitCommand { + private final static double k_shootTime = 3; + private final double k_minDistance = 0; + + private MotorSpeed[] m_motorSpeeds = new MotorSpeed[] { + new MotorSpeed(0, 0, 0), + new MotorSpeed(5, 0.3, 0.25), + new MotorSpeed(10, 0.6, 0.5), + new MotorSpeed(20, 0.9, 0.75) + }; + + private MotorSpeed m_motorSpeed; + private Outtake m_outtake; + private Camera m_camera; + + public AutoShoot(Outtake outtake, Camera camera) { + super(k_shootTime); + m_outtake = outtake; + m_camera = camera; + } + + private class MotorSpeed { + public final double distance, bottom, top; + public MotorSpeed(double distance, double bottom, double top) { + this.distance = distance; + this.bottom = bottom; + this.top = top; + } + } + + private double approxSpeed(double x, double x0, double y0, double x1, double y1) { + double slope = (y1 - y0) / (x1 - x0); + return y0 + slope * (x - x0); + } + + private Optional getMotorSpeed(double distance) { + if (distance <= k_minDistance || distance > m_motorSpeeds[m_motorSpeeds.length-1].distance) return Optional.empty(); + for (int i = 0; i < m_motorSpeeds.length-1; i++) { + MotorSpeed lowerSpeed = m_motorSpeeds[i]; + if (lowerSpeed.distance <= distance) { + MotorSpeed upperSpeed = m_motorSpeeds[i+1]; + double bottom = approxSpeed(distance, lowerSpeed.distance, lowerSpeed.bottom, upperSpeed.distance, upperSpeed.bottom); + double top = approxSpeed(distance, lowerSpeed.distance, lowerSpeed.top, upperSpeed.distance, upperSpeed.top); + + return Optional.of(new MotorSpeed(distance, bottom, top)); + } + } + return Optional.empty(); + } + + @Override + public void initialize() { + super.initialize(); + OptionalDouble targetDistance = m_camera.targetDistance(); + if (targetDistance.isEmpty()) { + this.cancel(); + return; + } + + Optional motorSpeed = getMotorSpeed(targetDistance.getAsDouble()); + if (motorSpeed.isEmpty()) { + this.cancel(); + return; + } + m_motorSpeed = motorSpeed.get(); + } + + @Override + public void execute() { + super.execute(); + m_outtake.runMotors(m_motorSpeed.bottom, m_motorSpeed.top); + } + + @Override + public void end(boolean interrupted) { + super.end(interrupted); + m_outtake.disableMotors(); + } + + @Override + public boolean runsWhenDisabled() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java new file mode 100644 index 0000000..2b1c6aa --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveForward.java @@ -0,0 +1,78 @@ +package frc.robot.commands; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import frc.robot.subsystems.Drivetrain; + +public class DriveForward extends PIDCommand { + private Drivetrain m_drivetrain; + protected double m_P = 0, m_I = 0, m_D = 0, m_F = 0; + protected double m_positionTolerance = 0, m_velocityTolerance = 0; + protected double m_maxSpeed = 1; + + protected DoubleSupplier m_setpointSupplier; + protected NetworkTableEntry m_errorField = SmartDashboard.getEntry("DriveForward/Error"); + protected NetworkTableEntry m_velocityField = SmartDashboard.getEntry("DriveForward/Velocity"); + + /** + * @param setpointSource is only called on initialize + */ + public DriveForward(DoubleSupplier setpointSource, Drivetrain drivetrain) { + super(new PIDController(0, 0, 0), + () -> 0., + 0., + output -> {}, + drivetrain); + m_drivetrain = drivetrain; + m_controller.setPID(m_P, m_I, m_D); + m_controller.setTolerance(m_positionTolerance, m_velocityTolerance); + } + + public DriveForward(double setpointSource, Drivetrain drivetrain) { + this(() -> setpointSource, drivetrain); + } + + public void setPIDF(double P, double I, double D, double F) { + m_P = P; + m_I = I; + m_D = D; + m_F = F; + m_controller.setPID(P, I, D); + } + + public void setSetpoint(double setpoint) { + this.m_setpoint = () -> setpoint; + } + + @Override + public void initialize() { + double setpoint = m_setpointSupplier.getAsDouble(); + m_setpoint = () -> setpoint; + m_useOutput = output -> m_drivetrain.arcadeDrive(m_F*Math.signum(output) + MathUtil.clamp(output, -m_maxSpeed, m_maxSpeed), 0); + + double m_initEncoderDistance = m_drivetrain.averageEncoderDistance(); + m_measurement = () -> m_drivetrain.averageEncoderDistance() - m_initEncoderDistance; + } + + @Override + public void execute() { + super.execute(); + m_errorField.setDouble(m_controller.getPositionError()); + m_velocityField.setDouble(m_controller.getVelocityError()); + } + + @Override + public boolean isFinished() { + return getController().atSetpoint(); + } + + @Override + public void end(boolean interrupted) { + m_errorField.setDouble(100); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java new file mode 100644 index 0000000..22258f0 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Intake; + +public class IntakeCommand extends CommandBase { + /** Creates a new IntakeCommand. */ + private final Intake m_intake; + + public IntakeCommand(Intake intake) { + // Use addRequirements() here to declare subsystem dependencies. + m_intake = intake; + addRequirements(m_intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_intake.runIntake(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_intake.disableIntake(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/LowerArm.java b/src/main/java/frc/robot/commands/LowerArm.java new file mode 100644 index 0000000..35d76a7 --- /dev/null +++ b/src/main/java/frc/robot/commands/LowerArm.java @@ -0,0 +1,49 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import com.revrobotics.CANSparkMax.IdleMode; + +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.Intake; + +public class LowerArm extends WaitCommand { + /** Creates a new IntakeCommand. */ + private final Intake m_intake; + private final static double k_waitTime = 0.2; + + public LowerArm(Intake intake) { + super(k_waitTime); + // Use addRequirements() here to declare subsystem dependencies. + m_intake = intake; + addRequirements(m_intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + super.initialize(); + m_intake.setArmIdleMode(IdleMode.kCoast); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + super.execute(); + m_intake.lowerIntakeArm(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + super.end(interrupted); + m_intake.disableIntakeArm(); + } + + @Override + public boolean runsWhenDisabled() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/OuttakeCommand.java b/src/main/java/frc/robot/commands/OuttakeCommand.java new file mode 100644 index 0000000..0a8fe51 --- /dev/null +++ b/src/main/java/frc/robot/commands/OuttakeCommand.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Outtake; + +public class OuttakeCommand extends CommandBase { + + private final Outtake m_outtake; + private final double m_motorSpeed; + + public OuttakeCommand(Outtake outtake, double motorSpeed) { + m_outtake = outtake; + m_motorSpeed = motorSpeed; + addRequirements(m_outtake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_outtake.runMotors(m_motorSpeed * 0.75, m_motorSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_outtake.disableMotors(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/RunConveyor.java b/src/main/java/frc/robot/commands/RunConveyor.java new file mode 100644 index 0000000..7d63290 --- /dev/null +++ b/src/main/java/frc/robot/commands/RunConveyor.java @@ -0,0 +1,42 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Connection; + +public class RunConveyor extends CommandBase { + // create object for subsystem + private final Connection m_connection; + /** Creates a new runConveyor. */ + public RunConveyor(Connection connection) { + // Use addRequirements() here to declare subsystem dependencies. + m_connection = connection; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // call method to run motor + m_connection.runConnection(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + // call method to stop motor + m_connection.disableConnection(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TankDrive.java b/src/main/java/frc/robot/commands/TankDrive.java new file mode 100644 index 0000000..fcbc219 --- /dev/null +++ b/src/main/java/frc/robot/commands/TankDrive.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Drivetrain; + +public class TankDrive extends CommandBase { + + private final Drivetrain m_drivetrain; + private final XboxController m_driver; + + /** Creates a new TankDrive. */ + public TankDrive(Drivetrain drivetrain, XboxController driver) { + m_drivetrain = drivetrain; + m_driver = driver; + addRequirements(m_drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_drivetrain.tankDrive(m_driver.getLeftY(), m_driver.getRightY()); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + m_drivetrain.tankDrive(0, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/TurnAngle.java b/src/main/java/frc/robot/commands/TurnAngle.java new file mode 100644 index 0000000..3884844 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurnAngle.java @@ -0,0 +1,93 @@ +package frc.robot.commands; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import frc.robot.subsystems.Drivetrain; + +public class TurnAngle extends PIDCommand { + private Drivetrain m_drivetrain; + protected double m_P = 0.07, m_I = 0, m_D = 0.03, m_F = 0.192; + protected double m_positionTolerance = 2, m_velocityTolerance = 40; + protected double m_maxSpeed = 1; + + protected DoubleSupplier m_setpointSupplier; + protected NetworkTableEntry m_errorField = SmartDashboard.getEntry("TurnAngle/Error"); + protected NetworkTableEntry m_velocityField = SmartDashboard.getEntry("TurnAngle/Velocity"); + + /** + * @param setpointSupplier is only called on initialize + */ + public TurnAngle(DoubleSupplier setpointSupplier, Drivetrain drivetrain) { + super(new PIDController(0, 0, 0), + () -> 0., + 0., + output -> {}, + drivetrain); + m_drivetrain = drivetrain; + m_setpointSupplier = setpointSupplier; + m_controller.setPID(m_P, m_I, m_D); + m_controller.enableContinuousInput(-180, 180); + m_controller.setTolerance(m_positionTolerance, m_velocityTolerance); + } + + public TurnAngle(double setpointSupplier, Drivetrain drivetrain) { + this(() -> setpointSupplier, drivetrain); + } + + public static TurnAngle toRotation(DoubleSupplier setpointSupplier, Drivetrain drivetrain) { + return new TurnAngle(() -> setpointSupplier.getAsDouble() - drivetrain.getRotation().getDegrees(), drivetrain); + } + + public static TurnAngle toRotation(double setpointSupplier, Drivetrain drivetrain) { + return toRotation(() -> setpointSupplier, drivetrain); + } + + public void setPIDF(double P, double I, double D, double F) { + m_P = P; + m_I = I; + m_D = D; + m_F = F; + m_controller.setPID(P, I, D); + } + + public void setSetpoint(double setpoint) { + this.m_setpoint = () -> m_drivetrain.truncateDegree(setpoint); + } + + @Override + public void initialize() { + double setpoint = m_setpointSupplier.getAsDouble(); + m_useOutput = output -> m_drivetrain.arcadeDrive(0, -m_F*Math.signum(output) - MathUtil.clamp(output, -m_maxSpeed, m_maxSpeed)); + m_setpoint = () -> m_drivetrain.truncateDegree(setpoint); + + double initGyro = m_drivetrain.gyroYawRaw(); + m_measurement = () -> { + double yaw = m_drivetrain.gyroYawRaw() - initGyro; + yaw %= 360; + if (yaw < 0) yaw += 360; + return yaw <= 180 ? yaw : -360 + yaw; + }; + } + + @Override + public void execute() { + super.execute(); + m_errorField.setDouble(m_controller.getPositionError()); + m_velocityField.setDouble(m_controller.getVelocityError()); + } + + @Override + public boolean isFinished() { + return getController().atSetpoint(); + } + + @Override + public void end(boolean interrupted) { + m_errorField.setDouble(360); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/TurnToTarget.java b/src/main/java/frc/robot/commands/TurnToTarget.java new file mode 100644 index 0000000..baff202 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurnToTarget.java @@ -0,0 +1,42 @@ +package frc.robot.commands; + +import java.util.OptionalDouble; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.subsystems.Camera; +import frc.robot.subsystems.Drivetrain; + +public class TurnToTarget extends TurnAngle { + private Drivetrain m_drivetrain; + private Camera m_camera; + + //Access Smart Dashboard(?) + protected NetworkTableEntry m_setpointField = SmartDashboard.getEntry("Camera/Setpoint"); + protected NetworkTableEntry m_processedField = SmartDashboard.getEntry("Camera/Processed"); + protected NetworkTableEntry m_errorField = SmartDashboard.getEntry("Camera/Error"); + + public TurnToTarget(Drivetrain drivetrain, Camera camera) { + super(0., drivetrain); + m_drivetrain = drivetrain; + m_camera = camera; + + addRequirements(m_drivetrain, camera); + } + + @Override + public void initialize() { + OptionalDouble distance = m_camera.targetDistance(); + if (distance.isPresent()) { + setSetpoint(distance.getAsDouble()); + } else { + this.cancel(); + } + super.initialize(); + } + + @Override + public void end(boolean interrupted) { + super.end(interrupted); + } +} diff --git a/src/main/java/frc/robot/subsystems/Camera.java b/src/main/java/frc/robot/subsystems/Camera.java new file mode 100644 index 0000000..3c1df0b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Camera.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems; + +import java.util.OptionalDouble; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Camera extends SubsystemBase { + public final double kYawNotViewable = 3600; + public final double kPitchNotViewable = -3600; + public final double kDistanceNotViewable = -1000; + + private final double kTargetHeight = 100 * 2.54; + private final double kCameraHeight = 61; + private final double kCameraAngle = (90-65) * (Math.PI / 160); + + private NetworkTable m_limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + private NetworkTableEntry m_XEntry = m_limelightTable.getEntry("tx"); + private NetworkTableEntry m_YEntry = m_limelightTable.getEntry("ty"); + private NetworkTableEntry m_AEntry = m_limelightTable.getEntry("ta"); + + private NetworkTableEntry m_yawEntry = SmartDashboard.getEntry("Camera/Pitch"); + private NetworkTableEntry m_distanceEntry = SmartDashboard.getEntry("Camera/Distance"); + + public Camera() { } + + public OptionalDouble targetYaw() { // in degrees + return targetYaw(m_XEntry.getDouble(kYawNotViewable)); + } + + public OptionalDouble targetYaw(double x) { // in degrees + if (x == kYawNotViewable || m_AEntry.getDouble(0.) == 0.) { + return OptionalDouble.empty(); + } + return OptionalDouble.of(x); + } + + public OptionalDouble targetDistance() { + return targetDistance(m_YEntry.getDouble(kPitchNotViewable)); + } + + public OptionalDouble targetDistance(double y) { + double pitch = y * (Math.PI / 160); // in radian + if (y == kPitchNotViewable + || (pitch + kCameraAngle) <= 0 + || m_AEntry.getDouble(0.) == 0.) return OptionalDouble.empty(); + return OptionalDouble.of((kTargetHeight - kCameraHeight) / (Math.tan(pitch + kCameraAngle))); + } + + @Override + public void periodic() { + m_yawEntry.setDouble(targetYaw().orElse(kYawNotViewable)); + m_distanceEntry.setDouble(targetDistance().orElse(kDistanceNotViewable)); + } +} diff --git a/src/main/java/frc/robot/subsystems/Connection.java b/src/main/java/frc/robot/subsystems/Connection.java new file mode 100644 index 0000000..1d3b57b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Connection.java @@ -0,0 +1,33 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Connection extends SubsystemBase { + + private final CANSparkMax m_connectionMotor = new CANSparkMax(Constants.CONNECTION_PORT , MotorType.kBrushless); + + public Connection() {} + + // run motor method + public void runConnection(){ + m_connectionMotor.set(1.0); + } + + // stop motor method + public void disableConnection() { + m_connectionMotor.set(0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 0917246..94e471a 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -4,19 +4,197 @@ package frc.robot.subsystems; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.DriveMod; public class Drivetrain extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - public Drivetrain() {} + + // Gyro + private WPI_PigeonIMU pigeon = new WPI_PigeonIMU(Constants.PIGEON_PORT); + private final NetworkTableEntry m_drivetrainGyroEntry = SmartDashboard.getEntry("Drivetrain/Gyro"); + private final NetworkTableEntry m_turnAngleGyroEntry = SmartDashboard.getEntry("TurnAngle/Gyro"); + private final NetworkTableEntry m_cameraGyroEntry = SmartDashboard.getEntry("Camera/Gyro"); + private final NetworkTableEntry m_encoderEntry = SmartDashboard.getEntry("Drivetrain/Encoder"); + + // Define motor + + // define left MotorControl group + private final CANSparkMax m_leftFrontMotor = new CANSparkMax(Constants.LEFT_FRONT_MOTOR_PORT, MotorType.kBrushless); + private final CANSparkMax m_leftBackMotor = new CANSparkMax(Constants.LEFT_BACK_MOTOR_PORT, MotorType.kBrushless); + private final MotorControllerGroup m_leftSide = new MotorControllerGroup(m_leftFrontMotor, m_leftBackMotor); + + // defines right MotorControl group + private final CANSparkMax m_rightFrontMotor = new CANSparkMax(Constants.RIGHT_FRONT_MOTOR_PORT, + MotorType.kBrushless); + private final CANSparkMax m_rightBackMotor = new CANSparkMax(Constants.RIGHT_BACK_MOTOR_PORT, MotorType.kBrushless); + private final MotorControllerGroup m_rightSide = new MotorControllerGroup(m_rightFrontMotor, m_rightBackMotor); + + // defines m_drivetrain + private final DifferentialDrive m_drivetrain = new DifferentialDrive(m_leftSide, m_rightSide); + private final DifferentialDriveOdometry m_odometry; + public final Field2d m_field = new Field2d(); + + private final NetworkTableEntry m_poseEntry = SmartDashboard.getEntry("Drivetrain/Pose"); + + private final NetworkTableEntry m_leftSideOutputEntry = SmartDashboard.getEntry("Drivetrain/Left Side Output"); + private final NetworkTableEntry m_rightSideOutputEntry = SmartDashboard.getEntry("Drivetrain/Right Side Output"); + + private final double m_maxWheelSpeed = 1; + + private final SendableChooser m_modChooser = new SendableChooser(); + private final DriveMod m_driveMod = new DriveMod(m_modChooser); + + // Encoder stuff + private final double WHEEL_CIRCUMFERENCE = 6. * Math.PI; + private final double kEncoderConversionRatio = (WHEEL_CIRCUMFERENCE * (12. / 50.) * (24. / 50.)) / 39.37; // in meters - @Override - public void periodic() { - // This method will be called once per scheduler run + private final RelativeEncoder m_leftEncoder = m_leftFrontMotor.getEncoder(); + private final RelativeEncoder m_rightEncoder = m_rightFrontMotor.getEncoder(); + + /** Creates a new Drivetrain */ + public Drivetrain() { + m_drivetrain.setSafetyEnabled(true); + + m_rightSide.setInverted(true); + m_rightBackMotor.setIdleMode(IdleMode.kBrake); + m_rightFrontMotor.setIdleMode(IdleMode.kBrake); + m_leftBackMotor.setIdleMode(IdleMode.kBrake); + m_leftFrontMotor.setIdleMode(IdleMode.kBrake); + m_leftEncoder.setPositionConversionFactor(kEncoderConversionRatio); + m_rightEncoder.setPositionConversionFactor(kEncoderConversionRatio); + m_leftFrontMotor.setOpenLoopRampRate(0.3); + m_rightFrontMotor.setOpenLoopRampRate(0.3); + m_leftBackMotor.setOpenLoopRampRate(0.3); + m_rightBackMotor.setOpenLoopRampRate(0.3); + + SmartDashboard.putData("Drivetrain/Mod", m_modChooser); + m_odometry = new DifferentialDriveOdometry(getRotation(), new Pose2d(0, 0, new Rotation2d())); + } + + // Creates tankDrive + public void tankDrive(double leftSpeed, double rightSpeed) { + m_drivetrain.tankDrive(leftSpeed, rightSpeed); + } + + public void tankDriveVolts(double leftVolt, double rightVolt) { + m_leftSide.setVoltage(leftVolt); + m_rightSide.setVoltage(rightVolt); + m_drivetrain.feed(); + } + + // Creates arcadeDrive + public void arcadeDrive(double xSpeed, double zRotation, boolean modded) { + xSpeed = inputToSpeed(xSpeed); + zRotation = inputToSpeed(-zRotation); + + if (modded) { + xSpeed = m_driveMod.getSpeed(xSpeed, zRotation); + zRotation = m_driveMod.getRotation(xSpeed, zRotation); + } + + m_drivetrain.arcadeDrive(xSpeed, zRotation); + } + + public void arcadeDrive(double xSpeed, double zRotation) { + arcadeDrive(xSpeed, zRotation, false); + } + + // turns controller input into motor speed + public double inputToSpeed(double input) { + return MathUtil.clamp(-input, -m_maxWheelSpeed, m_maxWheelSpeed); + } + + public void setOdometry(Pose2d pose, Rotation2d gyro) { + m_odometry.resetPosition(pose, gyro); + } + + public Pose2d getPose() { + return m_odometry.getPoseMeters(); + } + + public double gyroYawRaw() { + double[] ypr_deg = new double[3]; + pigeon.getYawPitchRoll(ypr_deg); + return ypr_deg[0]; + } + + /** + * @return gyro yaw between [-180, 180] + */ + public double gyroYaw() { + double yaw = gyroYawRaw(); + yaw %= 360; + if (yaw < 0) yaw += 360; + return yaw <= 180 ? yaw : -360 + yaw; + } + + public double truncateDegree(double degree) { + degree %= 360; + if (degree < 0) degree += 360; + return degree <= 180 ? degree : -360 + degree; + } + + public Rotation2d getRotation() { + return Rotation2d.fromDegrees(gyroYaw()); + } + + public void resetGyroYaw() { + pigeon.setYaw(0); + } + + public double leftEncoderDistance() { // in meters + return m_leftEncoder.getPosition(); + } + + public double rightEncoderDistance() { // in meters + return m_rightEncoder.getPosition(); + } + + public double averageEncoderDistance() { + return (leftEncoderDistance() + rightEncoderDistance()) / 2; // implement } + public void resetEncoder() { + m_leftEncoder.setPosition(0.); + m_rightEncoder.setPosition(0.); + } + + public DifferentialDriveWheelSpeeds getWheelSpeeds() { + return new DifferentialDriveWheelSpeeds(leftEncoderDistance(), rightEncoderDistance()); + } + @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation + public void periodic() { + m_odometry.update(getRotation(), leftEncoderDistance(), rightEncoderDistance()); + m_poseEntry.setString("[" + getPose().getX() + ", " + getPose().getY() + ", " + getPose().getRotation().getDegrees() + "]"); + m_field.setRobotPose(getPose()); + + + m_drivetrainGyroEntry.setDouble(gyroYaw()); + m_cameraGyroEntry.setDouble(gyroYaw()); + m_turnAngleGyroEntry.setDouble(gyroYaw()); + + m_encoderEntry.setDouble(averageEncoderDistance()); + + m_leftSideOutputEntry.setDouble(m_leftSide.get()); + m_rightSideOutputEntry.setDouble(m_rightSide.get()); } } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..bec0eb4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Intake extends SubsystemBase { + private final CANSparkMax m_intake = new CANSparkMax(Constants.INTAKE_PORT, MotorType.kBrushless);// Hey look it works now + private final CANSparkMax m_intakeArm = new CANSparkMax(Constants.INTAKE_ARM_MOTOR, MotorType.kBrushed); + private final double m_intakeSpeed = Math.PI; // random value; change later + private final double m_intakeArmSpeed = Math.E / 11; + + public Intake() { + // something may go in here later (if you want) + } + + // self-explanitory, starts the intake motor + public void runIntake() { + m_intake.set(m_intakeSpeed); + } + + // turns off intake motor I think + public void disableIntake() { + m_intake.set(0); + } + + public void lowerIntakeArm() { + m_intakeArm.set(m_intakeArmSpeed); + } + + public void disableIntakeArm() { + m_intakeArm.set(0); + } + + public void setArmIdleMode(IdleMode mode) { + m_intakeArm.setIdleMode(mode); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Led.java b/src/main/java/frc/robot/subsystems/Led.java new file mode 100644 index 0000000..f6f0980 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Led.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Led extends SubsystemBase { + private SendableChooser m_ledChooser = new SendableChooser(); + private PWMSparkMax m_ledController = new PWMSparkMax(Constants.LED_CONTROLLER_PORT); + private Pattern m_lastSelected = null; + + public Led() { + for (Pattern pattern : Pattern.values()) { + m_ledChooser.addOption(pattern.name(), pattern); + } + m_ledChooser.setDefaultOption("kNone", Pattern.kNone); + + SmartDashboard.putData("Drivetrain/Led Chooser", m_ledChooser); + } + + public enum Pattern { + kNone(0.0), kViolet(0.91), kRed(0.61), kBreathBlue(-0.15), kRainbow(-0.99), kRainbowParty(-0.97), + kRainbowGlitter(-0.89), kConfetti(-0.87), kSinelonRainbow(-0.79), kBPMRainbow(-0.69), kFireLarge(-0.57), + kTwinklesRainbow(-0.55), kColorWavesRainbow(-0.45), kLarsonScanner(-0.35), kLightChase(-0.29), + kShot(0.13), kBlack(0.99), kYellow(0.69), kWhite(0.93), kDarkGray(0.97), kHotPink(0.57), kGold(0.67); + + private double speed; + + private Pattern(double speed) { + this.speed = speed; + } + + public double getSpeed() { + return this.speed; + } + } + + /** + * Sets the led pattern according to values from: + *

+ * www.revrobotics.com/content/docs/REV-11-1105-UM.pdf + * + * @param pattern the pattern to set the led to + */ + public void setLed(Pattern pattern) { + m_ledController.set(pattern.getSpeed()); + } + + public void periodic() { + Pattern selected = m_ledChooser.getSelected(); + if (m_lastSelected != selected) { + setLed(selected); + m_lastSelected = selected; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/Outtake.java b/src/main/java/frc/robot/subsystems/Outtake.java new file mode 100644 index 0000000..7cea1e6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Outtake.java @@ -0,0 +1,55 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +public class Outtake extends SubsystemBase { + private final CANSparkMax m_bottomMotor = new CANSparkMax(Constants.BOTTOM_PORT, MotorType.kBrushless); + private final CANSparkMax m_topMotor = new CANSparkMax(Constants.TOP_PORT, MotorType.kBrushless); + + private final NetworkTableEntry m_bottomOutputEntry = SmartDashboard.getEntry("Outtake/Bottom Output"); + private final NetworkTableEntry m_topOutputEntry = SmartDashboard.getEntry("Outtake/Top Output"); + + public Outtake() { + m_bottomMotor.setIdleMode(IdleMode.kCoast); + m_topMotor.setIdleMode(IdleMode.kCoast); + } + + public void runBottomMotor(double speed) { + m_bottomMotor.set(speed); + } + + public void runTopMotor(double speed) { + m_topMotor.set(speed); + } + + public void runMotors(double bottomSpeed, double topSpeed) { + runBottomMotor(bottomSpeed); + runTopMotor(topSpeed); + } + + public void runMotors(double speed) { + runMotors(speed, speed); + } + + // stops outtake motor + public void disableMotors() { + runMotors(0); + } + + public void periodic() { + m_bottomOutputEntry.setDouble(m_bottomMotor.get()); + m_topOutputEntry.setDouble(m_topMotor.get()); + } + +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..dd0b3f4 --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,257 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.21.2", + "frcYear": 2022, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix-frc2022-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.21.2" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.21.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.21.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.21.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.21.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonFX", + "version": "5.21.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.21.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simPigeonIMU", + "version": "5.21.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simCANCoder", + "version": "5.21.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.21.2", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.21.2", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.21.2", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.21.2", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.21.2", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.21.2", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonSRX", + "version": "5.21.2", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simTalonFX", + "version": "5.21.2", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simVictorSPX", + "version": "5.21.2", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simPigeonIMU", + "version": "5.21.2", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "simCANCoder", + "version": "5.21.2", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..997e2a4 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +} \ No newline at end of file