Skip to content

Commit

Permalink
removed redundant methods from limelightsubsystem
Browse files Browse the repository at this point in the history
  • Loading branch information
SJJCoding committed Mar 20, 2024
1 parent cfc6016 commit 1e6874b
Showing 1 changed file with 1 addition and 34 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@ public class LimelightSubsystem extends SubsystemBase {
final NetworkTable networkTable;

String currentPoseString;
String targetPoseString;

// network tables

Expand All @@ -34,7 +33,6 @@ public LimelightSubsystem() {

// logging
currentPoseString = "";
targetPoseString = "";

networkTable = NetworkTableInstance.getDefault().getTable("limelight");
ShuffleboardTab limelightTab = Shuffleboard.getTab("Limelight");
Expand All @@ -59,10 +57,6 @@ public LimelightSubsystem() {
.addString("Current Pose ", this::getCurrentPoseString)
.withPosition(0, 1)
.withSize(4, 1);
limelightTab
.addString("Target Pose ", this::getTargetPoseString)
.withPosition(0, 2)
.withSize(4, 1);
GOAL_DISTANCE_FROM_NOTE =
limelightTab
.addPersistent("Goal distance", 20.0)
Expand Down Expand Up @@ -108,38 +102,11 @@ public double getDistanceFromTargetInches() {
return 0.0;
}
}
// tan(degree) * distance = sideways distance

// target height / tan(vertical angle)

public Pose2d getTargetPose(Pose2d currentPose) {

// math thing to get target pose using current pose

Rotation2d targetHeading =
new Rotation2d(
currentPose.getRotation().getRadians() + Units.degreesToRadians(getHorizontalOffset()));

double targetDistance = Units.inchesToMeters(getDistanceFromTargetInches());

Translation2d translationOffset = new Translation2d(targetDistance, targetHeading);
Pose2d targetPose =
new Pose2d(currentPose.getTranslation().plus(translationOffset), targetHeading);

currentPoseString = currentPose.toString();
targetPoseString = targetPose.toString();

return targetPose;
}


public String getCurrentPoseString() {
return currentPoseString;
}

public String getTargetPoseString() {
return targetPoseString;
}

public boolean isWithinDistance() {
return (getDistanceFromTargetInches() <= GOAL_DISTANCE_FROM_NOTE.getDouble(20.0));
}
Expand Down

0 comments on commit 1e6874b

Please sign in to comment.