mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-08 16:28:07 -06:00
Remove angle offset from shooter table
Fix logging using format strings Add autonomous chooser to the dashboard Update path recorder Move path commands to a class
This commit is contained in:
nathanrsxtn
@@ -18,6 +18,7 @@
|
||||
- [ ] Fix all XXX comments.
|
||||
- [ ] Visit TODO comments.
|
||||
- [ ] Forward the limelight port so it can be accessed over a USB tether.
|
||||
- [x] Remove turret offset from shooter tables.
|
||||
## Planned
|
||||
- [ ] Shuffleboard replacements options:
|
||||
- [ ] Run a web server on the robot and bypass network tables.
|
||||
|
||||
+255
-93
@@ -18,12 +18,178 @@
|
||||
"autoPopulate": true,
|
||||
"autoPopulatePrefix": "LiveWindow/",
|
||||
"widgetPane": {
|
||||
"gridSize": 128.0,
|
||||
"gridSize": 60.0,
|
||||
"showGrid": true,
|
||||
"hgap": 16.0,
|
||||
"vgap": 16.0,
|
||||
"hgap": 10.0,
|
||||
"vgap": 10.0,
|
||||
"titleType": 0,
|
||||
"tiles": {}
|
||||
"tiles": {
|
||||
"4,5": {
|
||||
"size": [
|
||||
4,
|
||||
6
|
||||
],
|
||||
"content": {
|
||||
"_type": "Subsystem Layout",
|
||||
"_source0": "network_table:///LiveWindow/Climber",
|
||||
"_title": "LiveWindow/Climber",
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Climber/Left Claw/Value",
|
||||
"_title": "Left Claw/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Climber/Right Claw/Value",
|
||||
"_title": "Right Claw/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Climber/Elbow/Value",
|
||||
"_title": "Elbow/Value"
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"0,0": {
|
||||
"size": [
|
||||
4,
|
||||
13
|
||||
],
|
||||
"content": {
|
||||
"_type": "Subsystem Layout",
|
||||
"_source0": "network_table:///LiveWindow/Drive",
|
||||
"_title": "LiveWindow/Drive",
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Left Front Drive Motor/Value",
|
||||
"_title": "Left Front Drive Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Right Front Steer Motor/Value",
|
||||
"_title": "Right Front Steer Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Right Back Steer Motor/Value",
|
||||
"_title": "Right Back Steer Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Right Back Drive Motor/Value",
|
||||
"_title": "Right Back Drive Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Left Back Steer Motor/Value",
|
||||
"_title": "Left Back Steer Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Left Front Steer Motor/Value",
|
||||
"_title": "Left Front Steer Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Left Back Drive Motor/Value",
|
||||
"_title": "Left Back Drive Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Drive/Right Front Drive Motor/Value",
|
||||
"_title": "Right Front Drive Motor/Value"
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"4,11": {
|
||||
"size": [
|
||||
4,
|
||||
2
|
||||
],
|
||||
"content": {
|
||||
"_type": "Subsystem Layout",
|
||||
"_source0": "network_table:///LiveWindow/Intake",
|
||||
"_title": "LiveWindow/Intake",
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Intake/Intake Motor/Value",
|
||||
"_title": "Intake Motor/Value"
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"4,0": {
|
||||
"size": [
|
||||
4,
|
||||
5
|
||||
],
|
||||
"content": {
|
||||
"_type": "Subsystem Layout",
|
||||
"_source0": "network_table:///LiveWindow/Shooter",
|
||||
"_title": "LiveWindow/Shooter",
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Shooter/BoomBoom Left Motor/Value",
|
||||
"_title": "BoomBoom Left Motor/Value"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Shooter/BoomBoom Right Motor/Value",
|
||||
"_title": "BoomBoom Right Motor/Value"
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"8,0": {
|
||||
"size": [
|
||||
5,
|
||||
13
|
||||
],
|
||||
"content": {
|
||||
"_type": "Subsystem Layout",
|
||||
"_source0": "network_table:///LiveWindow/Ungrouped",
|
||||
"_title": "LiveWindow/Ungrouped",
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Ungrouped/Spark[4]/Value",
|
||||
"_title": "Spark[4]/Value"
|
||||
},
|
||||
{
|
||||
"_type": "PID Controller",
|
||||
"_source0": "network_table:///LiveWindow/Ungrouped/PIDController[1]",
|
||||
"_title": "PIDController[1]"
|
||||
},
|
||||
{
|
||||
"_type": "PID Controller",
|
||||
"_source0": "network_table:///LiveWindow/Ungrouped/PIDController[3]",
|
||||
"_title": "PIDController[3]"
|
||||
},
|
||||
{
|
||||
"_type": "Text View",
|
||||
"_source0": "network_table:///LiveWindow/Ungrouped/Pigeon 2 [14]/Heading",
|
||||
"_title": "Pigeon 2 [14]/Heading"
|
||||
},
|
||||
{
|
||||
"_type": "PID Controller",
|
||||
"_source0": "network_table:///LiveWindow/Ungrouped/PIDController[2]",
|
||||
"_title": "PIDController[2]"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -37,68 +203,6 @@
|
||||
"vgap": 10.0,
|
||||
"titleType": 1,
|
||||
"tiles": {
|
||||
"0,0": {
|
||||
"size": [
|
||||
6,
|
||||
8
|
||||
],
|
||||
"content": {
|
||||
"_type": "Grid Layout",
|
||||
"_title": "Match",
|
||||
"Layout/Number of columns": 1,
|
||||
"Layout/Number of rows": 3,
|
||||
"Layout/Label position": "HIDDEN",
|
||||
"_children": {
|
||||
"0,0": {
|
||||
"_type": "Basic FMS Info",
|
||||
"_title": "Basic FMS Info"
|
||||
},
|
||||
"0,1": {
|
||||
"_type": "Field",
|
||||
"_title": "Field",
|
||||
"Game/Game": "A2021_Infinite_Recharge",
|
||||
"Visuals/Robot Icon Size": 50.0,
|
||||
"Visuals/Show Outside Circles": false
|
||||
},
|
||||
"0,2": {
|
||||
"_type": "Grid Layout",
|
||||
"_title": "Match Time",
|
||||
"Layout/Number of columns": 3,
|
||||
"Layout/Number of rows": 1,
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": {
|
||||
"0,0": {
|
||||
"_type": "Number Bar",
|
||||
"_title": "Autonomous",
|
||||
"Range/Min": 0.0,
|
||||
"Range/Max": 15.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 4,
|
||||
"Visuals/Show text": true
|
||||
},
|
||||
"1,0": {
|
||||
"_type": "Number Bar",
|
||||
"_title": "Teleoperated",
|
||||
"Range/Min": 0.0,
|
||||
"Range/Max": 90.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 5,
|
||||
"Visuals/Show text": true
|
||||
},
|
||||
"2,0": {
|
||||
"_type": "Number Bar",
|
||||
"_title": "Climb",
|
||||
"Range/Min": 0.0,
|
||||
"Range/Max": 30.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 5,
|
||||
"Visuals/Show text": true
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"0,8": {
|
||||
"size": [
|
||||
6,
|
||||
@@ -111,16 +215,18 @@
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Graph",
|
||||
"_source0": "network_table:///Robot/Values/Voltage",
|
||||
"_title": "Graph",
|
||||
"Graph/Visible time": 30.0,
|
||||
"Graph/Visible time": 15.0,
|
||||
"Graph/X-axis auto scrolling": true,
|
||||
"Y-axis/Automatic bounds": true,
|
||||
"Y-axis/Upper bound": 1.0,
|
||||
"Y-axis/Lower bound": -1.0,
|
||||
"Y-axis/Unit": "ul"
|
||||
"Y-axis/Upper bound": 14.0,
|
||||
"Y-axis/Lower bound": 0.0,
|
||||
"Y-axis/Unit": "V"
|
||||
},
|
||||
{
|
||||
"_type": "PDP",
|
||||
"_source0": "network_table:///Robot/PDP",
|
||||
"_title": "PDP",
|
||||
"Visuals/Show voltage and current values": true
|
||||
}
|
||||
@@ -134,6 +240,7 @@
|
||||
],
|
||||
"content": {
|
||||
"_type": "Robot Preferences",
|
||||
"_source0": "network_table:///Preferences",
|
||||
"_title": "Robot Preferences",
|
||||
"Miscellaneous/Show search box": false
|
||||
}
|
||||
@@ -150,7 +257,8 @@
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Swerve Drivebase",
|
||||
"_title": "",
|
||||
"_source0": "network_table:///Robot/Drivebase",
|
||||
"_title": "/Robot/Drive/Drivebase",
|
||||
"Visuals/Show velocity vectors": true
|
||||
},
|
||||
{
|
||||
@@ -169,10 +277,12 @@
|
||||
"_children": {
|
||||
"0,1": {
|
||||
"_type": "Split Button Chooser",
|
||||
"_source0": "network_table:///Robot/Drive Speed",
|
||||
"_title": "Speed"
|
||||
},
|
||||
"0,0": {
|
||||
"_type": "3-Axis Accelerometer",
|
||||
"_source0": "network_table:///Robot/Accelerometer",
|
||||
"_title": "Accelerometer",
|
||||
"Accelerometer/Range": "k16G",
|
||||
"Visuals/Show value": true,
|
||||
@@ -183,6 +293,7 @@
|
||||
},
|
||||
"0,0": {
|
||||
"_type": "Gyro",
|
||||
"_source0": "network_table:///Robot/Gyro",
|
||||
"_title": "Gyro",
|
||||
"Visuals/Major tick spacing": 45.0,
|
||||
"Visuals/Starting angle": 180.0,
|
||||
@@ -201,6 +312,7 @@
|
||||
],
|
||||
"content": {
|
||||
"_type": "Camera Stream",
|
||||
"_source0": "network_table:///Robot/Driver Camera",
|
||||
"_title": "Camera",
|
||||
"Crosshair/Show crosshair": false,
|
||||
"Crosshair/Crosshair color": "#FFFFFFFF",
|
||||
@@ -226,6 +338,7 @@
|
||||
"_children": {
|
||||
"1,0": {
|
||||
"_type": "Boolean Box",
|
||||
"_source0": "network_table:///Robot/Values/Shooter Safety",
|
||||
"_title": "Shooter Safety",
|
||||
"Colors/Color when true": "#7CFC00FF",
|
||||
"Colors/Color when false": "#8B0000FF"
|
||||
@@ -237,30 +350,31 @@
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Number Bar",
|
||||
"_source0": "network_table:///Robot/Values/Drum",
|
||||
"_title": "Speed",
|
||||
"Range/Min": 0.0,
|
||||
"Range/Max": 100.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 0,
|
||||
"Visuals/Num tick marks": 6,
|
||||
"Visuals/Show text": true
|
||||
},
|
||||
{
|
||||
"_type": "Number Bar",
|
||||
"_type": "Number Slider",
|
||||
"_source0": "network_table:///Robot/Values/Angle",
|
||||
"_title": "Angle",
|
||||
"Range/Min": 0.0,
|
||||
"Range/Max": 100.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 0,
|
||||
"Visuals/Show text": true
|
||||
"Slider Settings/Min": -180.0,
|
||||
"Slider Settings/Max": 180.0,
|
||||
"Slider Settings/Block increment": 1.0,
|
||||
"Visuals/Display value": true
|
||||
},
|
||||
{
|
||||
"_type": "Number Bar",
|
||||
"_type": "Number Slider",
|
||||
"_source0": "network_table:///Robot/Values/Hood",
|
||||
"_title": "Hood",
|
||||
"Range/Min": 0.0,
|
||||
"Range/Max": 100.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 0,
|
||||
"Visuals/Show text": true
|
||||
"Slider Settings/Min": 0.0,
|
||||
"Slider Settings/Max": 120.0,
|
||||
"Slider Settings/Block increment": 1.0,
|
||||
"Visuals/Display value": true
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -288,29 +402,32 @@
|
||||
"_children": {
|
||||
"0,0": {
|
||||
"_type": "Number Bar",
|
||||
"_source0": "network_table:///Robot/Values/Intake",
|
||||
"_title": "Intake",
|
||||
"Range/Min": -1.0,
|
||||
"Range/Max": 1.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 5,
|
||||
"Visuals/Num tick marks": 3,
|
||||
"Visuals/Show text": true
|
||||
},
|
||||
"1,0": {
|
||||
"_type": "Number Bar",
|
||||
"_source0": "network_table:///Robot/Values/Serializer",
|
||||
"_title": "Serializer",
|
||||
"Range/Min": -1.0,
|
||||
"Range/Max": 1.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 5,
|
||||
"Visuals/Num tick marks": 3,
|
||||
"Visuals/Show text": true
|
||||
},
|
||||
"2,0": {
|
||||
"_type": "Number Bar",
|
||||
"_source0": "network_table:///Robot/Values/Storage",
|
||||
"_title": "Storage",
|
||||
"Range/Min": -1.0,
|
||||
"Range/Max": 1.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 5,
|
||||
"Visuals/Num tick marks": 3,
|
||||
"Visuals/Show text": true
|
||||
}
|
||||
}
|
||||
@@ -324,6 +441,7 @@
|
||||
"_children": {
|
||||
"0,0": {
|
||||
"_type": "Split Button Chooser",
|
||||
"_source0": "network_table:///Robot/Extender",
|
||||
"_title": "Extender"
|
||||
}
|
||||
}
|
||||
@@ -342,16 +460,17 @@
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Number Bar",
|
||||
"_type": "Number Slider",
|
||||
"_source0": "network_table:///Robot/Values/Claws",
|
||||
"_title": "Claws",
|
||||
"Range/Min": -1.0,
|
||||
"Range/Max": 1.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 5,
|
||||
"Visuals/Show text": true
|
||||
"Slider Settings/Min": 0.0,
|
||||
"Slider Settings/Max": 1.0,
|
||||
"Slider Settings/Block increment": 0.0625,
|
||||
"Visuals/Display value": true
|
||||
},
|
||||
{
|
||||
"_type": "Number Bar",
|
||||
"_source0": "network_table:///Robot/Values/Arm",
|
||||
"_title": "Arm",
|
||||
"Range/Min": -1.0,
|
||||
"Range/Max": 1.0,
|
||||
@@ -369,6 +488,7 @@
|
||||
],
|
||||
"content": {
|
||||
"_type": "Camera Stream",
|
||||
"_source0": "network_table:///photonvision/photoncamera",
|
||||
"_title": "Limelight",
|
||||
"Crosshair/Show crosshair": true,
|
||||
"Crosshair/Crosshair color": "#FFFFFFFF",
|
||||
@@ -379,6 +499,48 @@
|
||||
"imageWidth": -1,
|
||||
"imageHeight": -1
|
||||
}
|
||||
},
|
||||
"0,0": {
|
||||
"size": [
|
||||
6,
|
||||
4
|
||||
],
|
||||
"content": {
|
||||
"_type": "List Layout",
|
||||
"_title": "Match",
|
||||
"Layout/Label position": "BOTTOM",
|
||||
"_children": [
|
||||
{
|
||||
"_type": "Basic FMS Info",
|
||||
"_source0": "network_table:///FMSInfo",
|
||||
"_title": "Basic FMS Info"
|
||||
},
|
||||
{
|
||||
"_type": "Number Bar",
|
||||
"_source0": "network_table:///Robot/Values/Match Time",
|
||||
"_title": "Match Time",
|
||||
"Range/Min": 0.0,
|
||||
"Range/Max": 30.0,
|
||||
"Range/Center": 0.0,
|
||||
"Visuals/Num tick marks": 4,
|
||||
"Visuals/Show text": true
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"0,4": {
|
||||
"size": [
|
||||
6,
|
||||
4
|
||||
],
|
||||
"content": {
|
||||
"_type": "Field",
|
||||
"_source0": "network_table:///Robot/Field",
|
||||
"_title": "Field",
|
||||
"Game/Game": "A2021_Infinite_Recharge",
|
||||
"Visuals/Robot Icon Size": 50.0,
|
||||
"Visuals/Show Outside Circles": false
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,17 +1,17 @@
|
||||
Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) ,Turret Offset (deg)
|
||||
0 ,-29.5 ,8000 ,10
|
||||
78.5 ,-29.5 ,8500 ,8.5
|
||||
88 ,-34.2 ,8600 ,8.5
|
||||
90 ,-35.47 ,9500 ,8.5
|
||||
99 ,-39.62 ,9500 ,8.0
|
||||
111 ,-42 ,9500 ,8.0
|
||||
127.25 ,-49.12 ,9500 ,8.0
|
||||
141 ,-59.4 ,9900 ,7.5
|
||||
150 ,-66.22 ,10000 ,7.5
|
||||
164.5 ,-75.52 ,10000 ,6.5
|
||||
189.9 ,-81.39 ,11000 ,6.0
|
||||
207 ,-104.07 ,11000 ,5.5
|
||||
227 ,-105.32 ,11500 ,5.0
|
||||
239 ,-105.5 ,12380 ,4.5
|
||||
255.5 ,-105.8 ,13500 ,4.0
|
||||
999 ,-105.8 ,13500 ,3.0
|
||||
Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds)
|
||||
0 ,-29.5 ,8000
|
||||
78.5 ,-29.5 ,8500
|
||||
88 ,-34.2 ,8600
|
||||
90 ,-35.47 ,9500
|
||||
99 ,-39.62 ,9500
|
||||
111 ,-42 ,9500
|
||||
127.25 ,-49.12 ,9500
|
||||
141 ,-59.4 ,9900
|
||||
150 ,-66.22 ,10000
|
||||
164.5 ,-75.52 ,10000
|
||||
189.9 ,-81.39 ,11000
|
||||
207 ,-104.07 ,11000
|
||||
227 ,-105.32 ,11500
|
||||
239 ,-105.5 ,12380
|
||||
255.5 ,-105.8 ,13500
|
||||
999 ,-105.8 ,13500
|
||||
|
||||
|
@@ -67,6 +67,7 @@ public class Robot extends TimedRobot {
|
||||
// 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.
|
||||
m_robotContainer.updateValues();
|
||||
CommandScheduler.getInstance().run();
|
||||
}
|
||||
|
||||
|
||||
@@ -4,37 +4,42 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.Objects;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
||||
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.networktables.NTSendable;
|
||||
import edu.wpi.first.networktables.NTSendableBuilder;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.networktables.NetworkTableValue;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.Preferences;
|
||||
import edu.wpi.first.wpilibj.PowerDistribution;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandGroupBase;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.AutoConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.PathPlannerCommand;
|
||||
import frc4388.robot.commands.PathRecorder;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.RunExtender;
|
||||
import frc4388.robot.commands.shooter.TimedWaitUntilCommand;
|
||||
import frc4388.robot.commands.shooter.TrackTarget;
|
||||
import frc4388.robot.commands.shuffleboard.CommandSchedule;
|
||||
@@ -54,6 +59,7 @@ import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.utility.shuffleboard.CachingSendableChooser;
|
||||
|
||||
//TODO: Try using ConditionalCommand for subsystem default commands.
|
||||
//TODO: Replace Path Recorder with Auto Chooser.
|
||||
@@ -88,8 +94,8 @@ public class RobotContainer {
|
||||
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
||||
|
||||
/* Dashboard Tools */
|
||||
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
|
||||
private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
|
||||
private final CachingSendableChooser<Command> m_autoChooser = new CachingSendableChooser<>();
|
||||
private final PathRecorder m_pathRecorder = new PathRecorder(m_robotSwerveDrive, m_autoChooser);
|
||||
private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom);
|
||||
private final CommandSchedule m_commandSchedule = new CommandSchedule(10, 5, false);
|
||||
|
||||
@@ -114,16 +120,115 @@ public class RobotContainer {
|
||||
|
||||
private DriveMode currentDriveMode = DriveMode.ON;
|
||||
|
||||
|
||||
|
||||
private final Map<String, Sendable> tablesToData = new HashMap<>();
|
||||
private final NetworkTable networkTable = NetworkTableInstance.getDefault().getTable("Robot");
|
||||
|
||||
private synchronized void putData(String key, Sendable data) {
|
||||
Sendable sddata = tablesToData.get(key);
|
||||
if (sddata == null || sddata != data) {
|
||||
tablesToData.put(key, data);
|
||||
NetworkTable dataTable = networkTable.getSubTable(key);
|
||||
SendableBuilderImpl builder = new SendableBuilderImpl();
|
||||
builder.setTable(dataTable);
|
||||
SendableRegistry.publish(data, builder);
|
||||
builder.startListeners();
|
||||
dataTable.getEntry(".name").setString(key);
|
||||
}
|
||||
}
|
||||
|
||||
public synchronized void updateValues() {
|
||||
for (Sendable data : tablesToData.values()) {
|
||||
SendableRegistry.update(data);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
autoChooser.addOption("threeBallPlus", threeBallPlus);
|
||||
m_autoChooser.addOption("OneBall", this::makeOneBallCommand);
|
||||
m_autoChooser.addOption("TwoBall", this::makeTwoBallCommand);
|
||||
m_autoChooser.setDefaultOption("ThreeBall", this::makeThreeBallCommand);
|
||||
|
||||
SmartDashboard.putData("AutoChooser", autoChooser);
|
||||
SmartDashboard.putData("Autonomous", m_autoChooser);
|
||||
SmartDashboard.putData("Path Recorder", m_pathRecorder);
|
||||
|
||||
Preferences.initString("Autonomous", "Three Ball");
|
||||
// Preferences.initString("Autonomous", "Three Ball");
|
||||
|
||||
|
||||
|
||||
var pdp = new PowerDistribution() {
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
super.initSendable(builder);
|
||||
builder.setSmartDashboardType("PowerDistributionPanel");
|
||||
}
|
||||
};
|
||||
putData("Values", builder -> {
|
||||
builder.addDoubleProperty("Match Time", DriverStation::getMatchTime, null);
|
||||
builder.addDoubleProperty("Voltage", pdp::getVoltage, null);
|
||||
builder.addDoubleProperty("Claws", m_robotClaws.m_rightClaw::get, null);
|
||||
builder.addDoubleProperty("Arm", m_robotMap.elbow::get, null);
|
||||
builder.addDoubleProperty("Intake", m_robotMap.intakeMotor::get, null);
|
||||
builder.addDoubleProperty("Serializer", m_robotMap.serializerBelt::get, null);
|
||||
builder.addDoubleProperty("Storage", m_robotMap.storageMotor::get, null);
|
||||
builder.addDoubleProperty("Drum", m_robotMap.shooterFalconRight::get, null);
|
||||
builder.addDoubleProperty("Angle", m_robotMap.shooterTurret::get, null);
|
||||
builder.addDoubleProperty("Hood", m_robotMap.angleAdjusterMotor::get, null);
|
||||
builder.addBooleanProperty("Shooter Safety", this::isLockedOn, null);
|
||||
});
|
||||
putData("Field", m_robotSwerveDrive.m_field);
|
||||
putData("PDP", pdp);
|
||||
putData("Extender", new NTSendable() {
|
||||
@Override
|
||||
public void initSendable(NTSendableBuilder builder) {
|
||||
builder.setSmartDashboardType("String Chooser");
|
||||
builder.getEntry(".instance").setDouble(0);
|
||||
builder.addStringProperty("default", () -> "Retracted", null);
|
||||
builder.addStringArrayProperty("options", () -> new String[] {"Retracted", "Extended"}, null);
|
||||
builder.addStringProperty("active", () -> m_robotExtender.getPosition() <= 0 ? "Retracted" : "Extended", null);
|
||||
builder.addStringProperty("selected", null, null);
|
||||
}
|
||||
});
|
||||
putData("Drivebase", m_robotSwerveDrive);
|
||||
putData("Gyro", m_robotSwerveDrive.m_gyro);
|
||||
putData("Drive Speed", new NTSendable() {
|
||||
@Override
|
||||
public void initSendable(NTSendableBuilder builder) {
|
||||
builder.setSmartDashboardType("String Chooser");
|
||||
builder.getEntry(".instance").setDouble(0);
|
||||
builder.addStringProperty("default", () -> "Low", null);
|
||||
builder.addStringArrayProperty("options", () -> new String[] {"Low", "High"}, null);
|
||||
builder.addStringProperty("active", () -> m_robotSwerveDrive.speedAdjust == SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW ? "Low" : "High", null);
|
||||
builder.addStringProperty("selected", null, val -> m_robotSwerveDrive.speedAdjust = val.equals("Low") ? SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW : SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST);
|
||||
}
|
||||
});
|
||||
putData("Accelerometer", new NTSendable() {
|
||||
@Override
|
||||
public void initSendable(NTSendableBuilder builder) {
|
||||
builder.setSmartDashboardType("3AxisAccelerometer");
|
||||
NetworkTableEntry entryX = builder.getEntry("X");
|
||||
NetworkTableEntry entryY = builder.getEntry("Y");
|
||||
NetworkTableEntry entryZ = builder.getEntry("Z");
|
||||
builder.setUpdateTable(() -> {
|
||||
short[] data = new short[3];
|
||||
m_robotSwerveDrive.m_gyro.getBiasedAccelerometer(data);
|
||||
entryX.setDouble(data[0]);
|
||||
entryY.setDouble(data[1]);
|
||||
entryZ.setDouble(data[2]);
|
||||
});
|
||||
}
|
||||
});
|
||||
putData("Drive Camera", m_robotCamera);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
configureButtonBindings();
|
||||
/* Default Commands */
|
||||
// Swerve Drive with Input
|
||||
@@ -328,7 +433,7 @@ public class RobotContainer {
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kY.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
}
|
||||
|
||||
private boolean isLockedOn() {
|
||||
public boolean isLockedOn() {
|
||||
return m_robotTurret.isLockedOn() || m_robotHood.isLockedOn() || m_robotBoomBoom.isLockedOn();
|
||||
}
|
||||
|
||||
@@ -338,30 +443,63 @@ public class RobotContainer {
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
return autoChooser.getSelected();
|
||||
return m_autoChooser.getSelected();
|
||||
}
|
||||
|
||||
private final CommandGroupBase threeBallPlus = CommandGroupBase.sequence(
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"),
|
||||
// new RunExtender(m_robotExtender).withName("DeployExtender"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"),
|
||||
// Get Second Ball
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"),
|
||||
makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove1").withName("JMove1"),
|
||||
// Shoot Preloaded and Second Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"),
|
||||
// Get Third Ball
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove2").withName("JMove2"),
|
||||
// Shoot Third Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "ThirdBall"),
|
||||
// Stop
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StopRunningIntake"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed")
|
||||
);
|
||||
private Command makeOneBallCommand() {
|
||||
return CommandGroupBase.sequence(
|
||||
makeStartupCommandPart(),
|
||||
// Shoot Preloaded Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "FirstBall"),
|
||||
makeStopCommandPart()
|
||||
).withName("OneBall");
|
||||
}
|
||||
|
||||
private Command makeTwoBallCommand() {
|
||||
return CommandGroupBase.sequence(
|
||||
makeStartupCommandPart(),
|
||||
// Get Second Ball
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"),
|
||||
new PathPlannerCommand("JMove1", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_robotSwerveDrive).withName("JMove1"),
|
||||
// Shoot Preloaded and Second Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"),
|
||||
makeStopCommandPart()
|
||||
).withName("TwoBall");
|
||||
}
|
||||
|
||||
private Command makeThreeBallCommand() {
|
||||
return CommandGroupBase.sequence(
|
||||
makeStartupCommandPart(),
|
||||
// Get Second Ball
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"),
|
||||
new PathPlannerCommand("JMove1", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_robotSwerveDrive).withName("JMove1"),
|
||||
// Shoot Preloaded and Second Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"),
|
||||
// Get Third Ball
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
new PathPlannerCommand("JMove2", AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, m_robotSwerveDrive).withName("JMove2"),
|
||||
// Shoot Third Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "ThirdBall"),
|
||||
makeStopCommandPart()
|
||||
).withName("ThreeBall");
|
||||
}
|
||||
|
||||
private Command makeStartupCommandPart() {
|
||||
return CommandGroupBase.sequence(
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake")
|
||||
// new RunExtender(m_robotExtender).withName("DeployExtender")
|
||||
);
|
||||
}
|
||||
|
||||
private Command makeStopCommandPart() {
|
||||
return CommandGroupBase.sequence(
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StopRunningIntake"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("StopRunningStorage")
|
||||
);
|
||||
}
|
||||
|
||||
private ParallelDeadlineGroup makeTimeoutTrackShotGroup(double lockOnDuration, double lockOnTimeAllowance, double storageRunTime, String name) {
|
||||
return CommandGroupBase.sequence(
|
||||
@@ -372,30 +510,6 @@ public class RobotContainer {
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (paths) or commands you want to run (in order)
|
||||
* @return array of commands
|
||||
*/
|
||||
private SequentialCommandGroup makePathingGroup(Double maxVel, Double maxAccel, String input) {
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
PathPlannerTrajectory pathTrajectory = PathPlanner.loadPath(input, Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY), Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION));
|
||||
PathPlannerState initialState = pathTrajectory.getInitialState();
|
||||
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
|
||||
|
||||
return new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition), m_robotSwerveDrive),
|
||||
new PPSwerveControllerCommand(pathTrajectory, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
|
||||
new InstantCommand(m_robotSwerveDrive::stopModules, m_robotSwerveDrive)
|
||||
);
|
||||
}
|
||||
|
||||
public void switchControlMode() {
|
||||
currentControlMode = currentControlMode == ControlMode.SHOOTER ? ControlMode.CLIMBER : ControlMode.SHOOTER;
|
||||
}
|
||||
|
||||
@@ -255,14 +255,14 @@ public class RobotMap {
|
||||
}
|
||||
|
||||
private void registerDevices() {
|
||||
SendableRegistry.setName(frontLeftSteerMotor, "SwerveDrive", "Left Front Steer Motor");
|
||||
SendableRegistry.setName(frontLeftDriveMotor, "SwerveDrive", "Left Front Drive Motor");
|
||||
SendableRegistry.setName(frontRightSteerMotor, "SwerveDrive", "Right Front Steer Motor");
|
||||
SendableRegistry.setName(frontRightDriveMotor, "SwerveDrive", "Right Front Drive Motor");
|
||||
SendableRegistry.setName(backLeftSteerMotor, "SwerveDrive", "Left Back Steer Motor");
|
||||
SendableRegistry.setName(backLeftDriveMotor, "SwerveDrive", "Left Back Drive Motor");
|
||||
SendableRegistry.setName(backRightSteerMotor, "SwerveDrive", "Right Back Steer Motor");
|
||||
SendableRegistry.setName(backRightDriveMotor, "SwerveDrive", "Right Back Drive Motor");
|
||||
SendableRegistry.setName(frontLeftSteerMotor, "Drive", "Left Front Steer Motor");
|
||||
SendableRegistry.setName(frontLeftDriveMotor, "Drive", "Left Front Drive Motor");
|
||||
SendableRegistry.setName(frontRightSteerMotor, "Drive", "Right Front Steer Motor");
|
||||
SendableRegistry.setName(frontRightDriveMotor, "Drive", "Right Front Drive Motor");
|
||||
SendableRegistry.setName(backLeftSteerMotor, "Drive", "Left Back Steer Motor");
|
||||
SendableRegistry.setName(backLeftDriveMotor, "Drive", "Left Back Drive Motor");
|
||||
SendableRegistry.setName(backRightSteerMotor, "Drive", "Right Back Steer Motor");
|
||||
SendableRegistry.setName(backRightDriveMotor, "Drive", "Right Back Drive Motor");
|
||||
|
||||
SendableRegistry.setName(intakeMotor, "Intake", "Intake Motor");
|
||||
SendableRegistry.setName((SendableCANSparkMax) extenderMotor, "Intake", "Extender Motor");
|
||||
|
||||
@@ -0,0 +1,60 @@
|
||||
package frc4388.robot.commands;
|
||||
|
||||
import java.util.function.Function;
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
import java.util.regex.Matcher;
|
||||
import java.util.regex.Pattern;
|
||||
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
||||
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.PrintCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class PathPlannerCommand extends SequentialCommandGroup {
|
||||
private static final Logger LOGGER = Logger.getLogger(PathPlannerCommand.class.getSimpleName());
|
||||
/** Function that removes the ".path" from the end of a string. */
|
||||
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern.compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
|
||||
private final SwerveDrive m_swerveDrive;
|
||||
public PathPlannerCommand(String pathFileName, SwerveDrive swerveDrive) {
|
||||
this(pathFileName, SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION, swerveDrive);
|
||||
}
|
||||
public PathPlannerCommand(String pathName, double maxVel, double maxAccel, SwerveDrive swerveDrive) {
|
||||
m_swerveDrive = swerveDrive;
|
||||
addRequirements(swerveDrive);
|
||||
String pathFileName = PATH_EXTENSION_REMOVER.apply(pathName);
|
||||
setName(pathFileName);
|
||||
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
LOGGER.log(Level.WARNING, "Loading path {0}.", pathFileName);
|
||||
PathPlannerTrajectory pathTrajectory = PathPlanner.loadPath(pathFileName, maxVel, maxAccel);
|
||||
if (pathTrajectory != null) {
|
||||
LOGGER.info("Done loading.");
|
||||
m_swerveDrive.m_field.getObject("traj").setTrajectory(pathTrajectory);
|
||||
PathPlannerState initialState = pathTrajectory.getInitialState();
|
||||
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
|
||||
|
||||
addCommands(
|
||||
new InstantCommand(() -> m_swerveDrive.resetOdometry(initialPosition), m_swerveDrive),
|
||||
new PPSwerveControllerCommand(pathTrajectory, m_swerveDrive::getOdometry, m_swerveDrive.m_kinematics, xController, yController, thetaController, m_swerveDrive::setModuleStates, m_swerveDrive),
|
||||
new InstantCommand(m_swerveDrive::stopModules, m_swerveDrive)
|
||||
);
|
||||
} else {
|
||||
LOGGER.log(Level.SEVERE, "Failed loading path {0}.", pathFileName);
|
||||
addCommands(new PrintCommand("Path failed to load."));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -18,16 +18,11 @@ import java.util.Comparator;
|
||||
import java.util.List;
|
||||
import java.util.Objects;
|
||||
import java.util.Optional;
|
||||
import java.util.function.Function;
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
import java.util.regex.Matcher;
|
||||
import java.util.regex.Pattern;
|
||||
import java.util.stream.Collectors;
|
||||
|
||||
import com.diffplug.common.base.Errors;
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
|
||||
import edu.wpi.first.math.Pair;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
@@ -36,7 +31,7 @@ import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandBase;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.NotifierCommand;
|
||||
@@ -44,24 +39,22 @@ import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.PathPlannerUtil;
|
||||
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
|
||||
import frc4388.utility.shuffleboard.ListeningSendableChooser;
|
||||
|
||||
import frc4388.utility.shuffleboard.CachingSendableChooser;
|
||||
// TODO: Fix naming inconsistency with fields.
|
||||
public class PathRecorder extends CommandBase {
|
||||
private static final Logger LOGGER = Logger.getLogger(PathRecorder.class.getSimpleName());
|
||||
private static double PATH_POLLING_PERIOD = 0.5;
|
||||
private static final double PATH_POLLING_PERIOD = 0.5;
|
||||
private static final Clock SYSTEM_CLOCK = Clock.system(ZoneId.systemDefault());
|
||||
private static final Path PATHPLANNER_DIRECTORY = Filesystem.getDeployDirectory().toPath().resolve("pathplanner");
|
||||
private static final String PATHPLANNER_EXTENSION = ".path";
|
||||
private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'Recording' yyyy-MM-dd HH-mm-ss.SSS'.path'");
|
||||
// Function that removes the ".path" from the end of a string.
|
||||
private static final Function<CharSequence, String> PATH_EXTENSION_REMOVER = ((Function<CharSequence, Matcher>) Pattern.compile(PATHPLANNER_EXTENSION)::matcher).andThen(m -> m.replaceFirst(""));
|
||||
private static final String RECORDING_PREFIX = "Recording";
|
||||
private static final DateTimeFormatter RECORDING_FILE_NAME_FORMATTER = DateTimeFormatter.ofPattern("'" + RECORDING_PREFIX + "' yyyy-MM-dd HH-mm-ss.SSS'.path'");
|
||||
|
||||
private PathPlannerTrajectory loadedPathTrajectory = null;
|
||||
private WatchKey m_watchKey;
|
||||
private final ListeningSendableChooser<File> pathChooser = new ListeningSendableChooser<>(this::loadPath);
|
||||
private final CachingSendableChooser<Command> m_autoChooser;
|
||||
private final List<Waypoint> pathPoints = new ArrayList<>();
|
||||
private final NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
|
||||
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable("Recording");
|
||||
private final NetworkTable recordingNetworkTable = networkTableInstance.getTable(RECORDING_PREFIX);
|
||||
private final SwerveDrive m_swerveDrive;
|
||||
private final CommandBase m_pathWatcher = new NotifierCommand(this::updatePathChooser, PATH_POLLING_PERIOD) {
|
||||
@Override
|
||||
@@ -76,8 +69,9 @@ public class PathRecorder extends CommandBase {
|
||||
}
|
||||
}.withName("Save Recording");
|
||||
|
||||
public PathRecorder(SwerveDrive swerveDrive) {
|
||||
public PathRecorder(SwerveDrive swerveDrive, CachingSendableChooser<Command> autoChooser) {
|
||||
m_swerveDrive = swerveDrive;
|
||||
m_autoChooser = autoChooser;
|
||||
addRequirements(swerveDrive);
|
||||
setName("Record Path (Cancel to Save)");
|
||||
}
|
||||
@@ -85,7 +79,7 @@ public class PathRecorder extends CommandBase {
|
||||
/**
|
||||
* Creates a WatchKey for the path planner directory and registers it with the WatchService. Then
|
||||
* creates a NotifierCommand that will update the path chooser with the latest path files. Finally,
|
||||
* adds the existing path files to the path chooser
|
||||
* adds the existing recorded path files to the path chooser
|
||||
*/
|
||||
@Override
|
||||
public void initialize() {
|
||||
@@ -95,8 +89,7 @@ public class PathRecorder extends CommandBase {
|
||||
} catch (IOException exception) {
|
||||
LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
|
||||
}
|
||||
Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()).filter(file -> file.getName().endsWith(PATHPLANNER_EXTENSION)).sorted(Comparator.comparingLong(File::lastModified)).forEachOrdered(file -> pathChooser.addOption(file.getName(), file));
|
||||
SmartDashboard.putData("Path Chooser", pathChooser);
|
||||
Arrays.stream(PATHPLANNER_DIRECTORY.toFile().listFiles()).filter(file -> file.getName().endsWith(PATHPLANNER_EXTENSION) && file.getName().startsWith(RECORDING_PREFIX)).sorted(Comparator.comparingLong(File::lastModified)).forEachOrdered(file -> m_autoChooser.addOption(file.getName(), () -> new PathPlannerCommand(file.getName(), m_swerveDrive)));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -107,7 +100,7 @@ public class PathRecorder extends CommandBase {
|
||||
public void execute() {
|
||||
Translation2d position = m_swerveDrive.m_odometry.getPoseMeters().getTranslation();
|
||||
Rotation2d rotation = m_swerveDrive.m_gyro.getRotation2d();
|
||||
// FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity.
|
||||
// FIXME: Chassis speeds are created from joystick inputs and may not reflect actual robot velocity.
|
||||
Translation2d velocity = new Translation2d(m_swerveDrive.getChassisSpeeds().vxMetersPerSecond, m_swerveDrive.getChassisSpeeds().vyMetersPerSecond);
|
||||
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, SwerveDriveConstants.PATH_RECORD_VELOCITY ? velocity.getNorm() : null, false);
|
||||
pathPoints.add(waypoint);
|
||||
@@ -125,10 +118,6 @@ public class PathRecorder extends CommandBase {
|
||||
m_pathSaver.schedule();
|
||||
}
|
||||
|
||||
public PathPlannerTrajectory getPath() {
|
||||
return loadedPathTrajectory;
|
||||
}
|
||||
|
||||
/**
|
||||
* Called when a file is created, modified, or deleted. Adds newly created .path files to the
|
||||
* SendableChooser. Reloads the path if the currently selected file is modified.
|
||||
@@ -145,12 +134,12 @@ public class PathRecorder extends CommandBase {
|
||||
if (watchEventFileName.endsWith(PATHPLANNER_EXTENSION)) {
|
||||
if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_CREATE)) {
|
||||
LOGGER.log(Level.WARNING, "PathPlanner file {0} created. Options added to SendableChooser.", watchEventFileName);
|
||||
pathChooser.addOption(watchEventFile.getName(), watchEventFile);
|
||||
m_autoChooser.addOption(watchEventFile.getName(), () -> new PathPlannerCommand(watchEventFile.getName(), m_swerveDrive));
|
||||
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_MODIFY)) {
|
||||
LOGGER.log(Level.WARNING, "PathPlanner file {0} modified.", watchEventFileName);
|
||||
if (watchEventFileName.equals(pathChooser.getSelected().getName())) {
|
||||
if (watchEventFileName.equals(m_autoChooser.getSelected().getName())) {
|
||||
LOGGER.log(Level.SEVERE, "PathPlanner file {0} already selected. Reloading path.", watchEventFileName);
|
||||
loadPath(watchEventFileName);
|
||||
m_autoChooser.reloadSelected();
|
||||
}
|
||||
} else if (pathWatchEvent.kind().equals(StandardWatchEventKinds.ENTRY_DELETE)) {
|
||||
LOGGER.log(Level.SEVERE, "PathPlanner file {0} deleted. Removing options from SendableChooser not yet implemented.", watchEventFileName);
|
||||
@@ -162,20 +151,6 @@ public class PathRecorder extends CommandBase {
|
||||
}
|
||||
}
|
||||
|
||||
private void loadPath(String pathName) {
|
||||
LOGGER.warning("Unloading path.");
|
||||
loadedPathTrajectory = null;
|
||||
LOGGER.info("Done unloading.");
|
||||
if (pathName != null) {
|
||||
LOGGER.log(Level.WARNING, "Loading path {0}.", pathName);
|
||||
loadedPathTrajectory = PathPlanner.loadPath(PATH_EXTENSION_REMOVER.apply(pathName), SwerveDriveConstants.PATH_MAX_VELOCITY, SwerveDriveConstants.PATH_MAX_ACCELERATION);
|
||||
m_swerveDrive.m_field.getObject("traj").setTrajectory(loadedPathTrajectory);
|
||||
LOGGER.info("Done loading.");
|
||||
} else {
|
||||
LOGGER.severe("No path to load.");
|
||||
}
|
||||
}
|
||||
|
||||
private void savePath() {
|
||||
// INFO: Had to chown the pathplanner folder in order to save paths.
|
||||
File outputFile = PATHPLANNER_DIRECTORY.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
|
||||
@@ -217,7 +192,6 @@ public class PathRecorder extends CommandBase {
|
||||
StringWriter writer = new StringWriter();
|
||||
path.write(writer);
|
||||
recordingNetworkTable.getEntry(outputFile.getName()).setString(writer.toString());
|
||||
pathChooser.setDefaultOption(outputFile.getName(), outputFile);
|
||||
LOGGER.log(Level.INFO, "Recorded path to {0}.", outputFile.getPath());
|
||||
} else LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
|
||||
}
|
||||
|
||||
@@ -61,7 +61,6 @@ public class ShooterTuner extends CommandBase {
|
||||
sbi.setTable(NetworkTableInstance.getDefault().getTable("ShooterTuner/Manual Shooter Data"));
|
||||
sbi.addDoubleProperty("Drum Velocity", () -> tableOverrideEntry.drumVelocity, d -> tableOverrideEntry.drumVelocity = d);
|
||||
sbi.addDoubleProperty("Hood Extension", () -> tableOverrideEntry.hoodExt, d -> tableOverrideEntry.hoodExt = d);
|
||||
sbi.addDoubleProperty("Turret Offset", () -> tableOverrideEntry.turretOffset, d -> tableOverrideEntry.turretOffset = d);
|
||||
sbi.addDoubleProperty("Distance", () -> tableOverrideEntry.distance, d -> tableOverrideEntry.distance = d);
|
||||
sbi.addBooleanProperty("Measure Distance", () -> measureDistance, b -> measureDistance = b);
|
||||
// manual.add("Manual Shooter Data", m_shotEditor);
|
||||
@@ -79,7 +78,6 @@ public class ShooterTuner extends CommandBase {
|
||||
tableOverrideEntry.distance = 0.0;
|
||||
tableOverrideEntry.hoodExt = 0.0;
|
||||
tableOverrideEntry.drumVelocity = 0.0;
|
||||
tableOverrideEntry.turretOffset = 0.0;
|
||||
m_boomBoom.setShooterTable(new ShooterTableEntry[] { tableOverrideEntry });
|
||||
Shuffleboard.selectTab("Shooter Tuner");
|
||||
SmartDashboard.putData("Shooter Table", m_tableEditor);
|
||||
@@ -116,7 +114,6 @@ public class ShooterTuner extends CommandBase {
|
||||
builder.setSmartDashboardType("RobotPreferences");
|
||||
builder.addDoubleProperty("Drum Velocity", () -> tableOverrideEntry.drumVelocity, d -> tableOverrideEntry.drumVelocity = d);
|
||||
builder.addDoubleProperty("Hood Extension", () -> tableOverrideEntry.hoodExt, d -> tableOverrideEntry.hoodExt = d);
|
||||
builder.addDoubleProperty("Turret Offset", () -> tableOverrideEntry.turretOffset, d -> tableOverrideEntry.turretOffset = d);
|
||||
builder.addDoubleProperty("Distance", () -> tableOverrideEntry.distance, d -> tableOverrideEntry.distance = d);
|
||||
builder.addBooleanProperty("Measure Distance", () -> measureDistance, b -> measureDistance = b);
|
||||
}
|
||||
|
||||
@@ -55,7 +55,7 @@ public class BoomBoom extends SubsystemBase {
|
||||
// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later
|
||||
|
||||
public static class ShooterTableEntry {
|
||||
public Double distance, hoodExt, drumVelocity, turretOffset;
|
||||
public Double distance, hoodExt, drumVelocity;
|
||||
}
|
||||
|
||||
private ShooterTableEntry[] m_shooterTable;
|
||||
@@ -101,17 +101,6 @@ public class BoomBoom extends SubsystemBase {
|
||||
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
|
||||
}
|
||||
|
||||
/**
|
||||
* This is a function that takes a value (distance) and returns a value (turretOffset) that is a linear
|
||||
* interpolation of the two values (turretOffset) at the two closest points in the table (m_shooterTable)
|
||||
* to the given value (distance).
|
||||
* @param distance Distance in shooter table
|
||||
* @return Turret offset in degrees
|
||||
*/
|
||||
public Double getOffset(final Double distance) {
|
||||
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.turretOffset).doubleValue();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// This method will be called once per scheduler run
|
||||
@@ -191,7 +180,6 @@ public class BoomBoom extends SubsystemBase {
|
||||
dummyEntry.distance = 0.0;
|
||||
dummyEntry.hoodExt = 0.0;
|
||||
dummyEntry.drumVelocity = 0.0;
|
||||
dummyEntry.turretOffset = 0.0;
|
||||
LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
|
||||
m_shooterTable = new ShooterTableEntry[] { dummyEntry };
|
||||
}
|
||||
|
||||
@@ -57,10 +57,6 @@ public class SwerveDrive extends SubsystemBase {
|
||||
public final Field2d m_field = new Field2d();
|
||||
|
||||
public SwerveDrive(SwerveModule frontLeft, SwerveModule frontRight, SwerveModule backLeft, SwerveModule backRight, WPI_Pigeon2 gyro) {
|
||||
//XXX:
|
||||
setName("Swerve Drive");
|
||||
addChild("Gyro", m_gyro);
|
||||
|
||||
m_frontLeft = frontLeft;
|
||||
m_frontRight = frontRight;
|
||||
m_backLeft = backLeft;
|
||||
|
||||
@@ -99,7 +99,7 @@ public class AnsiLogging {
|
||||
ZonedDateTime time = ZonedDateTime.ofInstant(logRecord.getInstant(), ZONE_ID);
|
||||
// Get the logger name, source class name, and/or source method name.
|
||||
String source = Optional.ofNullable(logRecord.getLoggerName()).or(() -> Optional.ofNullable(logRecord.getSourceClassName())).map(s -> s + " ").orElse("") + Optional.ofNullable(logRecord.getSourceMethodName()).orElse("");
|
||||
String message = logRecord.getMessage();
|
||||
String message = formatMessage(logRecord);
|
||||
// Get the stack trace of the exception if it was thrown.
|
||||
String throwable = Optional.ofNullable(logRecord.getThrown()).map(LoggingAnsiFormatter::makeStackTraceString).orElse("");
|
||||
// Select the appropriate format string for the log level.
|
||||
|
||||
@@ -0,0 +1,170 @@
|
||||
// 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 edu.wpi.first.wpilibj.smartdashboard;
|
||||
package frc4388.utility.shuffleboard;
|
||||
|
||||
import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
|
||||
|
||||
import edu.wpi.first.networktables.NTSendable;
|
||||
import edu.wpi.first.networktables.NTSendableBuilder;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
import java.util.ArrayList;
|
||||
import java.util.LinkedHashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
import java.util.concurrent.atomic.AtomicInteger;
|
||||
import java.util.concurrent.locks.ReentrantLock;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
/**
|
||||
* The {@link CachingSendableChooser} class is a useful tool for presenting a selection of options to the
|
||||
* {@link SmartDashboard}.
|
||||
*
|
||||
* <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
|
||||
* this by putting every possible Command you want to run as an autonomous into a {@link
|
||||
* CachingSendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
|
||||
* on the laptop. Once autonomous starts, simply ask the {@link CachingSendableChooser} what the selected
|
||||
* value is.
|
||||
*
|
||||
* @param <V> The type of the values to be stored
|
||||
*/
|
||||
public class CachingSendableChooser<V> implements NTSendable, AutoCloseable {
|
||||
/** The key for the default value. */
|
||||
private static final String DEFAULT = "default";
|
||||
/** The key for the selected option. */
|
||||
private static final String SELECTED = "selected";
|
||||
/** The key for the active option. */
|
||||
private static final String ACTIVE = "active";
|
||||
/** The key for the option array. */
|
||||
private static final String OPTIONS = "options";
|
||||
/** The key for the instance number. */
|
||||
private static final String INSTANCE = ".instance";
|
||||
/** A map linking strings to the objects the represent. */
|
||||
private final Map<String, Supplier<V>> m_map = new LinkedHashMap<>();
|
||||
|
||||
private String m_defaultChoice = "";
|
||||
private final int m_instance;
|
||||
private static final AtomicInteger s_instances = new AtomicInteger();
|
||||
|
||||
/** Instantiates a {@link ListeningSendableChooser}. */
|
||||
public CachingSendableChooser() {
|
||||
m_instance = s_instances.getAndIncrement();
|
||||
SendableRegistry.add(this, "SendableChooser", m_instance);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() {
|
||||
SendableRegistry.remove(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
|
||||
* object will appear as the given name.
|
||||
*
|
||||
* @param name the name of the option
|
||||
* @param object the option
|
||||
*/
|
||||
public void addOption(String name, Supplier<V> object) {
|
||||
m_map.put(name, object);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the given object to the list of options and marks it as the default. Functionally, this is
|
||||
* very close to {@link #addOption(String, Object)} except that it will use this as the default
|
||||
* option if none other is explicitly selected.
|
||||
*
|
||||
* @param name the name of the option
|
||||
* @param object the option
|
||||
*/
|
||||
public void setDefaultOption(String name, Supplier<V> object) {
|
||||
requireNonNullParam(name, "name", "setDefaultOption");
|
||||
|
||||
m_defaultChoice = name;
|
||||
addOption(name, object);
|
||||
m_mutex.lock();
|
||||
try {
|
||||
m_selectedValue = object.get();
|
||||
} finally {
|
||||
m_mutex.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the selected option. If there is none selected, it will return the default. If there is
|
||||
* none selected and no default, then it will return {@code null}.
|
||||
*
|
||||
* @return the option selected
|
||||
*/
|
||||
public V getSelected() {
|
||||
m_mutex.lock();
|
||||
try {
|
||||
return m_selectedValue;
|
||||
} finally {
|
||||
m_mutex.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Re-runs the value supplier for the current selection.
|
||||
*/
|
||||
public void reloadSelected() {
|
||||
m_mutex.lock();
|
||||
try {
|
||||
m_selectedValue = m_map.get(m_selected).get();
|
||||
} finally {
|
||||
m_mutex.unlock();
|
||||
}
|
||||
}
|
||||
|
||||
private String m_selected;
|
||||
private V m_selectedValue;
|
||||
private final List<NetworkTableEntry> m_activeEntries = new ArrayList<>();
|
||||
private final ReentrantLock m_mutex = new ReentrantLock();
|
||||
|
||||
@Override
|
||||
public void initSendable(NTSendableBuilder builder) {
|
||||
builder.setSmartDashboardType("String Chooser");
|
||||
builder.getEntry(INSTANCE).setDouble(m_instance);
|
||||
builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
|
||||
builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
|
||||
builder.addStringProperty(
|
||||
ACTIVE,
|
||||
() -> {
|
||||
m_mutex.lock();
|
||||
try {
|
||||
if (m_selected != null) {
|
||||
return m_selected;
|
||||
} else {
|
||||
return m_defaultChoice;
|
||||
}
|
||||
} finally {
|
||||
m_mutex.unlock();
|
||||
}
|
||||
},
|
||||
null);
|
||||
m_mutex.lock();
|
||||
try {
|
||||
m_activeEntries.add(builder.getEntry(ACTIVE));
|
||||
} finally {
|
||||
m_mutex.unlock();
|
||||
}
|
||||
builder.addStringProperty(
|
||||
SELECTED,
|
||||
null,
|
||||
val -> {
|
||||
m_mutex.lock();
|
||||
try {
|
||||
m_selected = val;
|
||||
m_selectedValue = m_map.get(val).get();
|
||||
for (NetworkTableEntry entry : m_activeEntries) {
|
||||
entry.setString(val);
|
||||
}
|
||||
} finally {
|
||||
m_mutex.unlock();
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -24,18 +24,17 @@ public class SendableTable implements Sendable {
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("Table");
|
||||
builder.addRawProperty("table", this::getTableAsBytes, this::setTableFromBytes);
|
||||
builder.addStringArrayProperty("header", () -> new String[] { "distance", "hoodExt", "drumVelocity", "turretOffset" }, null);
|
||||
builder.addStringArrayProperty("header", () -> new String[] { "distance", "hoodExt", "drumVelocity"}, null);
|
||||
}
|
||||
|
||||
private byte[] getTableAsBytes() {
|
||||
ShooterTableEntry[] table = m_tableGetter.get();
|
||||
if (!Arrays.equals(tableCache, table)) {
|
||||
ByteBuffer byteBuffer = ByteBuffer.allocate(Double.BYTES * 4 * table.length);
|
||||
ByteBuffer byteBuffer = ByteBuffer.allocate(Double.BYTES * 3 * table.length);
|
||||
Arrays.stream(table).forEach(e -> {
|
||||
byteBuffer.putDouble(e.distance);
|
||||
byteBuffer.putDouble(e.hoodExt);
|
||||
byteBuffer.putDouble(e.drumVelocity);
|
||||
byteBuffer.putDouble(e.turretOffset);
|
||||
});
|
||||
tableCache = table;
|
||||
bytesCache = byteBuffer.array();
|
||||
@@ -46,12 +45,11 @@ public class SendableTable implements Sendable {
|
||||
private void setTableFromBytes(byte[] bytes) {
|
||||
if (bytes.length > 0 && !Arrays.equals(bytesCache, bytes)) {
|
||||
ByteBuffer byteBuffer = ByteBuffer.wrap(bytes);
|
||||
ShooterTableEntry[] table = new ShooterTableEntry[bytes.length / (4 * Double.BYTES)];
|
||||
ShooterTableEntry[] table = new ShooterTableEntry[bytes.length / (3 * Double.BYTES)];
|
||||
for (int i = 0; i < table.length; i++) {
|
||||
table[i].distance = byteBuffer.getDouble();
|
||||
table[i].hoodExt = byteBuffer.getDouble();
|
||||
table[i].drumVelocity = byteBuffer.getDouble();
|
||||
table[i].turretOffset = byteBuffer.getDouble();
|
||||
}
|
||||
m_tableSetter.accept(table);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user