merge swerve branch

src/main/java/frc4388/robot/Constants.java

@@ -10,6 +10,10 @@ package frc4388.robot;
 import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Rotations;
 
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
@@ -27,6 +31,18 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement;
 import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
@@ -269,7 +285,25 @@ public final class Constants {
         public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
       }
     
-    public static final class VisionConstants {
+    public static final class VisionConstants { 
+        public static final String CAMERA_NAME = "Camera_Module_v1";
+
+        public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI));
+
+        // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+
+        
+        public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
+            Arrays.asList(new AprilTag[] {
+                new AprilTag(1, new Pose3d(
+                    new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.)
+                )),
+            }), 100, 100);
+
+        // The standard deviations of our vision estimated poses, which affect correction rate
+        // (Fake values. Experiment and determine estimation noise on an actual robot.)
+        public static final Matrix<N3, N1> kSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
     }
 
     public static final class DriveConstants {
@@ -289,4 +323,4 @@ public final class Constants {
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
     }
-}
+}

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

@@ -165,6 +165,13 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
           
         // ?  /* Operator Buttons */
+
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision));
+        
+        new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); 
+            // creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time
             
         // ? /* Programer Buttons (Controller 3)*/
 
@@ -224,6 +231,8 @@ public class RobotContainer {
 	    DriverStation.reportError("Big oops: " + e.getMessage(), e.getStackTrace());
 	    return Commands.none();
 	}
+	// zach told me to do the below comment
+	//return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
     }
 
 

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

@@ -8,6 +8,9 @@
 package frc4388.robot;
 
 import com.ctre.phoenix6.hardware.TalonFX;
+
+import org.photonvision.PhotonCamera;
+
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
@@ -18,6 +21,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 // import edu.wpi.first.wpilibj.motorcontrol.Spark;
 // import frc4388.robot.Constants.LEDConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.robot.Constants.VisionConstants;
 // import frc4388.robot.subsystems.SwerveModule;
 import frc4388.utility.RobotGyro;
 
@@ -28,6 +32,8 @@ import frc4388.utility.RobotGyro;
 public class RobotMap {
     // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id);
     // public RobotGyro gyro = new RobotGyro(m_pigeon2);
+
+    public PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME);
     
     public RobotMap() {
         configureDriveMotorControllers();

src/main/java/frc4388/robot/commands/GotoPositionCommand.java

@@ -0,0 +1,173 @@
+package frc4388.robot.commands;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.Vision;
+import frc4388.utility.Gains;
+import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
+import frc4388.utility.UtilityStructs.AutoRecordingFrame;
+import frc4388.utility.controller.VirtualController;
+
+public class GotoPositionCommand extends Command {
+    
+
+    // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
+    // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
+    private Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
+    static Gains xygains = new Gains(3,0,0);
+    static Gains rotgains = new Gains(0.1,0,0.0);
+    static double tolerance = 0;
+
+    private PID xPID = new PID(xygains, 0);
+    private PID yPID = new PID(xygains, 0);
+    private PID rotPID = new PID(rotgains, 0);
+
+    SwerveDrive swerveDrive;
+    Vision vision;
+
+    /**
+     * Command to drive robot to position.
+     * @param SwerveDrive m_robotSwerveDrive
+     */
+
+    public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
+        this.swerveDrive = swerveDrive;
+        this.vision = vision;
+        addRequirements(swerveDrive);
+    }
+
+    @Override
+    public void initialize() {
+        xPID.initialize();
+        yPID.initialize();
+    }
+    
+    double xerr;
+    double yerr;
+    double roterr;
+    double xytolerance = 0.01;
+    double rottolerance = 1;
+
+    @Override
+    public void execute() {
+        xerr = targetpos.getX() - vision.getPose2d().getX();
+        yerr = targetpos.getY() - vision.getPose2d().getY();
+        roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees();
+
+        SmartDashboard.putNumber("PID X Error", xerr);
+        SmartDashboard.putNumber("PID Y Error", yerr);
+
+        double xoutput = xPID.update(xerr);
+        double youtput = yPID.update(yerr);
+        double rotoutput = rotPID.update(roterr);
+
+        Translation2d leftStick = new Translation2d(
+            Math.max(Math.min(youtput, 1), -1),
+            Math.max(Math.min(xoutput, 1), -1)
+            // 0,0
+        );
+
+        Translation2d rightStick = new Translation2d(
+            Math.max(Math.min(rotoutput, 1), -1), 
+           0
+        );
+
+        SmartDashboard.putNumber("PID X Output", xoutput);
+        SmartDashboard.putNumber("PID Y Output", youtput);
+        // SmartDashboard.putNumber("PID Y Output", youtput);
+
+        swerveDrive.driveWithInput(leftStick, rightStick, true);
+    }
+
+    @Override
+    public final boolean isFinished() {
+        return (Math.abs(xerr) < xytolerance && Math.abs(yerr) < xytolerance && Math.abs(roterr) < rottolerance);
+                // this statement is a boolean in and of itself
+    }
+    
+    // @Override
+    // public void end(boolean interrupted) {
+       
+    // }
+    
+
+
+    // @Override
+    // public double getError() {
+    //     return e; 
+    // }
+
+
+
+    // @Override
+    // public void runWithOutput(double output) {
+    //     // TODO Auto-generated method stub
+    //     Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0);
+    //     Translation2d rightStick = new Translation2d();
+    //     // System.out.println("Output = " + -output);
+    //     SmartDashboard.putNumber("PID Output", output);
+    //     swerveDrive.driveWithInput(leftStick, rightStick, true);
+    // }
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+    private class PID {
+        protected Gains  gains;
+        private   double output    = 0;
+        private   double tolerance = 0;
+
+        /** Creates a new PelvicInflammatoryDisease. */
+        public PID(double kp, double ki, double kd, double kf, double tolerance) {
+            gains          = new Gains(kp, ki, kd, kf, 0);
+            this.tolerance = tolerance;
+        }
+
+        public PID(Gains gains, double tolerance) {
+            this.gains     = gains;
+            this.tolerance = tolerance;
+        }
+
+        // Called when the command is initially scheduled.
+        public final void initialize() {
+            output = 0;
+        }
+
+        private double prevError, cumError = 0;
+        
+        // Called every time the scheduler runs while the command is scheduled.
+        public double update(double error) {
+            cumError += error * .02; // 20 ms
+            double delta = error - prevError;
+
+            output = error * gains.kP;
+            output += cumError * gains.kI;
+            output += delta * gains.kD;
+            output += gains.kF;
+
+            return output;
+        }
+
+        // // Returns true when the command should end.
+        // public final boolean isFinished() {
+        //     return Math.abs(getError()) < tolerance;
+        // }
+    }
+}

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

@@ -26,13 +26,15 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.robot.Constants.SwerveDriveConstants;
-
+import frc4388.robot.Constants.VisionConstants;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
 import frc4388.utility.Status.ReportLevel;
 
 public class SwerveDrive extends Subsystem {
   private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
+
+  private Vision vision;
   
   private int gear_index = SwerveDriveConstants.STARTING_GEAR;
   private boolean stopped = false;
@@ -45,16 +47,15 @@ public class SwerveDrive extends Subsystem {
   public Rotation2d orientRotTarget = new Rotation2d();
   public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
-  private Field2d field = new Field2d();
-
   /** Creates a new SwerveDrive. */
-  public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
+  public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
+    // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain) {
     super();
 
-    SmartDashboard.putData(field);
-
     this.swerveDriveTrain = swerveDriveTrain;
+    this.vision = vision;
   }
+
   // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){
   //   // double ang = Math.atan2(rightStick.getY(), rightStick.getX());
   //   // rightStick.getAngle()
@@ -154,6 +155,23 @@ public class SwerveDrive extends Subsystem {
     return false;
   }
 
+  public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) {
+    // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
+    //   stopModules(); // stop the swerve
+    
+    // if (leftStick.getNorm() < 0.05) //if no imput
+    //   return; // don't bother doing swerve drive math and return early.
+
+    leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET));
+    
+    swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle()
+      .withVelocityX(leftStick.getX()*-speedAdjust)
+      .withVelocityY(leftStick.getY()*speedAdjust)
+      .withTargetDirection(rot)
+    );
+      // double
+  }
+
   public double getGyroAngle() {
     return swerveDriveTrain.getRotation3d().getAngle();
   }
@@ -172,10 +190,15 @@ public class SwerveDrive extends Subsystem {
     SmartDashboard.putNumber("Gyro", getGyroAngle());
     SmartDashboard.putNumber("RotTartget", rotTarget);
 
-    Optional<Pose2d> e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds());
+    double time = Vision.getTime();
 
-    if(e.isPresent())
-      field.setRobotPose(e.get());
+    vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
+
+    if(vision.isTag()){
+      swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
+    }
+
+    // if(e.isPresent())
   }
 
   private void reset_index() {

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

@@ -0,0 +1,281 @@
+package frc4388.robot.subsystems;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
+import java.util.List;
+import java.util.Optional;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
+import org.photonvision.targeting.MultiTargetPNPResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import com.ctre.phoenix6.Utils;
+
+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.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.Constants.VisionConstants;
+import frc4388.utility.Status;
+import frc4388.utility.Subsystem;
+
+public class Vision extends Subsystem {
+
+    private PhotonCamera camera;
+
+    private boolean isTag = false;
+    private Pose2d lastVisionPose = new Pose2d();
+    private Pose2d lastPhysOdomPose = new Pose2d();
+
+    private Matrix<N3, N1> curStdDevs;
+    private final PhotonPoseEstimator photonEstimator;
+
+    private Field2d field = new Field2d();
+
+    
+    ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
+    .getLayout(getSubsystemName(), BuiltInLayouts.kList)
+    .withSize(2, 2);
+
+    GenericEntry sbTag = subsystemLayout
+    .add("Tag Detected", false)
+    .withWidget(BuiltInWidgets.kBooleanBox)
+    .getEntry();
+    
+    GenericEntry sbCamConnected = subsystemLayout
+    .add("Camera Connnected", false)
+    .withWidget(BuiltInWidgets.kBooleanBox)
+    .getEntry();
+    
+    public Vision(PhotonCamera camera){
+        this.camera = camera;
+        SmartDashboard.putData(field);
+
+        photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS);
+        photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
+    }
+
+    @Override
+    public void periodic() {
+        var result = camera.getLatestResult();
+        isTag = result.hasTargets();
+        
+        // Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
+
+        // if (multitag.isEmpty()) {
+        //     Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best;
+        // }else if()
+
+        
+        // sbTag.setBoolean(isTag);
+        // field.setRobotPose(getPose2d());
+
+        // sbCamConnected.setBoolean(camera);
+
+        // System.out.println(isTag);
+
+        if(!isTag){
+            sbTag.setBoolean(isTag);
+            field.setRobotPose(getPose2d());
+            return;
+        }
+
+        var EstimatedRobotPose = getEstimatedGlobalPose();
+
+        // In case the pose estimator fails to estimate the pose, fallback to physical odometry.
+        if(EstimatedRobotPose.isEmpty()){
+            isTag = false;
+            sbTag.setBoolean(isTag);
+            field.setRobotPose(getPose2d());
+            return;
+        }
+
+        lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
+        // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation()));
+        // lastVisionPose = new Pose2d(
+        //     lastVisionPose.getTranslation(),
+        //     lastPhysOdomPose.getRotation()
+        // );
+
+        
+        
+        field.setRobotPose(getPose2d());
+    }
+
+
+
+
+    /**
+     * The latest estimated robot pose on the field from vision data. This may be empty. This should
+     * only be called once per loop.
+     *
+     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * {@link getEstimationStdDevs}
+     *
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
+     *     used for estimation.
+     */
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+        Optional<EstimatedRobotPose> visionEst = Optional.empty();
+        for (var change : camera.getAllUnreadResults()) {
+            visionEst = photonEstimator.update(change);
+            updateEstimationStdDevs(visionEst, change.getTargets());
+
+            // if (Robot.isSimulation()) {
+            //     visionEst.ifPresentOrElse(
+            //             est ->
+            //                     getSimDebugField()
+            //                             .getObject("VisionEstimation")
+            //                             .setPose(est.estimatedPose.toPose2d()),
+            //             () -> {
+            //                 getSimDebugField().getObject("VisionEstimation").setPoses();
+            //             });
+            // }
+        }
+        return visionEst;
+    }
+
+
+
+
+
+
+
+    /**
+     * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard
+     * deviations based on number of tags, estimation strategy, and distance from the tags.
+     *
+     * @param estimatedPose The estimated pose to guess standard deviations for.
+     * @param targets All targets in this camera frame
+     */
+    private void updateEstimationStdDevs(
+            Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
+        if (estimatedPose.isEmpty()) {
+            // No pose input. Default to single-tag std devs
+            curStdDevs = VisionConstants.kSingleTagStdDevs;
+
+        } else {
+            // Pose present. Start running Heuristic
+            var estStdDevs = VisionConstants.kSingleTagStdDevs;
+            int numTags = 0;
+            double avgDist = 0;
+
+            // Precalculation - see how many tags we found, and calculate an average-distance metric
+            for (var tgt : targets) {
+                var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
+                if (tagPose.isEmpty()) continue;
+                numTags++;
+                avgDist +=
+                        tagPose
+                                .get()
+                                .toPose2d()
+                                .getTranslation()
+                                .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+            }
+
+            if (numTags == 0) {
+                // No tags visible. Default to single-tag std devs
+                curStdDevs = VisionConstants.kSingleTagStdDevs;
+            } else {
+                // One or more tags visible, run the full heuristic.
+                avgDist /= numTags;
+                // Decrease std devs if multiple targets are visible
+                if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs;
+                // Increase std devs based on (average) distance
+                if (numTags == 1 && avgDist > 4)
+                    estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+                else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
+                curStdDevs = estStdDevs;
+            }
+        }
+    }
+
+    /**
+     * Returns the latest standard deviations of the estimated pose from {@link
+     * #getEstimatedGlobalPose()}, for use with {@link
+     * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should
+     * only be used when there are targets visible.
+     */
+    public Matrix<N3, N1> getEstimationStdDevs() {
+        return curStdDevs;
+    }
+
+
+
+
+
+
+
+
+
+    public void setLastOdomPose(Optional<Pose2d> pose){
+        if(pose.isPresent())
+            lastPhysOdomPose = pose.get();
+    }
+
+    public Pose2d getPose2d() {
+        if(isTag)
+            return lastVisionPose;
+        else
+            return lastPhysOdomPose;
+    }
+
+    public static double getTime() {
+        return Utils.getCurrentTimeSeconds();
+    }
+
+    public boolean isTag(){
+        return isTag;
+    }
+
+
+
+
+
+
+
+
+
+
+
+    @Override
+    public String getSubsystemName() {
+        return "Vision";
+    }
+
+//   GenericEntry sbShiftState = subsystemLayout
+//   .add("Shift State", 0)
+//   .withWidget(BuiltInWidgets.kNumberBar)
+//   .getEntry();
+
+
+    @Override
+    public void queryStatus() {
+        sbTag.setBoolean(isTag);
+        sbCamConnected.setBoolean(camera.isConnected());
+        // field.setRobotPose(getPose2d());
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        Status status = new Status();
+
+        return status;
+    }
+    
+}

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

@@ -35,13 +35,13 @@ public class CanDevice {
         System.out.println(getName() + " - " + str);
     }
 
-    public Status queryStatus() {
-         Status s = new Status();
+    // public Status queryStatus() {
+    //      Status s = new Status();
 
-        s.addReport(ReportLevel.INFO, "TODO");
+    //     s.addReport(ReportLevel.INFO, "TODO");
 
-        return s;
-    }
+    //     return s;
+    // }
 
     public Status diagnosticStatus() {
         Status s = new Status();