diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv index a63fa24..e33cc7b 100644 --- a/src/main/deploy/Robot Data - Distances.csv +++ b/src/main/deploy/Robot Data - Distances.csv @@ -1,14 +1,7 @@ Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) -1,10,8000,2 -70,21,8000,2, check -100,24,9000,2 -145,28,10000,1 -230,31,12000,0,Add a 270 -246,32,15000,0 -320,32,17000,0,change 17000 and mark them lower -331,33,17000,0 -397,33,16000,0 -415,33,16250,0 -436,31,18000,0 -500,33,18000,0 -501,33,18000,0 \ No newline at end of file +70,20,7000,2.5, +127,27,8467,2, +223,31.25,10398,2.75, +272,32.4,11776,2.5, +342,33,13733,2, +458,30.5,17000,0, \ No newline at end of file diff --git a/src/main/deploy/Robot Data - DistancesOLD.csv b/src/main/deploy/Robot Data - DistancesOLD.csv new file mode 100644 index 0000000..a15874a --- /dev/null +++ b/src/main/deploy/Robot Data - DistancesOLD.csv @@ -0,0 +1,8 @@ +Distance (in),Hood Ext. (u),Drum Velocity (u/ds),Center Displacment (deg) +1,10,7769,0 +70,21,7769,0, check +100,24,8580,0 +145,28,9390,0 +230,31,11010,0,Add a 270 +397,33,14250,0 +415,33,14452,0 \ No newline at end of file diff --git a/src/main/driverStation/GOAT DRIVERSTATION.json b/src/main/driverStation/GOAT DRIVERSTATION.json new file mode 100644 index 0000000..a191e6e --- /dev/null +++ b/src/main/driverStation/GOAT DRIVERSTATION.json @@ -0,0 +1,865 @@ +{ + "tabPane": [ + { + "title": "SmartDashboard", + "autoPopulate": true, + "autoPopulatePrefix": "SmartDashboard/", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/limelight_Stream", + "_title": "limelight_Stream" + } + }, + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/limelight_PipelineName", + "_title": "limelight_PipelineName" + } + }, + "2,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/limelight_Interface", + "_title": "limelight_Interface" + } + }, + "3,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Auto?", + "_title": "Auto?" + } + }, + "4,0": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Gyro", + "_source0": "network_table:///SmartDashboard/Pigeon Gyro", + "_title": "Pigeon Gyro", + "Visuals/Major tick spacing": 45.0, + "Visuals/Starting angle": 180.0, + "Visuals/Show tick mark ring": true, + "Visuals/Counter clockwise": false + } + }, + "6,0": { + "size": [ + 3, + 2 + ], + "content": { + "_type": "Differential Drivebase", + "_source0": "network_table:///SmartDashboard/Drive Train", + "_title": "Drive Train", + "Wheels/Number of wheels": 4, + "Wheels/Wheel diameter": 80.0, + "Visuals/Show velocity vectors": true + } + }, + "9,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Left Motor Position Raw", + "_title": "Left Motor Position Raw" + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Right Motor Position Raw", + "_title": "Right Motor Position Raw" + } + }, + "1,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Average Motor Position Raw", + "_title": "Average Motor Position Raw" + } + }, + "2,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Avg Speed MPH", + "_title": "Avg Speed MPH" + } + }, + "3,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Gear Shift", + "_title": "Gear Shift", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "9,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drum Velocity", + "_title": "Drum Velocity" + } + }, + "0,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Drum Velocity CSV", + "_title": "Drum Velocity CSV" + } + }, + "1,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Shooter Temp C", + "_title": "Shooter Temp C" + } + }, + "2,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Shooter Current", + "_title": "Shooter Current" + } + }, + "3,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Drum Ready", + "_title": "Drum Ready", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Turret Angle Raw", + "_title": "Turret Angle Raw" + } + }, + "5,2": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Gyro", + "_source0": "network_table:///SmartDashboard/Turret Angle", + "_title": "Turret Angle", + "Visuals/Major tick spacing": 45.0, + "Visuals/Starting angle": 180.0, + "Visuals/Show tick mark ring": true, + "Visuals/Counter clockwise": false + } + }, + "7,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Turret Aimed", + "_title": "Turret Aimed", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "8,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Fire Angle CSV", + "_title": "Fire Angle CSV" + } + }, + "9,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Hood Angle Raw", + "_title": "Hood Angle Raw" + } + }, + "0,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Climber Safety", + "_title": "Climber Safety", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "1,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Rachet", + "_title": "Rachet", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "2,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Intake Beam", + "_title": "Intake Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "3,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Storage Beam", + "_title": "Storage Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Upper Beam", + "_title": "Upper Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "7,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Shooter Beam", + "_title": "Shooter Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + } + } + } + }, + { + "title": "LiveWindow", + "autoPopulate": true, + "autoPopulatePrefix": "LiveWindow/", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.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": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [5]", + "_title": "Talon FX [5]", + "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 FX [3]", + "_title": "Talon FX [3]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Spark[0]", + "_title": "Spark[0]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Speed Controller", + "_source0": "network_table:///LiveWindow/Ungrouped/Talon FX [8]", + "_title": "Talon FX [8]", + "Visuals/Orientation": "HORIZONTAL" + }, + { + "_type": "Text View", + "_source0": "network_table:///LiveWindow/Ungrouped/Servo[4]/Value", + "_title": "Servo[4]/Value" + }, + { + "_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[1]/Value", + "_title": "DigitalInput[1]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Text View", + "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[7,0]/Value", + "_title": "DoubleSolenoid[7,0]/Value" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[4]/Value", + "_title": "DigitalInput[4]/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": "Text View", + "_source0": "network_table:///LiveWindow/Ungrouped/DoubleSolenoid[7,3]/Value", + "_title": "DoubleSolenoid[7,3]/Value" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[13]/Value", + "_title": "DigitalInput[13]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[11]/Value", + "_title": "DigitalInput[11]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[14]/Value", + "_title": "DigitalInput[14]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + }, + { + "_type": "Boolean Box", + "_source0": "network_table:///LiveWindow/Ungrouped/DigitalInput[12]/Value", + "_title": "DigitalInput[12]/Value", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + ] + } + }, + "4,0": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/LimeLight", + "_title": "LimeLight", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "6,0": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Leveler", + "_title": "Leveler", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "8,0": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Drive", + "_title": "Drive", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "0,4": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Intake", + "_title": "Intake", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "2,4": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/ShooterHood", + "_title": "ShooterHood", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "4,4": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/LED", + "_title": "LED", + "Layout/Label position": "BOTTOM", + "_children": [] + } + }, + "6,4": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Shooter", + "_title": "Shooter", + "Layout/Label position": "BOTTOM", + "_children": [ + { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/ShooterHood", + "_title": "LiveWindow/ShooterHood", + "Layout/Label position": "BOTTOM", + "_children": [] + }, + { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/ShooterAim", + "_title": "LiveWindow/ShooterAim", + "Layout/Label position": "BOTTOM", + "_children": [] + } + ] + } + }, + "8,4": { + "size": [ + 2, + 4 + ], + "content": { + "_type": "Subsystem Layout", + "_source0": "network_table:///LiveWindow/Camera", + "_title": "Camera", + "Layout/Label position": "BOTTOM", + "_children": [] + } + } + } + } + }, + { + "title": "Driver Station", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "0,0": { + "size": [ + 4, + 4 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://front", + "_title": "front", + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "NONE", + "compression": -1.0, + "fps": -1, + "imageWidth": 0, + "imageHeight": 0 + } + }, + "3,4": { + "size": [ + 5, + 5 + ], + "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 + } + }, + "8,4": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Simple Dial", + "_source0": "network_table:///SmartDashboard/Drum Velocity", + "_title": "SmartDashboard/Drum Velocity", + "Range/Min": 0.0, + "Range/Max": 20000.0, + "Visuals/Show value": true + } + }, + "4,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Gear Shift", + "_title": "Gear Shift", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "5,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Rachet", + "_title": "Rachet", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "6,3": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Climber Safety", + "_title": "Climber Safety", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,1": { + "size": [ + 3, + 2 + ], + "content": { + "_type": "Differential Drivebase", + "_source0": "network_table:///SmartDashboard/Drive Train", + "_title": "SmartDashboard/Drive Train", + "Wheels/Number of wheels": 4, + "Wheels/Wheel diameter": 80.0, + "Visuals/Show velocity vectors": true + } + }, + "8,8": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Turret Aimed", + "_title": "/SmartDashboard/Turret Aimed", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "9,8": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Drum Ready", + "_title": "/SmartDashboard/Drum Ready", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "6,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Upper Beam", + "_title": "/SmartDashboard/Upper Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "5,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Storage Beam", + "_title": "/SmartDashboard/Storage Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "7,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Shooter Beam", + "_title": "/SmartDashboard/Shooter Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "4,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Boolean Box", + "_source0": "network_table:///SmartDashboard/Intake Beam", + "_title": "/SmartDashboard/Intake Beam", + "Colors/Color when true": "#7CFC00FF", + "Colors/Color when false": "#8B0000FF" + } + }, + "8,6": { + "size": [ + 2, + 2 + ], + "content": { + "_type": "Gyro", + "_source0": "network_table:///SmartDashboard/Turret Angle", + "_title": "SmartDashboard/Turret Angle", + "Visuals/Major tick spacing": 45.0, + "Visuals/Starting angle": 180.0, + "Visuals/Show tick mark ring": true, + "Visuals/Counter clockwise": false + } + }, + "7,1": { + "size": [ + 3, + 3 + ], + "content": { + "_type": "Gyro", + "_source0": "network_table:///SmartDashboard/Pigeon Gyro", + "_title": "SmartDashboard/Pigeon Gyro", + "Visuals/Major tick spacing": 45.0, + "Visuals/Starting angle": 180.0, + "Visuals/Show tick mark ring": true, + "Visuals/Counter clockwise": false + } + }, + "8,0": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Simple Dial", + "_source0": "network_table:///SmartDashboard/Avg Speed MPH", + "_title": "SmartDashboard/Avg Speed MPH", + "Range/Min": 0.0, + "Range/Max": 100.0, + "Visuals/Show value": true + } + }, + "8,5": { + "size": [ + 2, + 1 + ], + "content": { + "_type": "Simple Dial", + "_source0": "network_table:///SmartDashboard/Hood Angle Raw", + "_title": "SmartDashboard/Hood Angle Raw", + "Range/Min": 0.0, + "Range/Max": 66.0, + "Visuals/Show value": true + } + } + } + } + } + ], + "windowGeometry": { + "x": 40.0, + "y": 142.39999389648438, + "width": 1547.199951171875, + "height": 1481.5999755859375 + } +} \ No newline at end of file diff --git a/src/main/driverStation/themes/Ridgbotics/ridgeboticstheme.css b/src/main/driverStation/themes/Ridgbotics/ridgeboticstheme.css new file mode 100644 index 0000000..cb762ec --- /dev/null +++ b/src/main/driverStation/themes/Ridgbotics/ridgeboticstheme.css @@ -0,0 +1,9 @@ +@import "/edu/wpi/first/shuffleboard/app/midnight.css"; + +.root{ + -swatch-100: #66FF66; + -swatch-200: #4DFF4D; + -swatch-300: #1AFF1A; + -swatch-400: #00CC00; + -swatch-500: #009900; +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7443120..a69160e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -120,8 +120,9 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); - public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRUM_SHOOTER_GAINS = new Gains(1.5, 0.0, 100, 0.055, 0, 1.0); + //public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.55, 0.0, 100, 0.0, 0, 1.0); + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; public static final double ENCODER_TICKS_PER_REV = 2048; @@ -135,12 +136,16 @@ public final class Constants { public static final int TURRET_LEFT_SOFT_LIMIT = -55; public static final double TURRET_SPEED_MULTIPLIER = 0.3; public static final double TURRET_CALIBRATE_SPEED = 0.075; + public static final double TURRET_MOTOR_ROTS_PER_ROT = 101.04972; //89.56696; + public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = -28.452166; public static final int HOOD_UP_SOFT_LIMIT = 33; public static final int HOOD_DOWN_SOFT_LIMIT = 3; public static final double HOOD_CONVERT_SLOPE = 0.47; public static final double HOOD_CONVERT_B = 40.5; public static final double HOOD_CALIBRATE_SPEED = 0.2; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find public static final double DRUM_RAMP_LIMIT = 1000; public static final double DRUM_VELOCITY_BOUND = 300; @@ -208,9 +213,9 @@ public final class Constants { public static final double FOV = 29.8; //Field of view of limelight public static final double TARGET_HEIGHT = 71.5; public static final double LIME_ANGLE = 24.7; - public static final double TURN_P_VALUE = 0.6; + public static final double TURN_P_VALUE = 0.8; public static final double X_ANGLE_ERROR = 1.3; - public static final double MOTOR_DEAD_ZONE = 0.3; + public static final double MOTOR_DEAD_ZONE = 0.2; public static final double DISTANCE_ERROR_EQUATION_M = 1.1279; public static final double DISTANCE_ERROR_EQUATION_B = -15.0684; public static final double GRAV = 385.83; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c4a1d3e..dca2c7e 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -129,7 +129,7 @@ public class RobotContainer { m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); m_robotShooterHood.passRequiredSubsystem(m_robotShooter); - m_robotShooterAim.passRequiredSubsystem(m_robotShooter); + m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive); m_robotLeveler.passRequiredSubsystem(m_robotClimber); diff --git a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java index a2be80e..1ec57b0 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionMM.java @@ -33,9 +33,9 @@ public class DriveStraightToPositionMM extends CommandBase { m_pneumatics = subsystem2; try { if (m_pneumatics.m_isSpeedShiftHigh) { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; } else { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; } } catch (Exception e) { System.err.println("Error In Motion Magic Switch Gains."); diff --git a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java index 337ff6d..b910380 100644 --- a/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/drive/DriveStraightToPositionPID.java @@ -31,9 +31,9 @@ public class DriveStraightToPositionPID extends CommandBase { m_pneumatics = subsystem2; try { if (m_pneumatics.m_isSpeedShiftHigh) { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH * 2; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; } else { - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; } } catch (Exception e) { System.err.println("Error In Motion Magic Switch Gains."); diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java index 75a6896..b3d3ac1 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java @@ -34,7 +34,18 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel()); + //Tells whether the target velocity has been reached + m_actualVel = m_shooter.m_shooterFalcon.getSelectedSensorPosition(); + m_targetVel = m_shooter.addFireVel(); + double error = m_actualVel - m_targetVel; + if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){ + m_shooter.m_isDrumReady = true; + m_shooter.runDrumShooterVelocityPID(m_targetVel); + } + else{ + m_shooter.m_isDrumReady = false; + m_shooter.runDrumShooterVelocityPID(m_targetVel); + } } // Called once the command ends or is interrupted. @@ -45,16 +56,6 @@ public class ShooterVelocityControlPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - //Tells whether the target velocity has been reached - double upperBound = m_targetVel + ShooterConstants.DRUM_VELOCITY_BOUND; - double lowerBound = m_targetVel - ShooterConstants.DRUM_VELOCITY_BOUND; - if (m_actualVel < upperBound && m_actualVel > lowerBound){ - m_shooter.m_isDrumReady = true; - } - else{ - m_shooter.m_isDrumReady = false; - } - return false; } } diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index a1e688e..3f46e9f 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -110,6 +110,7 @@ public class TrackTarget extends CommandBase { m_shooter.m_fireVel = fireVel; m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; + m_shooterAim.m_targetDistance = distance; } } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fe96ca2..f833621 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -196,6 +196,8 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.SensorDifference, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSelectedFeedbackCoefficient(0.5, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + /* * Configure the Pigeon IMU to the other Remote Slot available on the right * Talon @@ -708,6 +710,16 @@ public class Drive extends SubsystemBase { return meters * DriveConstants.INCHES_PER_METER; } + /** + * Converts a value in inches per second to miles per hour + * @param ips The value in inches per second to convert + * @return The value in miles per hour + */ + public double inchesPerSecondToMilesPerHour(double ips) { + double mph = ips * (3600) * (1/63360); + return mph; + } + public void setRightMotorGains(boolean isHighGear) { if (!isHighGear) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); @@ -827,6 +839,7 @@ public class Drive extends SubsystemBase { public void updateSmartDashboard() { try { + // SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); // SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); // SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); @@ -834,6 +847,14 @@ public class Drive extends SubsystemBase { SmartDashboard.putData("Pigeon Gyro", m_pigeonGyro); SmartDashboard.putData("Drive Train", m_driveTrain); + SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition()); + SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition()); + + SmartDashboard.putNumber("Average Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); + + double avgSpeedMPH = inchesPerSecondToMilesPerHour(10 * ticksToInches(m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY))); + + SmartDashboard.putNumber("Avg Speed MPH", avgSpeedMPH); //SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); //SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); @@ -848,8 +869,6 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); - //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); @@ -888,8 +907,8 @@ public class Drive extends SubsystemBase { SmartDashboard.putString("Odometry Values Meters", getPose().toString()); //SmartDashboard.putNumber("Odometry Heading", getHeading()); - SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); - SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + //SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); if (m_currentSong != m_songChooser.getSelected()){ m_currentSong = m_songChooser.getSelected(); diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 1b7a0c2..890e0f7 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -54,7 +54,7 @@ public class Shooter extends SubsystemBase { m_shooterFalcon.setNeutralMode(NeutralMode.Coast); m_shooterFalcon.setInverted(true); m_shooterFalcon.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalcon.configClosedloopRamp(1.0, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalcon.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); setShooterGains(); m_shooterFalcon.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); @@ -83,17 +83,15 @@ public class Shooter extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run try{ - SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Drum Velocity", m_shooterFalcon.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); + SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature()); + SmartDashboard.putNumber("Shooter Temp C", m_shooterFalcon.getTemperature()); - SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); + SmartDashboard.putNumber("Shooter Current", m_shooterFalcon.getSupplyCurrent()); - //SmartDashboard.putNumber("Hood Position", m_shooter.getAnglePosition()); - - SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady); + SmartDashboard.putBoolean("Drum Ready" , m_isDrumReady); } catch(Exception e) diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java index 424175c..db9d231 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterAim.java @@ -17,6 +17,7 @@ import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.ControlType; +import edu.wpi.first.wpilibj.GyroBase; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -24,10 +25,14 @@ import frc4388.utility.Gains; public class ShooterAim extends SubsystemBase { public Shooter m_shooterSubsystem; + public Drive m_driveSubsystem; public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; + public GyroBase m_turretGyro; + + public double m_targetDistance = 0; public boolean m_isAimReady = false; @@ -42,6 +47,8 @@ public class ShooterAim extends SubsystemBase { //resetGyroShooterRotate(); m_shooterRotateMotor.setIdleMode(IdleMode.kBrake); + m_turretGyro = getGyroInterface(); + m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_shooterRightLimit.enableLimitSwitch(true); @@ -58,6 +65,10 @@ public class ShooterAim extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Turret Angle Raw", getShooterRotatePosition()); + + SmartDashboard.putData("Turret Angle", m_turretGyro); + SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady); } @@ -65,8 +76,9 @@ public class ShooterAim extends SubsystemBase { * Passes subsystem needed. * @param subsystem Subsystem needed. */ - public void passRequiredSubsystem(Shooter subsystem) { - m_shooterSubsystem = subsystem; + public void passRequiredSubsystem(Shooter subsystem0, Drive subsystem1) { + m_shooterSubsystem = subsystem0; + m_driveSubsystem = subsystem1; } public void runShooterWithInput(double input) { @@ -98,6 +110,68 @@ public class ShooterAim extends SubsystemBase { public double getShooterRotatePosition() { - return m_shooterRotateMotor.getEncoder().getPosition(); + return m_shooterRotateEncoder.getPosition(); + } + + public double getShooterAngleDegrees() { + return (m_shooterRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; + } + + /** + * Gets the angle of the Shooter relative to the target. + */ + public double getTargetAngleDegrees() { + return m_driveSubsystem.getHeading() + getShooterAngleDegrees(); + } + + public double getTargetXDisplacement() { + return m_targetDistance * Math.cos(getTargetAngleDegrees()); + } + + public double getTargetYDisplacement() { + return m_targetDistance * Math.sin(getTargetAngleDegrees()); + } + + /** + * Gets the angle of the Shooter relative to the inner port. + * Use for tuning the Shooter Displacement + */ + public double getInnerPortAngleDegrees() { + return Math.atan( getTargetYDisplacement() / (getTargetXDisplacement() + 29.25) ); + } + + public GyroBase getGyroInterface() { + return new GyroBase(){ + + @Override + public void close() throws Exception { + // TODO Auto-generated method stub + + } + + @Override + public void reset() { + // TODO Auto-generated method stub + + } + + @Override + public double getRate() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public double getAngle() { + // TODO Auto-generated method stub + return getShooterAngleDegrees(); + } + + @Override + public void calibrate() { + // TODO Auto-generated method stub + + } + }; } } diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java index c177c38..6f3bda9 100644 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ b/src/main/java/frc4388/robot/subsystems/ShooterHood.java @@ -58,6 +58,8 @@ public class ShooterHood extends SubsystemBase { public void periodic() { // This method will be called once per scheduler run SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); + + SmartDashboard.putNumber("Hood Angle Raw", getAnglePositionDegrees()); } /** @@ -93,4 +95,8 @@ public class ShooterHood extends SubsystemBase { public double getAnglePosition(){ return m_angleEncoder.getPosition(); } + + public double getAnglePositionDegrees(){ + return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; + } }