diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 15c07a8..28f2924 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -357,7 +357,6 @@ public final class Constants { public static final int L4_DRIVE_TIME = 250; //Milliseconds // public static final int L3_DRIVE_TIME = 500; public static final int L2_DRIVE_TIME = 250; //Milliseconds - public static final int ALGAE_DRIVE_TIME = 500; } @@ -372,7 +371,8 @@ public final class Constants { // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate - // (Fake values. Experiment and determine estimation noise on an actual robot.) + // X, Y, Theta + // https://www.chiefdelphi.com/t/photonvision-finding-standard-deviations-for-swervedriveposeestimator/467802/2 public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2377656..092de15 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -10,7 +10,6 @@ package frc4388.robot; import java.util.ArrayList; import java.util.Base64; import java.util.List; -import java.util.logging.Level; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; @@ -87,9 +86,9 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - SmartDashboard.putNumber("Time", System.currentTimeMillis()); - //System.out.println(m_robotContainer.limelight.isNearSpeaker()); - //mled.updateLED(); + // SmartDashboard.putNumber("Time", System.currentTimeMillis()); + + m_robotContainer.m_robotLED.update(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5dcbe54..39cb625 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -84,7 +84,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final LED m_robotLED = new LED(); + public final LED m_robotLED = new LED(); public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED); diff --git a/src/main/java/frc4388/robot/subsystems/DiffDrive.java b/src/main/java/frc4388/robot/subsystems/DiffDrive.java index 0cffe0b..a95c0a7 100644 --- a/src/main/java/frc4388/robot/subsystems/DiffDrive.java +++ b/src/main/java/frc4388/robot/subsystems/DiffDrive.java @@ -7,21 +7,17 @@ package frc4388.robot.subsystems; -import java.util.logging.Level; - import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -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. diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index 388dc91..0389d90 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -23,9 +23,12 @@ import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.AutoConstants; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; import frc4388.utility.TimesNegativeOne; +import frc4388.utility.Status.ReportLevel; -public class Elevator extends SubsystemBase { +public class Elevator extends Subsystem { /** Creates a new Elevator. */ private TalonFX elevatorMotor; private TalonFX endeffectorMotor; @@ -303,4 +306,23 @@ public class Elevator extends SubsystemBase { //shuffle the coral with the arm until coral hits beam break } } + + @Override + public String getSubsystemName() { + return "Elevator"; + } + + @Override + public void queryStatus() {} + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name()); + status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor); + status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor); + + return status; + } } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 41fa708..e85100f 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -9,29 +9,67 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.LEDConstants; import frc4388.utility.LEDPatterns; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; /** * Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED * Driver */ -public class LED extends SubsystemBase { +public class LED extends Subsystem { private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - private double mode = LEDConstants.DEFAULT_PATTERN.getValue(); + private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; public void setMode(LEDPatterns pattern){ - this.mode = pattern.getValue(); + this.mode = pattern; } @Override public void periodic() { - LEDController.set(mode); + update(); } + public void update() { + if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return; + + if(DriverStation.isDisabled()){ + LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue()); + }else + LEDController.set(mode.getValue()); + } + + @Override + public String getSubsystemName() { + return "LEDs"; + } + + @Override + public void queryStatus() { + SmartDashboard.putString("LED status", mode.name()); + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + if(LEDController.isAlive()) + status.addReport(ReportLevel.INFO, "LED is CONNECTED"); + else + status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED"); + + status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name()); + + return status; + } + + } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Lidar.java b/src/main/java/frc4388/robot/subsystems/Lidar.java index b903e17..3ac5cff 100644 --- a/src/main/java/frc4388/robot/subsystems/Lidar.java +++ b/src/main/java/frc4388/robot/subsystems/Lidar.java @@ -69,11 +69,15 @@ public class Lidar extends Subsystem { @Override public Status diagnosticStatus() { Status s = new Status(); + if(distance == -1){ - s.addReport(ReportLevel.ERROR, "LIDAR DISCONNECTED"); + s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED"); }else{ - s.addReport(ReportLevel.INFO, "LIDAR CONNECTED"); + s.addReport(ReportLevel.INFO, "LiDAR Connected"); } + + s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance); + return s; } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index eff4e3c..a790bb6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -423,7 +423,7 @@ public class SwerveDrive extends Subsystem { public Status diagnosticStatus() { Status status = new Status(); - status.addReport(ReportLevel.ERROR, + status.addReport(ReportLevel.INFO, "Don't know how to diganose new CTRE swerve systems. please check under the CAN(t) section for more detailed information about the swerves there."); return status; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index a66a00a..ed5ebe9 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -36,6 +36,7 @@ import frc4388.robot.Constants.FieldConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.utility.Status; import frc4388.utility.Subsystem; +import frc4388.utility.Status.ReportLevel; public class Vision extends Subsystem { @@ -311,6 +312,17 @@ public class Vision extends Subsystem { public Status diagnosticStatus() { Status status = new Status(); + if(cameras[0].isConnected()) + status.addReport(ReportLevel.INFO, "Left Camera Connected"); + else + status.addReport(ReportLevel.ERROR, "Left Camera DISCONNECTED"); + + if(cameras[1].isConnected()) + status.addReport(ReportLevel.INFO, "Right Camera Connected"); + else + status.addReport(ReportLevel.ERROR, "Right Camera DISCONNECTED"); + + return status; }