diff --git a/readme.md b/readme.md index 1ad89d2..87bb837 100644 --- a/readme.md +++ b/readme.md @@ -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. diff --git a/shuffleboard.json b/shuffleboard.json index b994472..c2bef77 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -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 + } } } } diff --git a/src/main/deploy/ShooterData.csv b/src/main/deploy/ShooterData.csv index b3d93af..d833fcb 100644 --- a/src/main/deploy/ShooterData.csv +++ b/src/main/deploy/ShooterData.csv @@ -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 diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 085b6d7..9f0ec80 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -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(); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b482307..0a75d87 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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 autoChooser = new SendableChooser(); + private final CachingSendableChooser 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 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; } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index ab3ab1e..519f33b 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -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"); diff --git a/src/main/java/frc4388/robot/commands/PathPlannerCommand.java b/src/main/java/frc4388/robot/commands/PathPlannerCommand.java new file mode 100644 index 0000000..ea2a687 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PathPlannerCommand.java @@ -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 PATH_EXTENSION_REMOVER = ((Function) 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.")); + } + } +} diff --git a/src/main/java/frc4388/robot/commands/PathRecorder.java b/src/main/java/frc4388/robot/commands/PathRecorder.java index 33d3140..98c635d 100644 --- a/src/main/java/frc4388/robot/commands/PathRecorder.java +++ b/src/main/java/frc4388/robot/commands/PathRecorder.java @@ -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 PATH_EXTENSION_REMOVER = ((Function) 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 pathChooser = new ListeningSendableChooser<>(this::loadPath); + private final CachingSendableChooser m_autoChooser; private final List 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 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()); } diff --git a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java index 2f02e68..db6c3f1 100644 --- a/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java +++ b/src/main/java/frc4388/robot/commands/shuffleboard/ShooterTuner.java @@ -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); } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index a86ec46..d585c49 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -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 }; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 33ffc47..795268d 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -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; diff --git a/src/main/java/frc4388/utility/AnsiLogging.java b/src/main/java/frc4388/utility/AnsiLogging.java index 0820892..7b706b8 100644 --- a/src/main/java/frc4388/utility/AnsiLogging.java +++ b/src/main/java/frc4388/utility/AnsiLogging.java @@ -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. diff --git a/src/main/java/frc4388/utility/shuffleboard/CachingSendableChooser.java b/src/main/java/frc4388/utility/shuffleboard/CachingSendableChooser.java new file mode 100644 index 0000000..3993949 --- /dev/null +++ b/src/main/java/frc4388/utility/shuffleboard/CachingSendableChooser.java @@ -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}. + * + *

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 The type of the values to be stored + */ +public class CachingSendableChooser 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> 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 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 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 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(); + } + }); + } +} diff --git a/src/main/java/frc4388/utility/shuffleboard/SendableTable.java b/src/main/java/frc4388/utility/shuffleboard/SendableTable.java index 000bdce..23548fc 100644 --- a/src/main/java/frc4388/utility/shuffleboard/SendableTable.java +++ b/src/main/java/frc4388/utility/shuffleboard/SendableTable.java @@ -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); }