diff --git a/.gitignore b/.gitignore index 6c573d38..acae35d3 100644 --- a/.gitignore +++ b/.gitignore @@ -175,3 +175,5 @@ src/main/java/org/jmhsrobotics/frc2024/BuildConstants.java *.wpilog simgui-ds.json simgui.json +networktables.json +networktables.json.bck diff --git a/build.gradle b/build.gradle index 1767cea6..1aa78ed3 100644 --- a/build.gradle +++ b/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() diff --git a/src/main/java/org/jmhsrobotics/frc2024/Robot.java b/src/main/java/org/jmhsrobotics/frc2024/Robot.java index 67d27544..f5e786da 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/Robot.java +++ b/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(); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java index 69784fa7..10b76a23 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java +++ b/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))); } diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java index 346483e2..3d3f6436 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmSubsystem.java +++ b/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; diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimberSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimberSubsystem.java index 90255cf3..6a2d3446 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/climber/ClimberSubsystem.java +++ b/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; diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/LockAprilTag.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/LockAprilTag.java index 31fb789c..c37248e8 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/LockAprilTag.java +++ b/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", diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java index b43d32d6..7d83b638 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java +++ b/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) { diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java index 0db630e0..4d012b38 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/shooter/ShooterSubsystem.java @@ -14,8 +14,9 @@ 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; @@ -23,7 +24,6 @@ public class ShooterSubsystem extends SubsystemBase { private BangBangController bangBangController; private double reference; - private ControlType controlType = ControlType.BANG_BANG; public enum ControlType { BANG_BANG, VOLTAGE diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java index d9029a2a..5406b987 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java +++ b/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 getEstimatedGlobalPose(Pose2d prevPose) {