From 864c49593f3ade53f4dc318fa30d6728c1bdacce Mon Sep 17 00:00:00 2001 From: Luhan Wang <117863446+Lu-han-wang@users.noreply.github.com> Date: Sat, 27 Jan 2024 10:09:50 -0500 Subject: [PATCH] finished untuned aprilTag locking --- .../frc2024/controlBoard/CompControl.java | 67 +++++++++---------- .../drive/commands/LockAprilTag.java | 40 +++++++++-- .../drive/swerve/RevSwerveDrive.java | 2 - .../subsystems/vision/VisionSubsystem.java | 12 +++- 4 files changed, 76 insertions(+), 45 deletions(-) diff --git a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java index 7a1110f3..e72dc875 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java +++ b/src/main/java/org/jmhsrobotics/frc2024/controlBoard/CompControl.java @@ -4,41 +4,36 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; public class CompControl implements ControlBoard { - private CommandXboxController driver = new CommandXboxController(0); - private CommandXboxController operator = new CommandXboxController(1); - - // TODO: Implement operator controls in the future - // =============Operator Controls============= - - // =============Driver Controls============= - @Override - public double xInput() { - // TODO Auto-generated method stub - return this.driver.getLeftX(); - } - - @Override - public double yInput() { - // TODO Auto-generated method stub - return this.driver.getLeftY(); - } - - @Override - public double rotationalInput() { - // TODO Auto-generated method stub - return this.driver.getRightX(); - } - - @Override - public Trigger brake() { - // TODO Auto-generated method stub - return this.driver.leftBumper(); - } - - @Override - public Trigger setZeroHeading() { - // TODO Auto-generated method stub - return this.driver.rightBumper(); - } + private CommandXboxController driver = new CommandXboxController(0); + private CommandXboxController operator = new CommandXboxController(1); + + // TODO: Implement operator controls in the future + // =============Operator Controls============= + + // =============Driver Controls============= + @Override + public double xInput() { + return this.driver.getLeftX(); + } + + @Override + public double yInput() { + return this.driver.getLeftY(); + } + + @Override + public double rotationalInput() { + return this.driver.getRightX(); + } + + @Override + public Trigger brake() { + return this.driver.leftBumper(); + } + + @Override + public Trigger setZeroHeading() { + return this.driver.rightBumper(); + } } 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 571414a0..6d99e845 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 @@ -2,13 +2,17 @@ import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem; import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem; +import org.photonvision.targeting.PhotonTrackedTarget; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.State; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +//TODO: move all hardcoded numbers to constants file public class LockAprilTag extends Command { private DriveSubsystem drive; private VisionSubsystem vision; @@ -16,10 +20,14 @@ public class LockAprilTag extends Command { private ProfiledPIDController lockPID; private Constraints lockPIdConstraints; private Pose2d currentPose; - private double currentAngle; + private double currentYaw; private double fiducialID; + private PhotonTrackedTarget target; + + private double angleGoal; + public LockAprilTag(DriveSubsystem drive, VisionSubsystem vision, double fiducialID) { this.drive = drive; this.vision = vision; @@ -28,25 +36,47 @@ public LockAprilTag(DriveSubsystem drive, VisionSubsystem vision, double fiducia this.lockPID = new ProfiledPIDController(0.3, 0, 0, this.lockPIdConstraints); this.fiducialID = fiducialID; + this.target = this.vision.getTarget(this.fiducialID); + + this.angleGoal = this.target.getYaw(); + + SmartDashboard.putData("LockPID", this.lockPID); addRequirements(this.drive, this.vision); } @Override public void initialize() { - this.lockPID.reset(new State(0, 0)); - this.lockPID.setGoal(0); + this.lockPID.reset(new State(this.angleGoal, 0)); + this.lockPID.setGoal(this.angleGoal); + this.lockPID.setTolerance(1, 1); + + this.drive.stopDrive(); } @Override public void execute() { this.currentPose = this.drive.getPose(); - this.currentAngle = this.drive.getPose().getRotation().getDegrees(); + this.currentYaw = this.drive.getPose().getRotation().getDegrees(); + + this.lockPID.setConstraints(this.lockPIdConstraints); + + var rawOutput = this.lockPID.calculate(this.currentYaw); + double output = MathUtil.clamp(rawOutput, -0.5, 0.5); + + this.drive.drive(0, 0, output, true, false); + + SmartDashboard.putNumber("LockPID/macAcc", this.lockPIdConstraints.maxAcceleration); + SmartDashboard.putNumber("LockPID/maxVel", this.lockPIdConstraints.maxVelocity); + SmartDashboard.putNumber("LockPID/PositionError", this.lockPID.getPositionError()); + SmartDashboard.putNumber("LockPID/VelocityError", this.lockPID.getVelocityError()); + SmartDashboard.putNumber("LockPID/output", output); + SmartDashboard.putNumber("LockPID/currentYaw", currentYaw); } @Override public boolean isFinished() { - return false; + return this.lockPID.atGoal(); } @Override diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/swerve/RevSwerveDrive.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/swerve/RevSwerveDrive.java index 3ca69a91..11f003ae 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/swerve/RevSwerveDrive.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/drive/swerve/RevSwerveDrive.java @@ -77,13 +77,11 @@ public double getHeading() { @Override public void stopMotor() { - // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'stopMotor'"); } @Override public String getDescription() { - // TODO Auto-generated method stub throw new UnsupportedOperationException("Unimplemented method 'getDescription'"); } 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 780c419f..cd1473a5 100644 --- a/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/org/jmhsrobotics/frc2024/subsystems/vision/VisionSubsystem.java @@ -48,6 +48,7 @@ public class VisionSubsystem extends SubsystemBase implements Logged { private DriveSubsystem drive; private double[] flucialIDs; + List targets; public VisionSubsystem(DriveSubsystem drive) { this.cam = new PhotonCamera("Sechenov"); @@ -60,7 +61,6 @@ public VisionSubsystem(DriveSubsystem drive) { try { layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); } catch (IOException e) { - // TODO: error handling System.out.println(e); DriverStation.reportError("Fail to load the april tag map", e.getStackTrace()); } @@ -75,7 +75,7 @@ public VisionSubsystem(DriveSubsystem drive) { public void periodic() { PhotonPipelineResult results = this.cam.getLatestResult(); - List targets = results.getTargets(); + targets = results.getTargets(); SmartDashboard.putBoolean("Vision/isConnected", this.cam.isConnected()); int len = targets.size(); @@ -105,6 +105,14 @@ public Optional getEstimatedGlobalPose(Pose2d prevPose) { return this.estimator.update(); } + public PhotonTrackedTarget getTarget(double fiducialID) { + for (PhotonTrackedTarget i : targets) { + if (i.getFiducialId() == fiducialID) { + return i; + } + } + return null; + } VisionSystemSim visionSim = new VisionSystemSim("main"); PhotonCameraSim cameraSim;