Merge branch 'formalise' of https://github.com/Team4388/Robot-Essentials into formalise

src/main/java/frc4388/robot/Robot.java

@@ -15,7 +15,9 @@ import java.util.logging.Level;
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.CANBus.CANBusStatus;
 
+import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.CanDevice;
@@ -49,6 +51,26 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+
+
+    new Thread() {
+      public void run() {
+        try{
+        while(!this.isInterrupted() && this.isAlive()){
+          Thread.sleep(500);
+          for(int i=0;i<Subsystem.subsystems.size(); i++){
+            Subsystem.subsystems.get(i).queryStatus();
+          }
+
+          System.out.println("Updated statuses!");
+          
+        }
+        }catch(Exception e){
+          e.printStackTrace();
+        }
+      }
+    }.start();
   }
 
   /**
@@ -146,9 +168,9 @@ public class Robot extends TimedRobot {
     // Subsystems header
     System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY")));
 
-    for(int i=0;i<m_robotContainer.subsystems.size();i++){
+    for(int i=0;i< Subsystem.subsystems.size();i++){
 
-      Subsystem subsystem = m_robotContainer.subsystems.get(i);
+      Subsystem subsystem = Subsystem.subsystems.get(i);
       System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":");
       Status status = subsystem.diagnosticStatus();
 

src/main/java/frc4388/robot/RobotContainer.java

@@ -70,7 +70,7 @@ public class RobotContainer {
     private final VirtualController m_virtualDriver = new VirtualController(0);
     private final VirtualController m_virtualOperator = new VirtualController(1);
 
-    public List<Subsystem> subsystems = new ArrayList<>();
+    // public List<Subsystem> subsystems = new ArrayList<>();
 
     // ! Teleop Commands
 
@@ -103,11 +103,11 @@ public class RobotContainer {
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();
 
-        this.subsystems.add(m_robotSwerveDrive);
+        // this.subsystems.add(m_robotSwerveDrive);
-        this.subsystems.add(m_robotMap.leftFront);
+        // this.subsystems.add(m_robotMap.leftFront);
-        this.subsystems.add(m_robotMap.rightFront);
+        // this.subsystems.add(m_robotMap.rightFront);
-        this.subsystems.add(m_robotMap.rightBack);
+        // this.subsystems.add(m_robotMap.rightBack);
-        this.subsystems.add(m_robotMap.leftBack);
+        // this.subsystems.add(m_robotMap.leftBack);
 
         // ! Swerve Drive One Module Test
         // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {

src/main/java/frc4388/robot/subsystems/DiffDrive.java

@@ -45,6 +45,8 @@ public class DiffDrive extends Subsystem {
   public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
       TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
     
+        super();
+
     m_leftFrontMotor = leftFrontMotor;
     m_rightFrontMotor = rightFrontMotor;
     m_leftBackMotor = leftBackMotor;
@@ -98,10 +100,8 @@ public class DiffDrive extends Subsystem {
   }
 
   @Override
-  public Status queryStatus() {
+  public void queryStatus() {
-      Status status = new Status();
       // TODO: Add Stuff
-      return status;
   }
 
   @Override

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -55,6 +55,7 @@ public class SwerveDrive extends Subsystem {
 
   /** Creates a new SwerveDrive. */
   public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) {
+    super();
     this.leftFront = leftFront;
     this.rightFront = rightFront;
     this.leftBack = leftBack;
@@ -334,14 +335,17 @@ public class SwerveDrive extends Subsystem {
   }
 
   @Override
-  public Status queryStatus() {
+  public void queryStatus() {
-    Status status = new Status();
+
+    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();
   
-    status.addReport(ReportLevel.INFO, "Gyro Angle: " + this.gyro.getAngle());
-    status.addReport(ReportLevel.INFO, "Shift State: " + this.speedAdjust);
     //TODO: Add more status things
-    
-    return status;
   }
 
   @Override

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -33,6 +33,7 @@ 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.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -56,6 +57,7 @@ public class SwerveModule extends Subsystem {
     
     /** Creates a new SwerveModule. */
     public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
+        super();
         this.name = name;
         this.driveMotor = driveMotor;
         this.angleMotor = angleMotor;
@@ -242,15 +244,10 @@ public class SwerveModule extends Subsystem {
     }
 
     @Override
-    public Status queryStatus() {
+    public void queryStatus() {
-        Status status = new Status();
+        SmartDashboard.putNumber("[" + getSubsystemName() + "] Drive motor speed", this.driveMotor.get());
-        
+        SmartDashboard.putNumber("[" + getSubsystemName() + "] Angle motor angle", this.angleMotor.getRotorPosition().getValueAsDouble());
-        status.addReport(ReportLevel.INFO, "Drive motor speed: " + this.driveMotor.get());
-        status.addReport(ReportLevel.INFO, "Angle motor speed: " + this.angleMotor.get());
         //TODO: Add more status things
-
-
-        return status;
     }
 
     public boolean motorsAlive() {

src/main/java/frc4388/utility/CanDevice.java

@@ -52,4 +52,6 @@ public class CanDevice {
 
         return s;
     }
+
+    
 }

src/main/java/frc4388/utility/Subsystem.java

@@ -1,16 +1,25 @@
 package frc4388.utility;
 
+import java.util.ArrayList;
+import java.util.List;
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public abstract class Subsystem extends SubsystemBase {
+    public static List<Subsystem> subsystems = new ArrayList<>();
+
+    public Subsystem(){
+        subsystems.add(this);
+    }
+
     public void Log(String str) {
         System.out.println(getSubsystemName() + " - " + str);
     }
 
     // Get name of subsystem, for use in log.
     public abstract String getSubsystemName();
-    // Get what the subystem is currently doing, such as "Shooter spun up"
+    // Get what the subystem is currently doing, such as "Shooter spun up". This should post to SmartDashboard
-    public abstract Status queryStatus();
+    public abstract void queryStatus();
     // Proactivly search for any errors in each subsystem
     public abstract Status diagnosticStatus(); 
 }