diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index e2db1832..2ec17495 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -8,7 +8,7 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.Alert.AlertType -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.NTSafePublisher import edu.wpi.first.hal.AllianceStationID @@ -197,7 +197,7 @@ object Robot : LoggedRobot() { "LoggedRobot/RemainingRamMB", Runtime.getRuntime().freeMemory() / 1024 / 1024 ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds ) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt index afefc58c..674d20a5 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/DriveModuleSteeringCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -20,6 +20,6 @@ class DriveModuleSteeringCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt index b73b6c7b..a75c01e7 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.commands -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command @@ -108,7 +108,7 @@ class SysIdCommand : Command { data += (subsystemData.velRadPerSec / (2 * Math.PI)).toString() + "," } - DebugLogger.recordDebugOutput("ActiveCommands/SysIdCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/SysIdCommand", true) } // Called once the command ends or is interrupted. diff --git a/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt index 67f089a5..9fe7de30 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/characterization/WheelRadiusCharacterizationCommand.kt @@ -9,7 +9,7 @@ import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.math.MathUtil import edu.wpi.first.math.filter.SlewRateLimiter import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -22,7 +22,6 @@ import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.rotations import kotlin.math.hypot -import kotlin.time.times class WheelRadiusCharacterizationCommand( val drivetrain: Drivetrain, @@ -84,16 +83,16 @@ class WheelRadiusCharacterizationCommand( averageWheelPositionDelta /= 4.0 currentEffectiveWheelRadius = (accumGyroYawRads * driveRadius / averageWheelPositionDelta).meters - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/DrivePosition", averageWheelPositionDelta.inRadians ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/AccumGyroYawRads", accumGyroYawRads.inRadians ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/LastGyroYawRads", lastGyroYawRads.inRadians ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/RadiusCharacterization/CurrentWheelRadiusInches", currentEffectiveWheelRadius.inInches ) @@ -101,12 +100,12 @@ class WheelRadiusCharacterizationCommand( override fun end(interrupted: Boolean) { if (accumGyroYawRads <= (Math.PI * 2.0).radians) { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/radiansOffFromWheelRadius", ((Math.PI * 2.0).radians - accumGyroYawRads).inRadians ) } else { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Drivetrain/effectiveWheelRadius", currentEffectiveWheelRadius.inInches ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 7549882c..34487fba 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -11,7 +11,7 @@ import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.FrameType import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -384,7 +384,7 @@ private constructor( Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference()) Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds) - DebugLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", true) if (thetakP.hasChanged()) thetaPID.proportionalGain = thetakP.get() if (thetakI.hasChanged()) thetaPID.integralGain = thetakI.get() @@ -414,7 +414,7 @@ private constructor( } override fun end(interrupted: Boolean) { - DebugLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { DriverStation.reportError(errorString, true) // Stop where we are if interrupted diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt index d3545ff7..e0c24fbc 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -17,7 +17,7 @@ class ResetGyroYawCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.d } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ResetGyroYawCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ResetGyroYawCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index b8e35831..000cc679 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -2,7 +2,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.geometry.Pose2d @@ -14,12 +14,12 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() override fun initialize() { drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose)) drivetrain.zeroGyroYaw(AllianceFlipUtil.apply(pose).rotation) - DebugLogger.recordDebugOutput("Drivetrain/lastResetPose", AllianceFlipUtil.apply(pose).pose2d) - DebugLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", true) + CustomLogger.recordDebugOutput("Drivetrain/lastResetPose", AllianceFlipUtil.apply(pose).pose2d) + CustomLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", true) } override fun isFinished(): Boolean { - DebugLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/ResetPoseCommand", false) return true } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt index 8d2ddce4..acd806b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { @@ -14,6 +14,6 @@ class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ResetZeroCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ResetZeroCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt index 5ac1cd55..3aaf5ed1 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetAngleCommand.kt @@ -4,7 +4,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command @@ -109,14 +109,14 @@ class TargetAngleCommand( override fun execute() { drivetrain.defaultCommand.end(true) - DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", true) Logger.recordOutput( "Testing/CurrentDrivetrainRotation", drivetrain.odomTRobot.rotation.inDegrees ) val thetaFeedback = thetaPID.calculate(drivetrain.odomTRobot.rotation, targetAngle()) - DebugLogger.recordDebugOutput("Testing/error", thetaPID.error.inDegrees) - DebugLogger.recordDebugOutput("Testing/thetaFeedback", thetaFeedback.inDegreesPerSecond) + CustomLogger.recordDebugOutput("Testing/error", thetaPID.error.inDegrees) + CustomLogger.recordDebugOutput("Testing/thetaFeedback", thetaFeedback.inDegreesPerSecond) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( @@ -131,7 +131,7 @@ class TargetAngleCommand( } override fun end(interrupted: Boolean) { - DebugLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/TargetAngleCommand", false) drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop( driver.rotationSpeedClampedSupplier(turn, slowMode), diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt index 6a3a9673..7cf3e7c6 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt @@ -6,7 +6,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.feeder.Feeder import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase @@ -120,8 +120,8 @@ class TargetNoteCommand( Logger.recordOutput("ActiveCommands/TargetNoteCommand", true) val thetaFeedback = thetaPID.calculate(limelight.targetGamePieceTx ?: 0.0.degrees, 0.0.degrees) - DebugLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees) - DebugLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond) + CustomLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees) + CustomLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond) if (!feeder.hasNote && limelight.inputs.gamePieceTargets.size > 0) { val driveVector = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 0ce65a00..6e068e01 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command @@ -28,7 +28,7 @@ class TeleopDriveCommand( val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) - DebugLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", true) } } override fun isFinished(): Boolean { @@ -36,6 +36,6 @@ class TeleopDriveCommand( } override fun end(interrupted: Boolean) { - DebugLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", false) + CustomLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", false) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt index 7ab857e6..111242dc 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TestDriveCommand.kt @@ -2,7 +2,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.derived.inRotation2ds @@ -16,7 +16,7 @@ class TestDriveCommand(val drivetrain: Drivetrain) : Command() { 4.0, 0.0, 0.0, drivetrain.odomTRobot.rotation.inRotation2ds ) ) - DebugLogger.recordDebugOutput("ActiveCommands/TestDriveCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/TestDriveCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index 4bdabc03..ff3178e1 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,7 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -20,6 +20,6 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) + CustomLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt index 2f88041b..a3e2cf97 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/MotorChecker.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.subsystems.falconspin -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj.DriverStation import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inAmperes @@ -27,7 +27,7 @@ object MotorChecker { } fun periodic() { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/subsystemNames", subsystemHardware.keys.toTypedArray() ) @@ -69,7 +69,7 @@ object MotorChecker { } } } - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/motorNames", motorNames.toTypedArray() ) @@ -89,57 +89,57 @@ object MotorChecker { // not clean but whatever fun logMotor(subsystemName: String, motor: Motor) { - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/AppliedVoltageVolts", motor.appliedVoltage.inVolts ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/BusVoltageVolts", motor.busVoltage.inVolts ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/TemperatureCelsius", motor.temperature.inCelsius ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/StatorCurrentAmps", motor.statorCurrent.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/SupplyCurrentAmps", motor.supplyCurrent.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/CurrentLimitStage", motor.currentLimitStage.name ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/BaseCurrentLimitAmps", motor.baseCurrentLimit.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/FirstStageTemperatureLimitCelsius", motor.firstStageTemperatureLimit.inCelsius ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/FirstStageCurrentLimitAmps", motor.firstStageCurrentLimit.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/MotorShutDownThresholdCelsius", motor.motorShutDownThreshold.inCelsius ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/CurrentLimitInUseAmps", motor.currentLimitInUse.inAmperes ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/MotorID", motor.id.toLong() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Errors", motor.errors.toTypedArray() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Warnings", motor.warnings.toTypedArray() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/Info", motor.info.toTypedArray() ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "MotorChecker/$subsystemName/${motor.name}/StickyFaults", motor.stickyFaults.toTypedArray() ) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index de7268fb..1712ac7a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -5,7 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.math.filter.Debouncer import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -146,7 +146,7 @@ class Feeder(val io: FeederIO) : SubsystemBase() { Logger.recordOutput( "Feeder/isAtCommandedState", currentState.equivalentToRequest(currentRequest) ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Feeder/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds ) Logger.recordOutput("Feeder/feederVoltageTarget", feederTargetVoltage.inVolts) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt index 7c9101b9..a2b1030f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt @@ -19,7 +19,7 @@ import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import org.littletonrobotics.junction.Logger import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity @@ -200,10 +200,10 @@ object FlywheelIOTalon : FlywheelIO { override fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) { val error = velocity - flywheelLeftSensor.velocity - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Flywheel/premotionTargetedVelocity", velocity.inRotationsPerMinute ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Flywheel/motionTargetedVelocity", flywheelLeftSensor.velocityToRawUnits(velocity) ) // flywheelRightTalon.setControl( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 015ae4ef..04bbbd9e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -5,7 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -109,7 +109,7 @@ class Intake(val io: IntakeIO) : SubsystemBase() { Logger.recordOutput( "Intake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) ) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput( "Intake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds ) Logger.recordOutput("Intake/rollerVoltageTarget", rollerVoltageTarget.inVolts) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index 9ea656ea..eaf5f4cb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -2,7 +2,7 @@ package com.team4099.robot2023.subsystems.led import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.LEDConstants -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import com.team4099.robot2023.util.FMSData import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotController @@ -71,6 +71,6 @@ class Leds(val io: LedIO) { } Logger.processInputs("LED", inputs) - DebugLogger.recordDebugOutput("LED/state", state.name) + CustomLogger.recordDebugOutput("LED/state", state.name) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 69d11d8a..c2f50ed1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -5,7 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request -import com.team4099.robot2023.util.DebugLogger +import com.team4099.robot2023.util.CustomLogger import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands @@ -417,7 +417,7 @@ class Wrist(val io: WristIO) : SubsystemBase() { } } - DebugLogger.recordDebugOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) + CustomLogger.recordDebugOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) Logger.recordOutput("Wrist/armFeedForward", feedforward.inVolts) Logger.recordOutput("Wrist/armTargetPosition", setPoint.position.inDegrees) Logger.recordOutput("Wrist/armTargetVelocity", setPoint.velocity.inDegreesPerSecond) diff --git a/src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt b/src/main/kotlin/com/team4099/robot2023/util/CustomLogger.kt similarity index 61% rename from src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt rename to src/main/kotlin/com/team4099/robot2023/util/CustomLogger.kt index 35e41368..e52956c4 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/DebugLogger.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/CustomLogger.kt @@ -7,9 +7,90 @@ import edu.wpi.first.util.struct.Struct import edu.wpi.first.util.struct.StructSerializable import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d import org.littletonrobotics.junction.Logger +import org.littletonrobotics.junction.inputs.LoggableInputs -class DebugLogger { +class CustomLogger { companion object { + inline fun processInputs(key: String, inputs: LoggableInputs) { + Logger.processInputs(key, inputs) + } + + inline fun > recordOutput(key: String, value: E) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: T) { + Logger.recordOutput(key, value) + } + + inline fun > recordOutput(key: String, value: Measure) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Struct) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, vararg value: T) { + Logger.recordOutput(key, *value) + } + + inline fun recordOutput(key: String, value: Mechanism2d) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Array) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: BooleanArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Boolean) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: ByteArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: DoubleArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Double) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Float) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: FloatArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Int) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: IntArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: Long) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: LongArray) { + Logger.recordOutput(key, value) + } + + inline fun recordOutput(key: String, value: String) { + Logger.recordOutput(key, value) + } + inline fun > recordDebugOutput(key: String, value: E) { if (Constants.Tuning.DEBUGING_MODE) { Logger.recordOutput(key, value) diff --git a/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt index d1ce504c..37a2ec71 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/NoteSimulation.kt @@ -110,14 +110,14 @@ class NoteSimulation( var flywheelAngularVelocitySupplier: () -> AngularVelocity = { 0.0.rotations.perMinute } fun periodic() { - DebugLogger.recordDebugOutput("NoteData/$id/inclinationAngle", inclinationAngleTheta.inDegrees) - DebugLogger.recordDebugOutput("NoteData/$id/azimuthalAngle", azimuthalAnglePhi.inDegrees) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput("NoteData/$id/inclinationAngle", inclinationAngleTheta.inDegrees) + CustomLogger.recordDebugOutput("NoteData/$id/azimuthalAngle", azimuthalAnglePhi.inDegrees) + CustomLogger.recordDebugOutput( "NoteData/$id/launchVelocityMetersPerSecond", launchVelocity.inMetersPerSecond ) - DebugLogger.recordDebugOutput("NoteData/$id/startPose", launchStartPose.pose3d) - DebugLogger.recordDebugOutput("NoteData/$id/state", currentState.name) - DebugLogger.recordDebugOutput( + CustomLogger.recordDebugOutput("NoteData/$id/startPose", launchStartPose.pose3d) + CustomLogger.recordDebugOutput("NoteData/$id/state", currentState.name) + CustomLogger.recordDebugOutput( "NoteSimulation/IntakePose", poseSupplier().transformBy(IntakeConstants.INTAKE_TRANSFORM).pose2d ) diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index e63a4888..8f8e893e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.7", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.7", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.7", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.7" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.7" + "version": "v2024.3.1" } ] } \ No newline at end of file