Merge pull request #26 from jbko6/shuffleboard

Drivebase logging and config enhancement

.gitignore

@@ -172,6 +172,7 @@ out/
 *-window.json
 simgui*.json
 networktables.json
+networktables.json.bck
 
 # Simulation data log directory
 logs/

src/main/java/frc/team2412/robot/Robot.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.team2412.robot.util.MACAddress;
+import frc.team2412.robot.util.MatchDashboard;
 
 public class Robot extends TimedRobot {
 	/** Singleton Stuff */
@@ -31,8 +32,9 @@ public static Robot getInstance() {
 	private final RobotType robotType;
 	public Controls controls;
 	public Subsystems subsystems;
+	public MatchDashboard dashboard;
 
-	private SendableChooser<Command> autoChooser;
+	public SendableChooser<Command> autoChooser;
 
 	protected Robot(RobotType type) {
 		// non public for singleton. Protected so test class can subclass
@@ -53,7 +55,7 @@ private static RobotType getTypeFromAddress() {
 		if (CRANE_ADDRESS.exists()) return RobotType.CRANE;
 		if (!COMPETITION_ADDRESS.exists())
 			DriverStation.reportWarning(
-					"Code running on unknown MAC Address! Running competition code anyways", true);
+					"Code running on unknown MAC Address! Running competition code anyways", false);
 		return RobotType.COMPETITION;
 	}
 
@@ -65,7 +67,6 @@ public void robotInit() {
 		controls = new Controls(subsystems);
 
 		autoChooser = AutoBuilder.buildAutoChooser();
-		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		Shuffleboard.startRecording();
 
@@ -86,6 +87,8 @@ public void robotInit() {
 		SmartDashboard.putData(CommandScheduler.getInstance());
 
 		DriverStation.silenceJoystickConnectionWarning(true);
+
+		dashboard = new MatchDashboard(subsystems);
 	}
 
 	@Override

src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java

@@ -17,12 +17,14 @@
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.team2412.robot.Robot;
 import frc.team2412.robot.Robot.RobotType;
 import java.io.File;
 import java.util.EnumSet;
+import java.util.Map;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 import swervelib.SwerveDrive;
@@ -38,12 +40,12 @@ public class DrivebaseSubsystem extends SubsystemBase {
 	private static final double MAX_SPEED =
 			Robot.getInstance().getRobotType() == RobotType.PRACTICE
 					? 2.0
-					: Robot.getInstance().getRobotType() == RobotType.CRANE ? 3.0 : 0.0;
+					: Robot.getInstance().getRobotType() == RobotType.CRANE ? 3.0 : 1.0;
 	// distance from center of the robot to the furthest module
 	private static final double DRIVEBASE_RADIUS =
 			Robot.getInstance().getRobotType() == RobotType.PRACTICE
 					? 0.305328701
-					: Robot.getInstance().getRobotType() == RobotType.CRANE ? 0.3937 : 0.0;
+					: Robot.getInstance().getRobotType() == RobotType.CRANE ? 0.3937 : 0.3;
 	private static final double JOYSTICK_DEADBAND = 0.05;
 	private static final double HEADING_CORRECTION_DEADBAND = 0.005;
 
@@ -61,6 +63,9 @@ public class DrivebaseSubsystem extends SubsystemBase {
 
 	// shuffleboard variables
 	private GenericEntry headingCorrectionEntry;
+	private GenericEntry translationSpeedEntry;
+	private GenericEntry rotationSpeedEntry;
+	private GenericEntry xWheelsEntry;
 
 	public DrivebaseSubsystem() {
 		initShuffleboard();
@@ -119,7 +124,7 @@ public DrivebaseSubsystem() {
 
 		// LOW verbosity only sends field position, HIGH sends full drive data, MACHINE sends data
 		// viewable by AdvantageScope
-		SwerveDriveTelemetry.verbosity = TelemetryVerbosity.MACHINE;
+		SwerveDriveTelemetry.verbosity = TelemetryVerbosity.LOW;
 	}
 
 	/**
@@ -159,13 +164,16 @@ public Command driveJoystick(
 					Rotation2d constrainedRotation =
 							Rotation2d.fromRotations(
 									SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND)
-											* MAX_SPEED);
+											* MAX_SPEED
+											* rotationSpeedEntry.getDouble(1.0));
 					Translation2d constrainedTranslation =
 							new Translation2d(
 									SwerveMath.applyDeadband(forward.getAsDouble(), true, JOYSTICK_DEADBAND)
-											* MAX_SPEED,
+											* MAX_SPEED
+											* translationSpeedEntry.getDouble(1.0),
 									SwerveMath.applyDeadband(strafe.getAsDouble(), true, JOYSTICK_DEADBAND)
-											* MAX_SPEED);
+											* MAX_SPEED
+											* translationSpeedEntry.getDouble(1.0));
 					drive(constrainedTranslation, constrainedRotation, true);
 				});
 	}
@@ -201,6 +209,11 @@ public void resetRobot() {
 
 	public void toggleXWheels() {
 		xWheelsEnabled = !xWheelsEnabled;
+		xWheelsEntry.setBoolean(xWheelsEnabled);
+	}
+
+	public Field2d getField() {
+		return swerveDrive.field;
 	}
 
 	private void initShuffleboard() {
@@ -218,5 +231,26 @@ private void initShuffleboard() {
 				event -> {
 					swerveDrive.setHeadingCorrection(event.valueData.value.getBoolean());
 				});
+
+		translationSpeedEntry =
+				drivebaseTab
+						.addPersistent("Translation Speed", 1.0)
+						.withWidget(BuiltInWidgets.kNumberSlider)
+						.withSize(2, 1)
+						.withProperties(Map.of("Min", 0.0))
+						.getEntry();
+		rotationSpeedEntry =
+				drivebaseTab
+						.addPersistent("Rotation Speed", 1.0)
+						.withWidget(BuiltInWidgets.kNumberSlider)
+						.withSize(2, 1)
+						.withProperties(Map.of("Min", 0.0))
+						.getEntry();
+		xWheelsEntry =
+				drivebaseTab
+						.add("X Wheels", xWheelsEnabled)
+						.withWidget(BuiltInWidgets.kBooleanBox)
+						.withSize(1, 1)
+						.getEntry();
 	}
 }

src/main/java/frc/team2412/robot/util/FMSWidget.java

@@ -0,0 +1,16 @@
+package frc.team2412.robot.util;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+public class FMSWidget implements Sendable {
+	public FMSWidget() {
+		SendableRegistry.add(this, "FMSInfo");
+	}
+
+	@Override
+	public void initSendable(SendableBuilder builder) {
+		builder.setSmartDashboardType("FMSInfo");
+	}
+}

src/main/java/frc/team2412/robot/util/MatchDashboard.java

@@ -0,0 +1,25 @@
+package frc.team2412.robot.util;
+
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import frc.team2412.robot.Robot;
+import frc.team2412.robot.Subsystems;
+
+/*
+ * Initializes and updates shuffleboard match dashboard entries
+ */
+public class MatchDashboard {
+
+	private final ShuffleboardTab tab = Shuffleboard.getTab("Match");
+	private final Field2d field;
+
+	public MatchDashboard(Subsystems s) {
+		field = s.drivebaseSubsystem.getField();
+
+		tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1);
+		tab.add(field).withPosition(0, 1).withSize(4, 3);
+		Robot r = Robot.getInstance();
+		tab.add("Auto Chooser", r.autoChooser).withPosition(4, 0).withSize(2, 1);
+	}
+}