Skip to content

Commit

Permalink
rename to CustomLogger
Browse files Browse the repository at this point in the history
  • Loading branch information
00magikarp committed Jun 19, 2024
1 parent 7d6196f commit 9a46c27
Show file tree
Hide file tree
Showing 21 changed files with 76 additions and 77 deletions.
4 changes: 2 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/Robot.kt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -197,7 +197,7 @@ object Robot : LoggedRobot() {
"LoggedRobot/RemainingRamMB", Runtime.getRuntime().freeMemory() / 1024 / 1024
)

DebugLogger.recordDebugOutput(
CustomLogger.recordDebugOutput(
"LoggedRobot/totalMS", (Clock.realTimestamp - startTime).inMilliseconds
)

Expand Down
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -20,6 +20,6 @@ class DriveModuleSteeringCommand(val drivetrain: Drivetrain) : Command() {
}

override fun execute() {
DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true)
CustomLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true)
}
}
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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,
Expand Down Expand Up @@ -84,29 +83,29 @@ 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
)
}

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
)
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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 {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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
}
}
Original file line number Diff line number Diff line change
@@ -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() {
Expand All @@ -14,6 +14,6 @@ class ResetZeroCommand(val drivetrain: Drivetrain) : Command() {
}

override fun execute() {
DebugLogger.recordDebugOutput("ActiveCommands/ResetZeroCommand", true)
CustomLogger.recordDebugOutput("ActiveCommands/ResetZeroCommand", true)
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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(
Expand All @@ -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),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -28,14 +28,14 @@ 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 {
return false
}

override fun end(interrupted: Boolean) {
DebugLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", false)
CustomLogger.recordDebugOutput("ActiveCommands/TeleopDriveCommand", false)
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 {
Expand Down
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -20,6 +20,6 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() {
}

override fun execute() {
DebugLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true)
CustomLogger.recordDebugOutput("ActiveCommands/ZeroSensorsCommand", true)
}
}
Loading

0 comments on commit 9a46c27

Please sign in to comment.