Improve Logging with Monologue

.gitignore

@@ -175,3 +175,5 @@ src/main/java/org/jmhsrobotics/frc2024/BuildConstants.java
 *.wpilog
 simgui-ds.json
 simgui.json
+networktables.json
+networktables.json.bck

build.gradle

@@ -56,7 +56,7 @@ def includeDesktopSupport = false
 // Also defines JUnit 5.
 dependencies {
     implementation 'com.github.FRC-Team-620:WarCore:v1.2'
-    implementation 'com.github.shueja:Monologue:v1.0.0-beta4'
+    implementation 'com.github.shueja:Monologue:v1.0.0-beta5'
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 

src/main/java/org/jmhsrobotics/frc2024/Robot.java

@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import monologue.Logged;
+import monologue.Monologue;
 
 public class Robot extends TimedRobot implements Logged {
 	private Command autonomousCommand;
@@ -38,13 +39,13 @@ public void setupLogs() {
 		BuildDataLogger.LogToNetworkTables(BuildConstants.class);
 		BuildDataLogger.LogToWpiLib(DataLogManager.getLog(), BuildConstants.class);
 		boolean fileOnly = false;
-		boolean lazyLogging = false;
-		// Monologue.setupMonologue(this, "Robot", fileOnly, lazyLogging);
+		boolean lazyLogging = true;
+		Monologue.setupMonologue(this, "Robot", fileOnly, lazyLogging);
 	}
 
 	@Override
 	public void robotPeriodic() {
-		// Monologue.updateAll();
+		Monologue.updateAll();
 		CommandScheduler.getInstance().run();
 	}
 

src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java

@@ -1,7 +1,6 @@
 package org.jmhsrobotics.frc2024.subsystems.arm;
 
 import org.jmhsrobotics.frc2024.Constants;
-import org.jmhsrobotics.warcore.nt.NT4Util;
 
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkBase.SoftLimitDirection;
@@ -24,8 +23,9 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import monologue.Logged;
 
-public class ArmPIDSubsystem extends SubsystemBase {
+public class ArmPIDSubsystem extends SubsystemBase implements Logged {
 
 	private MechanismLigament2d m_arm;
 	private CANSparkMax armPivot = new CANSparkMax(Constants.CAN.kArmPivotRightID, MotorType.kBrushless);
@@ -164,9 +164,9 @@ public void periodic() {
 		this.updateOdometry();
 
 		m_arm.setAngle(getArmPitch());
-
-		NT4Util.putPose3d("ArmSubsystem/armpose3d",
-				new Pose3d(-0.213, 0, 0.286, new Rotation3d(0, -Units.degreesToRadians(getArmPitch()), 0)));
+		log("armComponent", new Pose3d(-0.213, 0, 0.286, new Rotation3d(0, -Units.degreesToRadians(getArmPitch()), 0)));
+		log("armComponent_Goal",
+				new Pose3d(-0.213, 0, 0.286, new Rotation3d(0, -Units.degreesToRadians(armPID.getGoal().position), 0)));
 
 	}
 

src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java

@@ -10,6 +10,8 @@
 import com.revrobotics.SparkLimitSwitch;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
@@ -18,8 +20,9 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import monologue.Logged;
 
-public class ArmSubsystem extends SubsystemBase {
+public class ArmSubsystem extends SubsystemBase implements Logged {
 
 	private MechanismLigament2d m_arm;
 	private CANSparkMax armPivot = new CANSparkMax(Constants.CAN.kArmPivotRightID, MotorType.kBrushless);
@@ -107,6 +110,8 @@ public void periodic() {
 		// new Pose3d(-0.213, 0, 0.286, new Rotation3d(0,
 		// -Units.degreesToRadians(getArmPitch()), 0)));
 
+		log("armComponent", new Pose3d(-0.213, 0, 0.286, new Rotation3d(0, -Units.degreesToRadians(getArmPitch()), 0)));
+
 	}
 
 	SingleJointedArmSim armSim;

src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimberSubsystem.java

@@ -7,8 +7,9 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import monologue.Logged;
 
-public class ClimberSubsystem extends SubsystemBase {
+public class ClimberSubsystem extends SubsystemBase implements Logged {
 	private CANSparkMax climber, helper;
 	private RelativeEncoder encoder;
 

src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/LockAprilTag.java

@@ -9,11 +9,11 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import monologue.Logged;
 
 //TODO: move all hardcoded numbers to constants file
-public class LockAprilTag extends Command {
+public class LockAprilTag extends Command implements Logged {
 	private DriveSubsystem drive;
 	private VisionSubsystem vision;
 
@@ -30,7 +30,7 @@ public LockAprilTag(int fiducialID, DriveSubsystem drive, VisionSubsystem vision
 		this.drive = drive;
 		this.vision = vision;
 		this.lockPID = new PIDController(0.01, 0, 0);
-		SmartDashboard.putData(lockPID);
+		// SmartDashboard.putData(lockPID);
 		this.fiducialID = fiducialID;
 
 		this.angleGoal = 180;
@@ -41,7 +41,7 @@ public LockAprilTag(int fiducialID, DriveSubsystem drive, VisionSubsystem vision
 		// time.
 
 		// SmartDashboard.putData("LockPID", this.lockPID);
-		addRequirements(this.drive); //TODO: Figure out how to deal with vision requirements
+		addRequirements(this.drive); // TODO: Figure out how to deal with vision requirements
 	}
 
 	@Override
@@ -78,13 +78,14 @@ public void execute() {
 
 			var rawOutput = this.lockPID.calculate(theta);
 			double output = MathUtil.clamp(rawOutput, -1, 1);
-			//TODO: Remove smart dashboard values
+			// TODO: Remove smart dashboard values
 
-			SmartDashboard.putNumber("Output", output);
-			SmartDashboard.putNumber("mes", theta);
+			// SmartDashboard.putNumber("Output", output);
+			// SmartDashboard.putNumber("mes", theta);
 
 			// TODO: flip the output sign
-			// Figured out the issue it appears that we had some values flipped and it was causing the pid loop not to output a continous output rather a stepped output
+			// Figured out the issue it appears that we had some values flipped and it was
+			// causing the pid loop not to output a continous output rather a stepped output
 			this.drive.drive(0, 0, -output, true, true);
 
 			// SmartDashboard.putNumber("LockPID/PositionError",

src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java

@@ -16,8 +16,9 @@
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
 import edu.wpi.first.wpilibj.simulation.DIOSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import monologue.Logged;
 
-public class IntakeSubsystem extends SubsystemBase {
+public class IntakeSubsystem extends SubsystemBase implements Logged {
 	private CANSparkMax intakeMotor;
 
 	private TimeOfFlight lowerSensor;
@@ -56,6 +57,9 @@ public void periodic() {
 		// this.upperSensor.getRange());
 
 		// SmartDashboard.putBoolean("intake/hasNote", this.hasNote());
+		log("intakeDutyCycle", intakeMotor.get());
+		log("hasNote", this.hasNote());
+		log("noteTooHigh", this.noteTooHigh());
 	}
 
 	public void set(double speed) {

src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java

@@ -14,16 +14,16 @@
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.simulation.FlywheelSim;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import monologue.Logged;
 
-public class ShooterSubsystem extends SubsystemBase {
+public class ShooterSubsystem extends SubsystemBase implements Logged {
 	private CANSparkMax topFlywheel = new CANSparkMax(Constants.CAN.kShooterTopId, MotorType.kBrushless);
 	private CANSparkMax bottomFlywheel = new CANSparkMax(Constants.CAN.kShooterBottomId, MotorType.kBrushless);;
 	private RelativeEncoder topEncoder;
 	private RelativeEncoder bottomEncoder;
 
 	private BangBangController bangBangController;
 	private double reference;
-
 	private ControlType controlType = ControlType.BANG_BANG;
 	public enum ControlType {
 		BANG_BANG, VOLTAGE

src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java

@@ -5,7 +5,6 @@
 import java.util.Optional;
 
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
-import org.jmhsrobotics.warcore.nt.NT4Util;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -26,7 +25,6 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import monologue.Logged;
 
@@ -73,6 +71,7 @@ public VisionSubsystem(DriveSubsystem drive) {
 
 	@Override
 	public void periodic() {
+
 		PhotonPipelineResult results = this.cam.getLatestResult();
 
 		targets = results.getTargets();
@@ -90,15 +89,18 @@ public void periodic() {
 			flucialIDs[i] = targets.get(i).getFiducialId();
 		}
 
-		SmartDashboard.putNumberArray("Vision/flucialIDs", flucialIDs);
-		NT4Util.putPose3d("Vision/poseList", posList);
+		// SmartDashboard.putNumberArray("Vision/flucialIDs", flucialIDs);
+		log("flucialIDs", flucialIDs);
+		log("targets", posList);
+		log("cameraTransform", camOnRobot); // TODO: only log once
 
 		// Puting the estimated pose to the network table
 		var estimatedPose = this.getEstimatedGlobalPose(this.drive.getPose());
-		// if (estimatedPose.isPresent()) {
-		// NT4Util.putPose3d("Vision/EstimatedTarget",
-		// estimatedPose.get().estimatedPose);
-		// }
+		if (estimatedPose.isPresent()) {
+			// NT4Util.putPose3d("Vision/EstimatedTarget",
+			// estimatedPose.get().estimatedPose);
+			log("postEstimation", estimatedPose.get().estimatedPose);
+		}
 	}
 
 	public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevPose) {