Driver Dashboard configuration (#144)

* post camera to smartdashboard * add driver dashboard data * remove CameraServe in RobotContainer --------- Co-authored-by: Connor Murphy <[email protected]>
FRC-Team-620 · Apr 3, 2024 · 8ff9fd4 · 8ff9fd4
1 parent e3ea033
commit 8ff9fd4
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 2 deletions.
diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java
@@ -89,6 +89,8 @@ public class RobotContainer implements Logged {
 	// DefaultIntakeCommand(intakeSubsystem,
 	// shooterSubsystem);
 
+	private final DriveCommand driveCommand;
+
 	public RobotContainer() {
 		SwitchableControlBoard swboard = new SwitchableControlBoard(new CompControl());
 		if (Robot.isSimulation()) { // Switch to single control in sim
@@ -97,6 +99,7 @@ public RobotContainer() {
 		// swboard.setControlBoard(new CompControl());
 
 		this.control = swboard;
+		this.driveCommand = new DriveCommand(this.driveSubsystem, visionSubsystem, this.control);
 		this.driveSubsystem
 				.setDefaultCommand(new DriveCommand(this.driveSubsystem, this.visionSubsystem, this.control));
 
@@ -106,6 +109,9 @@ public RobotContainer() {
 
 		configureSmartDashboard();
 		SmartDashboard.putBoolean("HasNote", false);
+		SmartDashboard.putBoolean("LockAtGoal", this.driveCommand.lockAtGoal());
+		SmartDashboard.putNumber("MatchTime", DriverStation.getMatchTime());
+		SmartDashboard.putNumber("MatchNumber", DriverStation.getMatchNumber());
 		SmartDashboard.putData("lock speaker", new LockSpeaker(this.driveSubsystem, this.visionSubsystem));
 		configureDriverFeedback();
 

diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java
@@ -221,6 +221,7 @@ public void periodic() {
 				new Pose3d(-0.213, 0, 0.286, new Rotation3d(0, -Units.degreesToRadians(armPID.getGoal().position), 0)));
 		log("angleDegrees", getArmPitch());
 		log("angleGoalDegrees", armPID.getGoal().position);
+		log("ArmAtGoal", this.atGoal());
 	}
 
 	SingleJointedArmSim armSim;

diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java
@@ -26,6 +26,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.BuiltInAccelerometer.Range;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import monologue.Logged;
 import monologue.Annotations.Log;
@@ -92,6 +93,7 @@ public void periodic() {
 		log("Y-Gs-rio", rio_Accelerometer.getY());
 		log("X-Gs-rio", rio_Accelerometer.getX());
 		log("Z-Gs-rio", rio_Accelerometer.getZ());
+		SmartDashboard.putNumber("GyroAngle", m_gyro.getAngle());
 
 		if (Robot.isSimulation()) {
 			Robot.objSim.update(pose2d);

diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/DriveCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/DriveCommand.java
@@ -16,7 +16,6 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class DriveCommand extends Command {
@@ -78,7 +77,8 @@ public void execute() {
 		// SmartDashboard.putNumber("SwerveDrive/Input/SwerveDriveXSpeed", ySpeed);
 		// SmartDashboard.putNumber("SwerveDrive/Input/SwerveDriveXSpeed",
 		// rotationSpeed);
-		SmartDashboard.putNumber("Angle Offset", computeAngleLockValue());
+		// SmartDashboard.putNumber("Angle Offset", computeAngleLockValue());
+
 		if (this.control.AprilLockOn().getAsBoolean()) {
 			rotationSpeed += computeAngleLockValue();
 		}
@@ -140,5 +140,8 @@ public boolean isFinished() {
 	private double getSquareInput(double input) {
 		return Math.pow(input, 2) * Math.signum(input);
 	}
+	public boolean lockAtGoal() {
+		return this.lockPID.atSetpoint();
+	}
 
 }