From 5348cafeeabed815b4a2881acf7c83132ee0061b Mon Sep 17 00:00:00 2001
From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com>
Date: Sun, 5 Jan 2025 11:25:37 -0700
Subject: [PATCH] Add ShuffleBoard lists

---
 src/main/java/frc4388/robot/Robot.java        |  2 +-
 .../frc4388/robot/subsystems/SwerveDrive.java | 29 ++++++++++----
 .../robot/subsystems/SwerveModule.java        | 38 ++++++++++++++++++-
 3 files changed, 58 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index f8acad3..e64ca36 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -63,7 +63,7 @@ public class Robot extends TimedRobot {
             Subsystem.subsystems.get(i).queryStatus();
           }
 
-          System.out.println("Updated statuses!");
+          // System.out.println("Updated statuses!");
           
         }
         }catch(Exception e){
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
index cafe872..7adf8f8 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java
@@ -12,6 +12,11 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -334,16 +339,24 @@ public class SwerveDrive extends Subsystem {
     return "Swerve Drive Controller";
   }
 
+  ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
+  .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+  .withSize(2, 2);
+
+  GenericEntry sbGyro = subsystemLayout
+  .add("Gyro angle", 0)
+  .withWidget(BuiltInWidgets.kGyro)
+  .getEntry();
+
+  GenericEntry sbShiftState = subsystemLayout
+  .add("Shift State", 0)
+  .withWidget(BuiltInWidgets.kNumberBar)
+  .getEntry();
+
   @Override
   public void queryStatus() {
-
-    SmartDashboard.putNumber("[" + getSubsystemName() + "] Gyro angle", this.gyro.getAngle());
-    SmartDashboard.putNumber("[" + getSubsystemName() + "] Shift State", this.speedAdjust);
-
-    // this.leftFront.queryStatus();
-    // this.leftBack.queryStatus();
-    // this.rightFront.queryStatus();
-    // this.rightBack.queryStatus();
+    sbGyro.setDouble(this.gyro.getAngle());
+    sbShiftState.setDouble(this.speedAdjust);
   
     //TODO: Add more status things
   }
diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
index 2ee9ca6..471c6ec 100644
--- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java
@@ -33,6 +33,11 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -243,10 +248,39 @@ public class SwerveModule extends Subsystem {
         return this.name;
     }
 
+    ShuffleboardLayout subsystemLayout;
+    GenericEntry sbSpeed;
+    GenericEntry sbAngle;
+
+    private void createLayout(){
+
+        subsystemLayout = Shuffleboard.getTab("Subsystems")
+        .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+        .withSize(2, 2);
+
+        sbSpeed =  subsystemLayout
+        .add("Drive motor speed", 0)
+        .withWidget(BuiltInWidgets.kNumberBar)
+        .getEntry();
+
+        sbAngle =  subsystemLayout
+        .add("Angle motor angle", 0)
+        .withWidget(BuiltInWidgets.kGyro)
+        .getEntry();
+    }
+
+
     @Override
     public void queryStatus() {
-        SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get());
-        SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble());
+        if(subsystemLayout == null)
+            createLayout();
+
+        // Shuffleboard.getTab("Subsystems").set
+
+        // sbSpeed.setDouble(this.driveMotor.get());
+        sbAngle.setDouble(this.angleMotor.getRotorPosition().getValueAsDouble());
+        // SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get());
+        // SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble());
         //TODO: Add more status things
     }