diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fa5eebc..ce41b7b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import frc.robot.subsystems.drive.ModuleIOTalonFX; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.intakeAndLauncher.IntakeAndLauncher; +import frc.robot.util.CANHealthLogger; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -42,13 +43,16 @@ public class RobotContainer { private final Drive drive; private final IntakeAndLauncher intakeAndShooter; private final Feeder feederAndIndexer; - private final AutoFactory autoFactory; - public final AutoChooser autoChooser; // Controller private final CommandXboxController controller = new CommandXboxController(0); private static boolean hasRanAuto = false; + // Utility + private final AutoFactory autoFactory; + public final AutoChooser autoChooser; + private final CANHealthLogger canLogger; + // Dashboard inputs /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -114,6 +118,7 @@ public RobotContainer() { this.feederAndIndexer = new Feeder(); break; } + this.canLogger = new CANHealthLogger(); // Initialize ChoreoLib AutoFactory autoFactory = diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 5dc0241..5052e88 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -18,6 +18,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import frc.robot.generated.TunerConstants; import java.util.Queue; +import org.littletonrobotics.junction.Logger; /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { @@ -45,10 +46,14 @@ public GyroIOPigeon2() { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + var status = BaseStatusSignal.refreshAll(yaw, yawVelocity); + inputs.connected = status.equals(StatusCode.OK); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); + // Log CAN Bus Health + Logger.recordOutput("CAN/DeviceStatus/Pigeons/" + pigeon.getDeviceID(), status); + inputs.odometryYawTimestamps = yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.odometryYawPositions = diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 606eb54..a607c66 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -36,6 +36,7 @@ import edu.wpi.first.units.measure.Voltage; import frc.robot.generated.TunerConstants; import java.util.Queue; +import org.littletonrobotics.junction.Logger; /** * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and @@ -208,6 +209,11 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = turnCurrent.getValueAsDouble(); + // Log CAN Bus Health + Logger.recordOutput("CAN/DeviceStatus/Talons/" + driveTalon.getDeviceID(), driveStatus); + Logger.recordOutput("CAN/DeviceStatus/Talons/" + turnTalon.getDeviceID(), turnStatus); + Logger.recordOutput("CAN/DeviceStatus/CANCoders/" + cancoder.getDeviceID(), turnEncoderStatus); + // Update odometry inputs inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 721f556..65cde78 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -1,16 +1,25 @@ package frc.robot.subsystems.feeder; +import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; public class Feeder extends SubsystemBase { private double commandedrpm; private SparkMax motorController = new SparkMax(2, MotorType.kBrushed); - //TODO: needs current limits + // TODO: needs current limits + + @Override + public void periodic() { + // Log CAN Bus Health + REVLibError err = motorController.getLastError(); + Logger.recordOutput("CAN/DeviceStatus/Sparks/" + motorController.getDeviceId(), err); + } public void setRPM(double launcherRPMset) { commandedrpm = launcherRPMset; diff --git a/src/main/java/frc/robot/subsystems/intakeAndLauncher/IntakeAndLauncher.java b/src/main/java/frc/robot/subsystems/intakeAndLauncher/IntakeAndLauncher.java index 16ed6a8..867a96a 100644 --- a/src/main/java/frc/robot/subsystems/intakeAndLauncher/IntakeAndLauncher.java +++ b/src/main/java/frc/robot/subsystems/intakeAndLauncher/IntakeAndLauncher.java @@ -1,11 +1,13 @@ package frc.robot.subsystems.intakeAndLauncher; +import com.revrobotics.REVLibError; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkMax; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; public class IntakeAndLauncher extends SubsystemBase { private SparkMax motorController = new SparkMax(1, MotorType.kBrushed); @@ -13,6 +15,13 @@ public class IntakeAndLauncher extends SubsystemBase { private boolean state = false; private double trackedvoltage = 0; + @Override + public void periodic() { + // Log CAN Bus Health + REVLibError err = motorController.getLastError(); + Logger.recordOutput("CAN/DeviceStatus/Sparks/" + motorController.getDeviceId(), err); + } + public void turnon() { state = true; } diff --git a/src/main/java/frc/robot/util/CANHealthLogger.java b/src/main/java/frc/robot/util/CANHealthLogger.java new file mode 100644 index 0000000..df857ea --- /dev/null +++ b/src/main/java/frc/robot/util/CANHealthLogger.java @@ -0,0 +1,29 @@ +package frc.robot.util; + +import com.ctre.phoenix6.CANBus; +import edu.wpi.first.hal.can.CANStatus; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.Logger; + +public class CANHealthLogger extends SubsystemBase { + + @Override + public void periodic() { + CANStatus status = RobotController.getCANStatus(); + Logger.recordOutput("CAN/RIO/recieveErrorCount", status.receiveErrorCount); + Logger.recordOutput("CAN/RIO/transmitErrorCount", status.transmitErrorCount); + Logger.recordOutput("CAN/RIO/txFullCount", status.txFullCount); + Logger.recordOutput("CAN/RIO/busOffCount", status.busOffCount); + Logger.recordOutput("CAN/RIO/busUtilization", status.percentBusUtilization); + // Battery voltage can be useful when debugging intermittent dropouts + Logger.recordOutput("CAN/RIO/batteryVoltage", RobotController.getBatteryVoltage()); + + CANBus.CANBusStatus ctreStatus = CANBus.roboRIO().getStatus(); + Logger.recordOutput("CAN/RIOfromCTRE/recieveErrorCount", ctreStatus.REC); + Logger.recordOutput("CAN/RIOfromCTRE/transmitErrorCount", ctreStatus.TEC); + Logger.recordOutput("CAN/RIOfromCTRE/txFullCount", ctreStatus.TxFullCount); + Logger.recordOutput("CAN/RIOfromCTRE/busOffCount", ctreStatus.BusOffCount); + Logger.recordOutput("CAN/RIOfromCTRE/busUtilization", ctreStatus.BusUtilization); + } +}