-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
moved vision related things from DriveSubsystem to PhotonPoseEstimati…
…onSubsystem
- Loading branch information
Showing
3 changed files
with
174 additions
and
68 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
144 changes: 144 additions & 0 deletions
144
src/main/java/frc/robot/subsystems/PhotonPoseEstimationSubsystem.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,144 @@ | ||
| package frc.robot.subsystems; | ||
|
|
||
| import static frc.robot.Constants.DriveConstants.*; | ||
|
|
||
| import java.util.function.BooleanSupplier; | ||
| import java.util.function.DoubleSupplier; | ||
|
|
||
| import org.photonvision.targeting.PhotonTrackedTarget; | ||
|
|
||
| import edu.wpi.first.math.MathUtil; | ||
| import edu.wpi.first.math.controller.PIDController; | ||
| import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; | ||
| import edu.wpi.first.math.geometry.Pose2d; | ||
| import edu.wpi.first.math.geometry.Rotation2d; | ||
| import edu.wpi.first.math.geometry.Translation2d; | ||
| import edu.wpi.first.networktables.NetworkTableInstance; | ||
| import edu.wpi.first.networktables.StructPublisher; | ||
| import edu.wpi.first.wpilibj.Timer; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.SubsystemBase; | ||
| import frc.robot.Constants.ControllerConstants; | ||
|
|
||
| public class PhotonPoseEstimationSubsystem extends SubsystemBase { | ||
|
|
||
| private final SwerveDrivePoseEstimator m_poseEstimator; | ||
| private final Vision m_vision; | ||
| private final DriveSim m_driveSim; | ||
|
|
||
| private final StructPublisher<Pose2d> m_visionPosePublisher; | ||
| private DriveSubsystem m_driveSubsystem; | ||
|
|
||
| public PhotonPoseEstimationSubsystem(DriveSubsystem driveSubsystem) { | ||
| m_driveSubsystem = driveSubsystem; | ||
| m_visionPosePublisher = NetworkTableInstance.getDefault() | ||
| .getStructTopic("/SmartDashboard/VisionPose", Pose2d.struct).publish(); | ||
| m_driveSim = new DriveSim(driveSubsystem.kinematics(), driveSubsystem.getHeading(), | ||
| driveSubsystem.getModulePositions()); | ||
| m_vision = new Vision(); | ||
| m_poseEstimator = new SwerveDrivePoseEstimator(driveSubsystem.kinematics(), driveSubsystem.getHeading(), | ||
| driveSubsystem.getModulePositions(), | ||
| m_driveSim.getSimPose()); | ||
| } | ||
|
|
||
| public void addVisionMeasurement() { | ||
| PhotonTrackedTarget target = m_vision.getBestTarget(); // TODO should this just be integrated into Vision | ||
| Pose2d visionPose = m_vision.getVisionPose(target).toPose2d(); | ||
| m_poseEstimator.addVisionMeasurement(visionPose, Timer.getFPGATimestamp()); | ||
| } | ||
|
|
||
| @Override | ||
| public void periodic() { | ||
| // m_posePublisher.set(m_odometry.update(getHeading(), getModulePositions())); | ||
| m_poseEstimator.update(m_driveSubsystem.getHeading(), m_driveSubsystem.getModulePositions()); | ||
| var estPose = m_vision.getEstimatedGlobalPose(); | ||
| estPose.ifPresent( | ||
| est -> { | ||
| m_visionPosePublisher.set(est.estimatedPose.toPose2d()); | ||
| m_poseEstimator.addVisionMeasurement( | ||
| est.estimatedPose.toPose2d(), | ||
| est.timestampSeconds, | ||
| m_vision.currentSTD); | ||
| }); | ||
| } | ||
|
|
||
| @Override | ||
| public void simulationPeriodic() { | ||
| m_vision.updateVisionSim(m_driveSim.getSimPose()); | ||
| m_driveSim.updateSimPose(m_driveSubsystem.getHeading(), m_driveSubsystem.getModulePositions()); | ||
| // m_posePublisher.set(m_driveSim.getSimPose()); | ||
| } | ||
|
|
||
| public Command resetDriveSimCommand(Pose2d pose) { | ||
| return runOnce( | ||
| () -> m_driveSim | ||
| .resetSimPose(m_driveSubsystem.getHeading(), m_driveSubsystem.getModulePositions(), pose)) | ||
| .withName("ResetDriveSimCommand"); | ||
| } | ||
|
|
||
| /** | ||
| * Creates a command to drive the robot with joystick input. | ||
| * | ||
| * @param forwardSpeed Forward speed supplier. Positive values make the robot | ||
| * go forward (+X direction). | ||
| * @param strafeSpeed Strafe speed supplier. Positive values make the robot | ||
| * go to the left (+Y direction). | ||
| * @param rotation Rotation speed supplier. Positive values make the | ||
| * robot rotate CCW. | ||
| * @param isFieldRelative Supplier for determining if driving should be field | ||
| * relative. | ||
| * @return A command to drive the robot. | ||
| */ | ||
| public Command tagDriveCommand(DoubleSupplier forwardSpeed, DoubleSupplier strafeSpeed, | ||
| DoubleSupplier rotation, BooleanSupplier isFieldRelative) { | ||
| return run(() -> { | ||
| // Get the forward, strafe, and rotation speed, using a deadband on the joystick | ||
| // input so slight movements don't move the robot | ||
| // this can definitely be cleaned up but i didn't feel like it | ||
| double rotSpeed = MathUtil.applyDeadband(rotation.getAsDouble(), ControllerConstants.kDeadzone); | ||
| double fwdSpeed = MathUtil.applyDeadband(forwardSpeed.getAsDouble(), ControllerConstants.kDeadzone); | ||
| double strSpeed = MathUtil.applyDeadband(strafeSpeed.getAsDouble(), ControllerConstants.kDeadzone); | ||
| rotSpeed = Math.signum(rotSpeed) * Math.pow(rotSpeed, 2); | ||
| fwdSpeed = Math.signum(fwdSpeed) * Math.pow(fwdSpeed, 2); | ||
| strSpeed = Math.signum(strSpeed) * Math.pow(strSpeed, 2); | ||
|
|
||
| // Detect nearest April Tag and get translation data from joysticks and tag | ||
| // Calculate translation between driver movement and target movement to tag | ||
| PhotonTrackedTarget target = m_vision.getBestTarget(); | ||
| Translation2d jsTranslation = new Translation2d(forwardSpeed.getAsDouble(), | ||
| strafeSpeed.getAsDouble()); | ||
| Translation2d targetPos = m_vision.getTranslationToTarget(target); | ||
| Translation2d posDiff = jsTranslation.minus(targetPos); | ||
|
|
||
| // Get rotation data from joysticks and April Tag | ||
| // Calculate angle between driver rotation and target rotation to tag | ||
| Rotation2d jsRotation = new Rotation2d(forwardSpeed.getAsDouble(), | ||
| strafeSpeed.getAsDouble()); | ||
| Rotation2d targetAngle = m_vision.getYawToPose(target); | ||
| Rotation2d angleDiff = jsRotation.minus(targetAngle); | ||
|
|
||
| // Use PID controller to calculate needed speeds for movement towards April Tag | ||
| PIDController tagController = new PIDController(0.2, 0, 0); // TODO idk what | ||
| // PID constants to use | ||
| if (Math.abs(posDiff.getX()) < 3) { | ||
| fwdSpeed += tagController.calculate(jsTranslation.getX(), targetPos.getX()); | ||
| } | ||
| if (Math.abs(posDiff.getY()) < 3) { | ||
| strSpeed += tagController.calculate(jsTranslation.getY(), targetPos.getY()); | ||
| } | ||
| if (Math.abs(angleDiff.getDegrees()) < 30) { // TODO idk how close "close" is | ||
| rotSpeed += tagController.calculate( | ||
| jsRotation.getRotations(), | ||
| targetAngle.getRotations()); | ||
| } | ||
|
|
||
| // Convert speeds to volts | ||
| rotSpeed *= kTeleopMaxTurnVoltage; | ||
| fwdSpeed *= kTeleopMaxVoltage; | ||
| strSpeed *= kTeleopMaxVoltage; | ||
|
|
||
| m_driveSubsystem.drive(fwdSpeed, strSpeed, rotSpeed, isFieldRelative.getAsBoolean()); | ||
| }).withName("TagDriveCommand"); | ||
| } | ||
|
|
||
| } |