Do some stuff

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

@@ -7,11 +7,17 @@
 
 package frc4388.robot;
 
+import java.util.List;
+import java.util.logging.Level;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.RobotTime;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+import frc4388.utility.Status.Report;
 //import frc4388.robot.subsystems.LED;
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -125,10 +131,18 @@ public class Robot extends TimedRobot {
   //  m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
   @Override
-  public void testPeriodic() {
+  public void testInit() {
+    for(int i=0;i<m_robotContainer.subsystems.size();i++){
+
+      Subsystem subsystem = m_robotContainer.subsystems.get(i);
+      subsystem.Log("Subsystem diagnostic report for " + subsystem.getName() + ":");
+      Status status = subsystem.diagnosticStatus();
+
+      for(int a=0;a<status.reports.size();a++){
+        Report r = status.reports.get(i);
+        subsystem.Log(r.toString());
+      }
+    }
   }
 }

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

@@ -9,6 +9,10 @@ package frc4388.robot;
 
 // Drive Systems
 import edu.wpi.first.wpilibj.DriverStation;
+
+import java.util.ArrayList;
+import java.util.List;
+
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.GenericHID;
 import frc4388.utility.controller.XboxController;
@@ -33,6 +37,7 @@ import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.Subsystem;
 import frc4388.utility.configurable.ConfigurableString;
 
 /**
@@ -65,6 +70,8 @@ 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<>();
+
     // ! Teleop Commands
 
     // ! /*  Autos */
@@ -96,6 +103,12 @@ public class RobotContainer {
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();
 
+        this.subsystems.add(m_robotSwerveDrive);
+        this.subsystems.add(m_robotMap.leftFront);
+        this.subsystems.add(m_robotMap.rightFront);
+        this.subsystems.add(m_robotMap.rightBack);
+        this.subsystems.add(m_robotMap.leftBack);
+
         // ! Swerve Drive One Module Test
         // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
         //     m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());

src/main/java/frc4388/robot/RobotMap.java

@@ -57,9 +57,9 @@ public class RobotMap {
 
     void configureDriveMotorControllers() {
         // initialize SwerveModules
-        this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
-        this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
-        this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
-        this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
+        this.rightFront = new SwerveModule("Right Front Swerve Module", rightFrontWheel, rightFrontSteer, rightFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_RIGHT_ROT_OFFSET);
+        this.leftFront = new SwerveModule("Left Front Swerve Module", leftFrontWheel, leftFrontSteer, leftFrontEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.FRONT_LEFT_ROT_OFFSET);
+        this.leftBack = new SwerveModule("Left Back Swerve Module", leftBackWheel, leftBackSteer, leftBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_LEFT_ROT_OFFSET);
+        this.rightBack = new SwerveModule("Right Back Swerve Module", rightBackWheel, rightBackSteer, rightBackEncoder, SwerveDriveConstants.DefaultSwerveRotOffsets.BACK_RIGHT_ROT_OFFSET);
     }
 }

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

@@ -1,36 +0,0 @@
-package frc4388.robot.subsystems;
-
-//import edu.wpi.first.apriltag.AprilTag;
-//import edu.wpi.first.math.geometry.Pose3d;
-//import edu.wpi.first.math.geometry.Rotation3d;
-//import edu.wpi.first.networktables.NetworkTable;
-//import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-public class Apriltags {
-    public static class Tag {
-        public boolean visible = true;
-        public double x,  y,  z  = 0;
-        public double ry, rp, rr = 0;
-    }
-
-    public Tag getTagPosRot() {
-        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-
-        final Tag tag = new Tag();
-        tag.visible = isAprilTag();
-        tag.x  = tagTable.getEntry("TagPosX").getDouble(0);
-        tag.y  = tagTable.getEntry("TagPosY").getDouble(0);
-        tag.z  = tagTable.getEntry("TagPosZ").getDouble(0);
-        tag.ry = tagTable.getEntry("TagRotY").getDouble(0);
-        tag.rp = tagTable.getEntry("TagRotP").getDouble(0);
-        tag.rr = tagTable.getEntry("TagRotR").getDouble(0);
-
-        return tag;
-    }
-
-    public boolean isAprilTag() {
-        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-        return tagTable.getEntry("IsTag").getBoolean(false);
-    }
-}

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

@@ -7,6 +7,8 @@
 
 package frc4388.robot.subsystems;
 
+import java.util.logging.Level;
+
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.hardware.TalonFX;
 
@@ -17,11 +19,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotTime;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+import frc4388.utility.Status.ReportLevel;
 
 /**
  * Add your docs here.
  */
-public class DiffDrive extends SubsystemBase {
+public class DiffDrive extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
@@ -85,4 +90,23 @@ public class DiffDrive extends SubsystemBase {
     SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
     //SmartDashboard.putData(m_gyro);
   }
+
+
+  @Override
+  public String getSubsystemName() {
+      return "Diff Drive";
+  }
+
+  @Override
+  public Status queryStatus() {
+      Status status = new Status();
+      // TODO: Add Stuff
+      return status;
+  }
+
+  @Override
+  public Status diagnosticStatus() {
+      Log("Diagnostic info for this has not been inplemented!"); //TODO
+      return new Status();
+  }
 }

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

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems;
 
+import java.util.logging.Level;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -17,8 +19,11 @@ import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.SwerveDriveConstants.Conversions;
 import frc4388.utility.RobotGyro;
 import frc4388.utility.RobotUnits;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+import frc4388.utility.Status.ReportLevel;
 
-public class  SwerveDrive extends SubsystemBase {
+public class SwerveDrive extends Subsystem {
   
   private SwerveModule leftFront;
   private SwerveModule rightFront;
@@ -285,29 +290,23 @@ public class  SwerveDrive extends SubsystemBase {
 
   public void setToSlow() {
     this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED;
-    System.out.println("SLOW");
-    System.out.println("SLOW");
-    System.out.println("SLOW");
-    System.out.println("SLOW");
-    System.out.println("SLOW");
+    for(int i=0;i<5;i++){
+      Log("SLOW");
+    }
   }
 
   public void setToFast() {
     this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED;
-    System.out.println("FAST");
-    System.out.println("FAST");
-    System.out.println("FAST");
-    System.out.println("FAST");
-    System.out.println("FAST");
+    for(int i=0;i<5;i++){
+      Log("FAST");
+    }
   }
 
   public void setToTurbo() {
     this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED;
-    System.out.println("TURBO");
-    System.out.println("TURBO");
-    System.out.println("TURBO");
-    System.out.println("TURBO");
-    System.out.println("TURBO");
+        for(int i=0;i<5;i++){
+      Log("TURBO");
+    }
   }
 
   public void toggleGear(double angle) {
@@ -328,6 +327,28 @@ public class  SwerveDrive extends SubsystemBase {
     rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
   }
 
+  @Override
+  public String getSubsystemName() {
+      return "Swerve Drive Controller";
+  }
+
+  @Override
+  public Status queryStatus() {
+      Status status = new Status();
+      
+      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
+  public Status diagnosticStatus() {
+      Log("Diagnostic info for this has not been inplemented!"); //TODO
+      return new Status();
+  }
+
   
 
 }

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

@@ -4,6 +4,8 @@
 
 package frc4388.robot.subsystems;
 
+import java.util.logging.Level;
+
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
@@ -33,8 +35,12 @@ import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+import frc4388.utility.Status.ReportLevel;
 
-public class SwerveModule extends SubsystemBase {
+public class SwerveModule extends Subsystem {
+    private String name = "Null";
     private TalonFX driveMotor;
     private TalonFX angleMotor;
     private CANcoder encoder;
@@ -47,7 +53,8 @@ public class SwerveModule extends SubsystemBase {
   
     
     /** Creates a new SwerveModule. */
-    public SwerveModule(TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
+    public SwerveModule(String name, TalonFX driveMotor, TalonFX angleMotor, CANcoder encoder, double offset) {
+        this.name = name;
         this.driveMotor = driveMotor;
         this.angleMotor = angleMotor;
         this.encoder = encoder;
@@ -227,6 +234,29 @@ public class SwerveModule extends SubsystemBase {
         // encoder.setPosition(0);
     }
 
+    @Override
+    public String getSubsystemName() {
+        return this.name;
+    }
+
+    @Override
+    public Status queryStatus() {
+        Status status = new Status();
+        
+        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;
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Log("Diagnostic info for this swereve module has not been inplemented!"); //TODO
+        return new Status();
+    }
+
     // public double getCurrent() {
     //     return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
     // }

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

@@ -1,38 +0,0 @@
-package frc4388.robot.subsystems;
-
-import edu.wpi.first.apriltag.AprilTag;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-public class Vision {
-    private final NetworkTableEntry m_isTags;
-    private final NetworkTableEntry m_xPoses;
-    private final NetworkTableEntry m_yPoses;
-    private final NetworkTableEntry m_zPoses;
-
-    public Vision() {
-        final var tagTable = NetworkTableInstance.getDefault().getTable("apriltag");
-
-        m_isTags = tagTable.getEntry("IsTag");
-        m_xPoses = tagTable.getEntry("TagPosX");
-        m_yPoses = tagTable.getEntry("TagPosY");
-        m_zPoses = tagTable.getEntry("TagPosZ");
-    }
-
-    public AprilTag[] getAprilTags() {
-        if (!m_isTags.getBoolean(false)) return new AprilTag[0];
-
-        double xarr[] = m_xPoses.getDoubleArray(new double[] {});
-        double yarr[] = m_yPoses.getDoubleArray(new double[] {});
-        double zarr[] = m_zPoses.getDoubleArray(new double[] {});
-
-        AprilTag tags[] = new AprilTag[xarr.length];
-        for (int i = 0; i < tags.length; i++) {
-            tags[i] = new AprilTag(0, new Pose3d(xarr[i], yarr[i], zarr[i], new Rotation3d()));
-        }
-
-        return tags;
-    }
-}

src/main/java/frc4388/utility/Status.java

@@ -0,0 +1,39 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class Status {
+    public enum ReportLevel {
+        INFO,
+        WARNING,
+        ERROR
+    }
+
+    public class Report {
+        public ReportLevel reportLevel;
+        public String description;
+
+        @Override
+        public String toString() {
+            return this.reportLevel.name() + ": " + this.description;
+        }
+    }
+
+    public List<Report> reports;
+
+    public Status() {
+        this.reports = new ArrayList<>();
+    }
+
+    public void addReport(ReportLevel level, String description) {
+        Report r = new Report();
+        r.reportLevel = level;
+        r.description = description;
+        this.reports.add(r);
+    }
+
+    public boolean hasReport() {
+        return reports.size() == 0;
+    }
+}

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

@@ -0,0 +1,16 @@
+package frc4388.utility;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class Subsystem extends SubsystemBase {
+    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"
+    public abstract Status queryStatus();
+    // Proactivly search for any errors in each subsystem
+    public abstract Status diagnosticStatus(); 
+}