Skip to content

Limelight constants #72

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
ProfessorAtomicManiac merged 7 commits into from
Jun 13, 2024
Merged

Limelight constants #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
9e563a1
testing
stwiggy Jun 9, 2024
20d9600
worksession
DeepBlueRobots Jun 10, 2024
e8a613b
updated constants
stwiggy Jun 10, 2024
997ab27
updated limelighthelpers
stwiggy Jun 10, 2024
ea164bb
fixed target angle units
stwiggy Jun 13, 2024
bd3a169
Merge branch 'master' into limelight-constants+testing
stwiggy Jun 13, 2024
3ad4780
original controls
stwiggy Jun 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 12 additions & 13 deletions src/main/java/org/carlmontrobotics/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -371,32 +371,31 @@ public static final class Autoc {
2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the
// angular constraints have no effect.
}
// #endregion
}

public static final class Limelightc {
public static final String INTAKE_LL_NAME = "limelight-intake";
public static final String SHOOTER_LL_NAME = "limelight-shooter";

public static final double ERROR_TOLERANCE_RAD = 0.1;
public static final double HORIZONTAL_FOV_DEG = 0;
public static final double RESOLUTION_WIDTH_PIX = 640;
public static final double MOUNT_ANGLE_DEG_SHOOTER = 42.5;
public static final double MOUNT_ANGLE_DEG_INTAKE = -22; // 23.228
public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(11.5); // 16.6
public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); // 16.6
public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115;
public static final double NOTE_HEIGHT = Units.inchesToMeters(0);
public static final double ERROR_TOLERANCE_RAD = 0.1; // unused
public static final double HORIZONTAL_FOV_DEG = 0; // unused
public static final double RESOLUTION_WIDTH_PIX = 640; // unused
public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446;
public static final double MOUNT_ANGLE_DEG_INTAKE = -29;
public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891);
public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52);
public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused
public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused
public static final double MIN_MOVEMENT_METERSPSEC = 0.5;
public static final double MIN_MOVEMENT_RADSPSEC = 0.5;
public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675);
public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(19.5);
public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911);
public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75);
public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65);
public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused
public static final class Apriltag {
public static final int RED_SPEAKER_CENTER_TAG_ID = 4;
public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7;
public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125
public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125);
public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26);
public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875);
}
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {

rotationPID.enableContinuousInput(-180, 180);
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
.minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg()));
.minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
SendableRegistry.addChild(this, rotationPID);
Expand All @@ -43,7 +43,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {
@Override
public void execute() {
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
.minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg()));
.minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
if (teleopDrive == null)
drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
Expand Down
16 changes: 8 additions & 8 deletions src/main/java/org/carlmontrobotics/subsystems/Limelight.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,13 @@ public void periodic() {
getDistanceToSpeakerMeters();
getCurrentPose();
getDistanceToNoteMeters();
}

// intake limelight testing
SmartDashboard.putBoolean("see note", LimelightHelpers.getTV(INTAKE_LL_NAME));
SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters());
SmartDashboard.putNumber("intake tx", LimelightHelpers.getTX(INTAKE_LL_NAME));
SmartDashboard.putNumber("rotation to align", getRotateAngleRad());
}

public void updateBotPose3d() {
botPose = LimelightHelpers.getBotPose3d(SHOOTER_LL_NAME);
Expand All @@ -49,7 +54,6 @@ public Pose2d getCurrentPose() {
return estimatedPos;
}


public double getTXDeg(String limelightName) {
return (limelightName == INTAKE_LL_NAME) ? LimelightHelpers.getTX(INTAKE_LL_NAME) : -LimelightHelpers.getTY(SHOOTER_LL_NAME);
}
Expand All @@ -61,21 +65,18 @@ public double getTYDeg(String limelightName) {
public double getDistanceToSpeakerMeters() {
if (LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID
|| LimelightHelpers.getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID) {
// TODO: change MOUNT_ANGLE_DEG_SHOOTER
Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER)
.plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); //because limelight is mounted horizontally
double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER) / angleToGoal.getTan();
// SmartDashboard.putNumber("limelight distance", distance);
return distance;
}

else {
// SmartDashboard.putNumber("limelight distance", -1);
return -1;
}
}


public double getDistanceToNoteMeters() {
Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE)
.plus(Rotation2d.fromDegrees(getTYDeg(INTAKE_LL_NAME)));
Expand All @@ -89,16 +90,15 @@ public double getDistanceToNoteMeters() {
}
}

public double getArmAngleToShootSpeakerRad(){
public double getArmAngleToShootSpeakerRad() {
double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS;
double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH;
return END_EFFECTOR_BASE_ANGLE_RADS - Math.atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters);
}

public double getRotateAngleDeg() {
public double getRotateAngleRad() {
double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters();
double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters());
Comment thread
ProfessorAtomicManiac marked this conversation as resolved.
return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters());
}

}
Loading