mirror of
https://github.com/Team4388/2025RidgeScape.git
synced 2026-06-09 00:38:02 -06:00
Slightly overhaul status system
This commit is contained in:
Michael Mikovsky
@@ -7,10 +7,6 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Base64;
|
||||
import java.util.List;
|
||||
|
||||
// Advantagekit
|
||||
import org.littletonrobotics.junction.LogFileUtil;
|
||||
import org.littletonrobotics.junction.LoggedRobot;
|
||||
@@ -19,23 +15,16 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGReader;
|
||||
import org.littletonrobotics.junction.wpilog.WPILOGWriter;
|
||||
|
||||
import com.ctre.phoenix6.CANBus;
|
||||
import com.ctre.phoenix6.CANBus.CANBusStatus;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.robot.constants.BuildConstants;
|
||||
import frc4388.robot.constants.Constants;
|
||||
import frc4388.robot.constants.Constants.SimConstants;
|
||||
import frc4388.utility.DeferredBlock;
|
||||
import frc4388.utility.DeferredBlockMulti;
|
||||
import frc4388.utility.Trim;
|
||||
import frc4388.utility.compute.RobotTime;
|
||||
import frc4388.utility.status.CanDevice;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.Subsystem;
|
||||
import frc4388.utility.status.Status.Report;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
|
||||
//import frc4388.robot.subsystems.LED;
|
||||
/**
|
||||
* The VM is configured to automatically run this class, and to call the
|
||||
@@ -63,23 +52,7 @@ public class Robot extends LoggedRobot {
|
||||
|
||||
|
||||
// Create a shuffleboard update thread, that will periodically update the values on shuffleboard
|
||||
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();
|
||||
FaultReporter.startThread();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -189,71 +162,7 @@ public class Robot extends LoggedRobot {
|
||||
|
||||
@Override
|
||||
public void testInit() {
|
||||
|
||||
List<String> errors = new ArrayList<>();
|
||||
|
||||
// Subsystems header
|
||||
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWluKWl+KWliDilpfilpbilpfiloTiloTilpYgIOKWl+KWhOKWhOKWluKWl+KWliAg4paX4paW4paX4paE4paE4paW4paX4paE4paE4paE4paW4paX4paE4paE4paE4paW4paX4paWICDilpfilpYg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMICAgIOKWneKWmuKWnuKWmOKWkOKWjCAgICAg4paIICDilpDilowgICDilpDilpvilprilp7ilpzilozilpDilowgICAKIOKWneKWgOKWmuKWluKWkOKWjCDilpDilozilpDilpviloDilprilpYg4pad4paA4paa4paWICDilpDilowgIOKWneKWgOKWmuKWliAg4paIICDilpDilpviloDiloDilpjilpDilowgIOKWkOKWjCDilp3iloDilprilpYK4paX4paE4paE4pae4paY4pad4paa4paE4pae4paY4paQ4paZ4paE4pae4paY4paX4paE4paE4pae4paYICDilpDilowg4paX4paE4paE4pae4paYICDiloggIOKWkOKWmeKWhOKWhOKWluKWkOKWjCAg4paQ4paM4paX4paE4paE4pae4paY")));
|
||||
|
||||
for(int i=0;i< Subsystem.subsystems.size();i++){
|
||||
|
||||
Subsystem subsystem = Subsystem.subsystems.get(i);
|
||||
System.out.println("** Subsystem diagnostic report for " + subsystem.getName() + ":");
|
||||
Status status = subsystem.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
if(r.reportLevel == ReportLevel.ERROR)
|
||||
errors.add(subsystem.getName() + " - " + r.toString());
|
||||
subsystem.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// CAN header
|
||||
System.out.println(new String(Base64.getDecoder().decode("IOKWl+KWhOKWhOKWliDilpfiloTilpYg4paX4paWICDilpfilpYK4paQ4paMICAg4paQ4paMIOKWkOKWjOKWkOKWm+KWmuKWluKWkOKWjArilpDilowgICDilpDilpviloDilpzilozilpDilowg4pad4pac4paMCuKWneKWmuKWhOKWhOKWluKWkOKWjCDilpDilozilpDilowgIOKWkOKWjCh0KQ==")));
|
||||
|
||||
CANBus canBus = new CANBus(Constants.CANBUS_NAME);
|
||||
|
||||
CANBusStatus canInfo = canBus.getStatus();
|
||||
|
||||
System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount);
|
||||
System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);
|
||||
System.out.println("CANInfo RX Errors count - " + canInfo.REC);
|
||||
System.out.println("CANInfo TX Errors count - " + canInfo.TEC);
|
||||
System.out.println("CANInfo Transmit buffer full count - " + canInfo.TxFullCount);
|
||||
// Broken turniary operator
|
||||
ReportLevel canReportLevel = canInfo.Status.isOK() ? (canInfo.Status.isWarning() ? ReportLevel.WARNING : ReportLevel.ERROR) : ReportLevel.INFO;
|
||||
String canStatus = "CAN " + canReportLevel.name() + " - " + canInfo.Status.getName() + " (" + canInfo.Status.getDescription() + ")";
|
||||
if(canReportLevel == ReportLevel.ERROR) {
|
||||
errors.add(canStatus);
|
||||
}
|
||||
System.out.println(canStatus);
|
||||
|
||||
for(int i=0;i<CanDevice.devices.size();i++){
|
||||
|
||||
CanDevice device = CanDevice.devices.get(i);
|
||||
System.out.println("** CAN diagnostic report for " + device.name + ":");
|
||||
Status status = device.diagnosticStatus();
|
||||
|
||||
for(int a=0;a<status.reports.size();a++){
|
||||
Report r = status.reports.get(a);
|
||||
if(r.reportLevel == ReportLevel.ERROR)
|
||||
errors.add(device.getName() + " - " + r.toString());
|
||||
device.Log(r.toString());
|
||||
}
|
||||
}
|
||||
|
||||
// System.out.println("Found CAN devices: " + new DeviceFinder().Find());
|
||||
|
||||
if(errors.size() > 0) {
|
||||
// Errors header
|
||||
System.out.println(new String(Base64.getDecoder().decode("4paX4paE4paE4paE4paW4paX4paE4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paWIOKWl+KWhOKWhOKWliAg4paX4paE4paE4paWCuKWkOKWjCAgIOKWkOKWjCDilpDilozilpDilowg4paQ4paM4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilpDilowgICAK4paQ4pab4paA4paA4paY4paQ4pab4paA4paa4paW4paQ4pab4paA4paa4paW4paQ4paMIOKWkOKWjOKWkOKWm+KWgOKWmuKWliDilp3iloDilprilpYK4paQ4paZ4paE4paE4paW4paQ4paMIOKWkOKWjOKWkOKWjCDilpDilozilp3ilpriloTilp7ilpjilpDilowg4paQ4paM4paX4paE4paE4pae4paY")));
|
||||
for(int i=0;i<errors.size(); i++){
|
||||
System.out.println(errors.get(i));
|
||||
}
|
||||
}
|
||||
|
||||
FaultReporter.printReport();
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -22,6 +22,10 @@ import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
import frc4388.robot.constants.DriveConstants;
|
||||
import frc4388.robot.subsystems.Lidar;
|
||||
import frc4388.utility.status.FaultCANCoder;
|
||||
import frc4388.utility.status.FaultPhotonCamera;
|
||||
import frc4388.utility.status.FaultPidgeon2;
|
||||
import frc4388.utility.status.FaultTalonFX;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
@@ -37,10 +41,6 @@ public class RobotMap {
|
||||
public final Lidar reefLidar = new Lidar(LiDARConstants.REEF_LIDAR_DIO_CHANNEL, "Reef");
|
||||
public final Lidar reverseLidar = new Lidar(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL, "Reverse");
|
||||
|
||||
|
||||
public RobotMap() {
|
||||
configureDriveMotorControllers();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
@@ -66,6 +66,31 @@ public class RobotMap {
|
||||
}
|
||||
|
||||
|
||||
public RobotMap() {
|
||||
configureDriveMotorControllers();
|
||||
|
||||
FaultPhotonCamera.addDevice(leftCamera, "Left Camera");
|
||||
FaultPhotonCamera.addDevice(rightCamera, "Right Camera");
|
||||
|
||||
FaultPidgeon2.addDevice(swerveDrivetrain.getPigeon2(), "Gyro");
|
||||
|
||||
FaultTalonFX.addDevice(elevator, "Elevator");
|
||||
FaultTalonFX.addDevice(endeffector, "Endeffector");
|
||||
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getDriveMotor(), "Module 0 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(0).getSteerMotor(), "Module 0 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrain.getModule(0).getEncoder(), "Module 0 CANCoder");
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getDriveMotor(), "Module 1 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(1).getSteerMotor(), "Module 1 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrain.getModule(1).getEncoder(), "Module 1 CANCoder");
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getDriveMotor(), "Module 2 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(2).getSteerMotor(), "Module 2 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrain.getModule(2).getEncoder(), "Module 2 CANCoder");
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getDriveMotor(), "Module 3 Drive");
|
||||
FaultTalonFX.addDevice(swerveDrivetrain.getModule(3).getSteerMotor(), "Module 3 Steer");
|
||||
FaultCANCoder.addDevice(swerveDrivetrain.getModule(3).getEncoder(), "Module 3 CANCoder");
|
||||
}
|
||||
|
||||
public class RobotMapSim {
|
||||
public PhotonCameraSim leftCamera;
|
||||
public PhotonCameraSim rightCamera;
|
||||
|
||||
@@ -7,12 +7,12 @@ public final class BuildConstants {
|
||||
public static final String MAVEN_GROUP = "";
|
||||
public static final String MAVEN_NAME = "2025RidgeScape";
|
||||
public static final String VERSION = "unspecified";
|
||||
public static final int GIT_REVISION = 164;
|
||||
public static final String GIT_SHA = "06d525ade1118ae193383bc86066ea5563d86a1f";
|
||||
public static final String GIT_DATE = "2025-04-22 18:21:00 EDT";
|
||||
public static final int GIT_REVISION = 165;
|
||||
public static final String GIT_SHA = "bf4da44338d8e91c50e34c3ac68dc12f9d335b08";
|
||||
public static final String GIT_DATE = "2025-07-11 14:07:53 MDT";
|
||||
public static final String GIT_BRANCH = "advantagekit";
|
||||
public static final String BUILD_DATE = "2025-07-11 15:07:31 EDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752260851324L;
|
||||
public static final String BUILD_DATE = "2025-07-11 17:46:28 MDT";
|
||||
public static final long BUILD_UNIX_TIME = 1752277588371L;
|
||||
public static final int DIRTY = 1;
|
||||
|
||||
private BuildConstants(){}
|
||||
|
||||
@@ -243,5 +243,4 @@ public final class DriveConstants {
|
||||
// misc
|
||||
public static final int TIMEOUT_MS = 30;
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
|
||||
}
|
||||
|
||||
@@ -12,16 +12,18 @@ 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.compute.RobotTime;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.Subsystem;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class DiffDrive extends Subsystem {
|
||||
public class DiffDrive extends SubsystemBase implements Queryable {
|
||||
// Put methods for controlling this subsystem
|
||||
// here. Call these from Commands.
|
||||
|
||||
@@ -40,7 +42,7 @@ public class DiffDrive extends Subsystem {
|
||||
public DiffDrive(TalonFX leftFrontMotor, TalonFX rightFrontMotor, TalonFX leftBackMotor,
|
||||
TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
|
||||
|
||||
super();
|
||||
FaultReporter.register(this);
|
||||
|
||||
m_leftFrontMotor = leftFrontMotor;
|
||||
m_rightFrontMotor = rightFrontMotor;
|
||||
@@ -90,7 +92,7 @@ public class DiffDrive extends Subsystem {
|
||||
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
public String getName() {
|
||||
return "Diff Drive";
|
||||
}
|
||||
|
||||
@@ -101,7 +103,7 @@ public class DiffDrive extends Subsystem {
|
||||
|
||||
@Override
|
||||
public Status diagnosticStatus() {
|
||||
Log("Diagnostic info for this has not been inplemented!"); //TODO
|
||||
// Log("Diagnostic info for this has not been inplemented!"); //TODO
|
||||
return new Status();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,14 +10,16 @@ import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.AutoConstants;
|
||||
import frc4388.robot.constants.Constants.ElevatorConstants;
|
||||
import frc4388.robot.constants.Constants.LEDConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.Subsystem;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
public class Elevator extends Subsystem {
|
||||
public class Elevator extends SubsystemBase implements Queryable {
|
||||
/** Creates a new Elevator. */
|
||||
private TalonFX elevatorMotor;
|
||||
private TalonFX endeffectorMotor;
|
||||
@@ -77,6 +79,8 @@ public class Elevator extends Subsystem {
|
||||
elevatorMotor.getConfigurator().apply(ElevatorConstants.ELEVATOR_PID);
|
||||
endeffectorMotor.getConfigurator().apply(ElevatorConstants.ENDEFFECTOR_PID);
|
||||
currentState = CoordinationState.Ready;
|
||||
|
||||
FaultReporter.register(this);
|
||||
}
|
||||
|
||||
//PID methods
|
||||
@@ -370,7 +374,7 @@ public class Elevator extends Subsystem {
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
public String getName() {
|
||||
return "Elevator";
|
||||
}
|
||||
|
||||
@@ -382,8 +386,6 @@ public class Elevator extends Subsystem {
|
||||
Status status = new Status();
|
||||
|
||||
status.addReport(ReportLevel.INFO, "Elevator Mode: " + currentState.name());
|
||||
status.diagnoseHardwareCTRE("Elevator Motor", elevatorMotor);
|
||||
status.diagnoseHardwareCTRE("Endeffector Motor", endeffectorMotor);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
@@ -10,9 +10,11 @@ package frc4388.robot.subsystems;
|
||||
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.Constants.LEDConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.Subsystem;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
import frc4388.utility.structs.LEDPatterns;
|
||||
|
||||
@@ -20,7 +22,10 @@ import frc4388.utility.structs.LEDPatterns;
|
||||
* Allows for the control of a 5v LED Strip using a Rev Robotics Blinkin LED
|
||||
* Driver
|
||||
*/
|
||||
public class LED extends Subsystem {
|
||||
public class LED extends SubsystemBase implements Queryable {
|
||||
public LED() {
|
||||
FaultReporter.register(this);
|
||||
}
|
||||
|
||||
private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
|
||||
@@ -44,7 +49,7 @@ public class LED extends Subsystem {
|
||||
}
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
public String getName() {
|
||||
return "LEDs";
|
||||
}
|
||||
|
||||
@@ -57,9 +62,7 @@ public class LED extends Subsystem {
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
if(LEDController.isAlive())
|
||||
status.addReport(ReportLevel.INFO, "LED is CONNECTED");
|
||||
else
|
||||
if(!LEDController.isAlive())
|
||||
status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
|
||||
|
||||
status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
|
||||
|
||||
@@ -6,19 +6,23 @@ 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.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.LiDARConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.Subsystem;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
|
||||
// https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
|
||||
public class Lidar extends Subsystem {
|
||||
public class Lidar extends SubsystemBase implements Queryable {
|
||||
|
||||
private Counter LidarPWM;
|
||||
private String name = "Lidar";
|
||||
|
||||
private double distance = -1;
|
||||
public Lidar(int port, String name) {
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.name = name;
|
||||
LidarPWM = new Counter(port);
|
||||
LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
|
||||
@@ -27,7 +31,7 @@ public class Lidar extends Subsystem {
|
||||
|
||||
|
||||
subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.getLayout(getName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
sbDistance = subsystemLayout
|
||||
@@ -63,7 +67,7 @@ public class Lidar extends Subsystem {
|
||||
GenericEntry sbWithinDistance;
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
public String getName() {
|
||||
return "Lidar " + name;
|
||||
}
|
||||
|
||||
@@ -77,12 +81,9 @@ public class Lidar extends Subsystem {
|
||||
public Status diagnosticStatus() {
|
||||
Status s = new Status();
|
||||
|
||||
if(distance == -1){
|
||||
if(distance == -1)
|
||||
s.addReport(ReportLevel.ERROR, "LiDAR DISCONNECTED");
|
||||
}else{
|
||||
s.addReport(ReportLevel.INFO, "LiDAR Connected");
|
||||
}
|
||||
|
||||
|
||||
s.addReport(ReportLevel.INFO, "LiDAR Distance: " + distance);
|
||||
|
||||
return s;
|
||||
|
||||
@@ -25,11 +25,12 @@ 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.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.DriveConstants;
|
||||
import frc4388.utility.compute.TimesNegativeOne;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.Subsystem;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
import com.pathplanner.lib.controllers.PPHolonomicDriveController;
|
||||
import com.pathplanner.lib.util.PathPlannerLogging;
|
||||
@@ -37,7 +38,7 @@ import com.pathplanner.lib.config.PIDConstants;
|
||||
import com.pathplanner.lib.auto.AutoBuilder;
|
||||
import com.pathplanner.lib.config.RobotConfig;
|
||||
|
||||
public class SwerveDrive extends Subsystem {
|
||||
public class SwerveDrive extends SubsystemBase implements Queryable {
|
||||
private SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
|
||||
|
||||
private Vision vision;
|
||||
@@ -65,7 +66,7 @@ public class SwerveDrive extends Subsystem {
|
||||
public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain, Vision vision) {
|
||||
// public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
|
||||
// swerveDriveTrain) {
|
||||
super();
|
||||
FaultReporter.register(this);
|
||||
|
||||
this.swerveDriveTrain = swerveDriveTrain;
|
||||
this.vision = vision;
|
||||
@@ -448,12 +449,12 @@ public class SwerveDrive extends Subsystem {
|
||||
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
public String getName() {
|
||||
return "Swerve Drive Controller";
|
||||
}
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.getLayout(getName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
GenericEntry sbGyro = subsystemLayout
|
||||
@@ -478,8 +479,8 @@ public class SwerveDrive extends Subsystem {
|
||||
public Status diagnosticStatus() {
|
||||
Status status = new Status();
|
||||
|
||||
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.");
|
||||
// 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;
|
||||
}
|
||||
|
||||
@@ -27,13 +27,14 @@ 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 edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.constants.Constants.FieldConstants;
|
||||
import frc4388.robot.constants.Constants.VisionConstants;
|
||||
import frc4388.utility.status.Status;
|
||||
import frc4388.utility.status.Subsystem;
|
||||
import frc4388.utility.status.Status.ReportLevel;
|
||||
import frc4388.utility.status.FaultReporter;
|
||||
import frc4388.utility.status.Queryable;
|
||||
|
||||
public class Vision extends Subsystem {
|
||||
public class Vision extends SubsystemBase implements Queryable {
|
||||
|
||||
// private PhotonCamera leftCamera;
|
||||
// private PhotonCamera rightCamera;
|
||||
@@ -59,7 +60,7 @@ public class Vision extends Subsystem {
|
||||
private Field2d field = new Field2d();
|
||||
|
||||
ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems")
|
||||
.getLayout(getSubsystemName(), BuiltInLayouts.kList)
|
||||
.getLayout(getName(), BuiltInLayouts.kList)
|
||||
.withSize(2, 2);
|
||||
|
||||
GenericEntry sbTagDetected = subsystemLayout
|
||||
@@ -72,17 +73,8 @@ public class Vision extends Subsystem {
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbLeftCamConnected = subsystemLayout
|
||||
.add("Left Camera Connnected", false)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
|
||||
GenericEntry sbRightCamConnected = subsystemLayout
|
||||
.add("Right Camera Connnected", false)
|
||||
.withWidget(BuiltInWidgets.kBooleanBox)
|
||||
.getEntry();
|
||||
|
||||
public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){
|
||||
FaultReporter.register(this);
|
||||
SmartDashboard.putData(field);
|
||||
|
||||
this.cameras = new PhotonCamera[]{leftCamera, rightCamera};
|
||||
@@ -307,7 +299,7 @@ public class Vision extends Subsystem {
|
||||
|
||||
|
||||
@Override
|
||||
public String getSubsystemName() {
|
||||
public String getName() {
|
||||
return "Vision";
|
||||
}
|
||||
|
||||
@@ -321,27 +313,12 @@ public class Vision extends Subsystem {
|
||||
public void queryStatus() {
|
||||
sbTagDetected.setBoolean(isTagDetected);
|
||||
sbTagProcessed.setBoolean(isTagProcessed);
|
||||
sbLeftCamConnected.setBoolean(cameras[0].isConnected());
|
||||
sbRightCamConnected.setBoolean(cameras[1].isConnected());
|
||||
// field.setRobotPose(getPose2d());
|
||||
}
|
||||
|
||||
@Override
|
||||
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;
|
||||
return new Status();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user