From 9d1017185689303382c38452588225f19799a797 Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Fri, 29 Mar 2024 17:34:27 -0400 Subject: [PATCH 1/3] post camera to smartdashboard --- src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 82f98c33..b6c819e1 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -50,6 +50,7 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -283,6 +284,7 @@ public ArmPIDSubsystem getArmSubsystem() { public void configureSmartDashboard() { SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); + CameraServer.startAutomaticCapture("Kirby", 0); // SmartDashboard.putData("AutoIntakeCommand", // new AutoIntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem)); From 730777cd4d009085f0a8ceff2181949112228311 Mon Sep 17 00:00:00 2001 From: Connor Murphy Date: Mon, 1 Apr 2024 16:57:14 -0400 Subject: [PATCH 2/3] add driver dashboard data --- src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java | 6 ++++++ .../frc2024/subsystems/arm/ArmPIDSubsystem.java | 1 + .../frc2024/subsystems/drive/DriveSubsystem.java | 2 ++ .../frc2024/subsystems/drive/commands/DriveCommand.java | 7 +++++-- 4 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index 82f98c33..56bbf65d 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -89,6 +89,8 @@ public class RobotContainer implements Logged { // DefaultIntakeCommand(intakeSubsystem, // shooterSubsystem); + private final DriveCommand driveCommand; + public RobotContainer() { SwitchableControlBoard swboard = new SwitchableControlBoard(new CompControl()); if (Robot.isSimulation()) { // Switch to single control in sim @@ -97,6 +99,7 @@ public RobotContainer() { // swboard.setControlBoard(new CompControl()); this.control = swboard; + this.driveCommand = new DriveCommand(this.driveSubsystem, visionSubsystem, this.control); this.driveSubsystem .setDefaultCommand(new DriveCommand(this.driveSubsystem, this.visionSubsystem, this.control)); @@ -106,6 +109,9 @@ public RobotContainer() { configureSmartDashboard(); SmartDashboard.putBoolean("HasNote", false); + SmartDashboard.putBoolean("LockAtGoal", this.driveCommand.lockAtGoal()); + SmartDashboard.putNumber("MatchTime", DriverStation.getMatchTime()); + SmartDashboard.putNumber("MatchNumber", DriverStation.getMatchNumber()); configureDriverFeedback(); configureBindings(); 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 cbe8a8d4..cf54bafa 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/ArmPIDSubsystem.java @@ -221,6 +221,7 @@ public void periodic() { new Pose3d(-0.213, 0, 0.286, new Rotation3d(0, -Units.degreesToRadians(armPID.getGoal().position), 0))); log("angleDegrees", getArmPitch()); log("angleGoalDegrees", armPID.getGoal().position); + log("ArmAtGoal", this.atGoal()); } SingleJointedArmSim armSim; diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java index 2a9f5146..e90307b0 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/DriveSubsystem.java @@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.BuiltInAccelerometer.Range; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import monologue.Logged; import monologue.Annotations.Log; @@ -92,6 +93,7 @@ public void periodic() { log("Y-Gs-rio", rio_Accelerometer.getY()); log("X-Gs-rio", rio_Accelerometer.getX()); log("Z-Gs-rio", rio_Accelerometer.getZ()); + SmartDashboard.putNumber("GyroAngle", m_gyro.getAngle()); if (Robot.isSimulation()) { Robot.objSim.update(pose2d); diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/DriveCommand.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/DriveCommand.java index 2fda54aa..79bef056 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/DriveCommand.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/commands/DriveCommand.java @@ -16,7 +16,6 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class DriveCommand extends Command { @@ -78,7 +77,8 @@ public void execute() { // SmartDashboard.putNumber("SwerveDrive/Input/SwerveDriveXSpeed", ySpeed); // SmartDashboard.putNumber("SwerveDrive/Input/SwerveDriveXSpeed", // rotationSpeed); - SmartDashboard.putNumber("Angle Offset", computeAngleLockValue()); + // SmartDashboard.putNumber("Angle Offset", computeAngleLockValue()); + if (this.control.AprilLockOn().getAsBoolean()) { rotationSpeed += computeAngleLockValue(); } @@ -140,5 +140,8 @@ public boolean isFinished() { private double getSquareInput(double input) { return Math.pow(input, 2) * Math.signum(input); } + public boolean lockAtGoal() { + return this.lockPID.atSetpoint(); + } } From 9b4428282965f28283799ee81656b1da8fdb13d6 Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Wed, 3 Apr 2024 08:37:31 -0400 Subject: [PATCH 3/3] remove CameraServe in RobotContainer --- src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java index e3d97ff2..56bbf65d 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java @@ -50,7 +50,6 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; -import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -290,7 +289,6 @@ public ArmPIDSubsystem getArmSubsystem() { public void configureSmartDashboard() { SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); - CameraServer.startAutomaticCapture("Kirby", 0); // SmartDashboard.putData("AutoIntakeCommand", // new AutoIntakeCommand(1, this.intakeSubsystem, this.shooterSubsystem));