Skip to content

Commit

Permalink
Merge branch 'main' into autoposition
Browse files Browse the repository at this point in the history
  • Loading branch information
@liulinden
liulinden committed Feb 29, 2024
2 parents b2d991f + 967796b commit 1dbcb6b
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 61 deletions.
46 changes: 25 additions & 21 deletions src/main/java/frc/robot/commands/AimAtTag.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,9 @@
import frc.robot.subsystems.SwerveDrivetrain;
import frc.robot.subsystems.Vision;

import org.photonvision.targeting.PhotonTrackedTarget;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -18,17 +18,19 @@ public class AimAtTag extends Command {
private final ChassisDriveInputs chassisDriveInputs;

private final Vision vision;
private final Integer tagID; // Integer as opposed to int so it can be null for best tag
private final Integer tagID; // Integer as opposed to int so it can be null for best tag

private final PIDController rotatePID;

/**
* Create a new AimAtTag command. Tries to constants aim at a tag while still allowing driver to control robot.
* Create a new AimAtTag command. Tries to constants aim at a tag while still
* allowing driver to control robot.
*
* @param drivetrain the drivetrain of the robot
* @param vision the vision subsystem of the robot
* @param tagID the numerical ID of the the tag to turn to, null for best tag
* @param chassisDriveControl collection of inputs for driving
* @param tagID the numerical ID of the the tag to turn to, null
* for best tag
* @param chassisDriveControl collection of inputs for driving
*/
public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, ChassisDriveInputs chassisDriveInputs) {
this.drivetrain = drivetrain;
Expand All @@ -37,12 +39,12 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass
this.tagID = tagID;
this.chassisDriveInputs = chassisDriveInputs;

rotatePID = new PIDController(
2,
0,
0);
rotatePID.enableContinuousInput(-1, 1);
rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
rotatePID = new PIDController(
2,
0,
0);
rotatePID.enableContinuousInput(-1, 1);
rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
rotatePID.setSetpoint(0);

addRequirements(drivetrain);
Expand All @@ -51,9 +53,10 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass
/**
* Create a new AimAtTag command. Tries to aim at a tag.
*
* @param drivetrain the drivetrain of the robot
* @param vision the vision subsystem of the robot
* @param tagID the numerical ID of the the tag to turn to, null for best tag
* @param drivetrain the drivetrain of the robot
* @param vision the vision subsystem of the robot
* @param tagID the numerical ID of the the tag to turn to, null for best
* tag
*/
public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID) {
this(drivetrain, vision, tagID, null);
Expand All @@ -66,11 +69,12 @@ public void initialize() {

@Override
public void execute() {
PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID);

double tagYawRadians = 0;
if (tag != null) {
tagYawRadians = Units.degreesToRadians(tag.getYaw());
if (distToTag != null) {
Rotation2d angleToTag = new Rotation2d(distToTag.getX(), distToTag.getY());
tagYawRadians = angleToTag.getRadians();
}

double xSpeed = 0;
Expand All @@ -79,8 +83,8 @@ public void execute() {
xSpeed = chassisDriveInputs.getX();
ySpeed = chassisDriveInputs.getY();
}
double rotateSpeed = rotatePID.calculate(tagYawRadians);
double rotateSpeed = rotatePID.calculate(tagYawRadians);

ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotateSpeed);

drivetrain.setDesiredState(desiredSpeeds, false, true);
Expand All @@ -97,4 +101,4 @@ public boolean isFinished() {
public void end(boolean interrupted) {
drivetrain.stop();
}
}
}
30 changes: 15 additions & 15 deletions src/main/java/frc/robot/commands/FollowTag.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,6 @@
import frc.robot.subsystems.SwerveDrivetrain;
import frc.robot.subsystems.Vision;

import org.photonvision.targeting.PhotonTrackedTarget;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
Expand All @@ -17,10 +15,10 @@ public class FollowTag extends Command {
private final SwerveDrivetrain drivetrain;

private final Vision vision;
private final Integer tagID; // Integer as opposed to int so it can be null for best tag
private final Integer tagID; // Integer as opposed to int so it can be null for best tag
private final Transform2d targetDistance;

private final Double loseTagAfterSeconds; // Double as opposed to double so it can be null for never lose mode.
private final Double loseTagAfterSeconds; // Double as opposed to double so it can be null for never lose mode.
private double secondsSinceTagLastSeen;

/**
Expand All @@ -29,9 +27,11 @@ public class FollowTag extends Command {
*
* @param drivetrain the drivetrain of the robot
* @param vision the vision subsystem of the robot
* @param tagID the numerical ID of the the tag to follow, null for best tag
* @param tagID the numerical ID of the the tag to follow, null
* for best tag
* @param targetDistanceToTag the target distance away from the tag to be
* @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
* @param loseTagAfterSeconds how long to wait before giving up on rediscover
* tag, set to null to never finish
*/
public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, Integer tagID,
Double loseTagAfterSeconds) {
Expand All @@ -52,7 +52,8 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetD
*
* @param drivetrain the drivetrain of the robot
* @param vision the vision subsystem of the robot
* @param tagID the numerical ID of the the tag to follow, null for whatever best is
* @param tagID the numerical ID of the the tag to follow, null
* for whatever best is
* @param targetDistanceToTag the target distance away from the tag to be
* @param loseTagAfterSeconds how long to wait before giving up on rediscover
* tag, set to null to never finish
Expand All @@ -70,23 +71,22 @@ public void initialize() {

@Override
public void execute() {

final PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);

if (tag == null) {
final Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID);

if (distToTag == null) {
secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod;
drivetrain.stop();
} else {
final Transform3d tagPosition3d = vision.getDistanceToTarget(tag);

secondsSinceTagLastSeen = 0;

// https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
// https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
final Transform2d tagPosition = new Transform2d(
tagPosition3d.getZ(),
tagPosition3d.getX(),
Rotation2d.fromRadians(tag.getYaw()));
distToTag.getZ(),
distToTag.getX(),
Rotation2d.fromRadians(distToTag.getRotation().getZ()));

final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());

Expand All @@ -106,4 +106,4 @@ public void end(boolean interrupted) {
drivetrain.clearDesiredPosition();
drivetrain.stop();
}
}
}
49 changes: 24 additions & 25 deletions src/main/java/frc/robot/subsystems/Vision.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,52 +18,51 @@ public class Vision extends SubsystemBase {
private final static boolean DEBUG_INFO = false;

final PhotonCamera camera;
final Transform3d cameraToFrontCenter;
final Transform3d robotToCamera;
final Transform3d cameraToRobot;

/**
* Create new PhotonCamera subsystem
*
* @param cameraName name of PhotonCamera
* @param cameraToFrontCenter distance from the camera to the front center point
* of the robot
* @param cameraName name of PhotonCamera
* @param robotToCamera distance from the camera to the center of the robot
*/
public Vision(String cameraName, Transform3d cameraToFrontCenter) {
public Vision(String cameraName, Transform3d robotToCamera) {
camera = new PhotonCamera(cameraName);
this.cameraToFrontCenter = cameraToFrontCenter;
this.robotToCamera = robotToCamera;
cameraToRobot = robotToCamera.inverse();
}

/**
* Get best april tag target
* Get distance to the distance to the best tag found by the camera
*
* @return The position of the tag (translation and rotation) based on the
* center of the robot. Returns null if no tag found.
*
* @return Object of best target
*/
public PhotonTrackedTarget getTag() {
return camera
.getLatestResult()
.getBestTarget();
public Transform3d getDistToTag() {
PhotonTrackedTarget target = camera.getLatestResult().getBestTarget();
if (target == null) {
return null;
}
return target.getBestCameraToTarget().plus(cameraToRobot);
}

/**
* Gives Transform3d from robot center to the desired target
* Get distance to the desired tag
*
* @param tagID The fiducial ID of the desired April Tag
* @return returns first tag with matching ID, null if None are found
* @param tagID the fiducial ID of the desired tag
* @return the position of the tag (translation and rotation) based on the
* center of the robot. Returns null if no tag found
*/
public PhotonTrackedTarget getTag(int tagID) {
public Transform3d getDistToTag(int tagID) {
for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) {
if (target.getFiducialId() == tagID)
return target;
return target.getBestCameraToTarget().plus(cameraToRobot);
}
return null;
}

public Transform3d getDistanceToTarget(PhotonTrackedTarget tag) {
return tag
.getBestCameraToTarget()
.plus(cameraToFrontCenter);
}


@Override
public void periodic() {
if (!DEBUG_INFO)
Expand Down Expand Up @@ -96,4 +95,4 @@ public void periodic() {
SmartDashboard.putNumber("Tag Pose Pitch", tagPoseRotation.getY());
SmartDashboard.putNumber("Tag Pose Roll", tagPoseRotation.getX());
}
}
}

0 comments on commit 1dbcb6b

Please sign in to comment.