Skip to content
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

Jump to bottom

Limelight megatag2 #73

Merged
merged 12 commits into from
Jun 14, 2024
Merged

Limelight megatag2 #73

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
12 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
f27ed96
incorporated megatag2 into calculations involving shooter limelight
stwiggy Jun 12, 2024
ea164bb
fixed target angle units
stwiggy Jun 13, 2024
4fa30b2
Merge branch 'master' into limelight-megatag2
stwiggy Jun 13, 2024
bd3a169
Merge branch 'master' into limelight-constants+testing
stwiggy Jun 13, 2024
3ad4780
original controls
stwiggy Jun 13, 2024
514a683
merged other branch and added comments
stwiggy Jun 13, 2024
a51458e
changed distance math
stwiggy Jun 13, 2024
7d21c52
small things
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
33 changes: 20 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,39 @@ 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 int[] VALID_IDS = { 4, 7 };
ProfessorAtomicManiac marked this conversation as resolved.
Show resolved Hide resolved
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

To make it clearer, instead of writing 4 use RED_SPEAKER_CENTER_TAG_ID and similarly for 7


public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field
public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field
public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high
// to represent unreliable heading
public static final int MAX_TRUSTED_ANG_VEL_DEGSPSEC = 720; // maximum trusted angular velocity
stwiggy marked this conversation as resolved.
Show resolved Hide resolved

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
5 changes: 1 addition & 4 deletions src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,6 @@

import org.carlmontrobotics.subsystems.Arm;
import org.carlmontrobotics.subsystems.Limelight;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command;

public class AimArmSpeaker extends Command {
Expand Down Expand Up @@ -41,6 +38,6 @@ public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return arm.armAtSetpoint();
}
}
44 changes: 44 additions & 0 deletions src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics.commands;

import org.carlmontrobotics.subsystems.Arm;
import org.carlmontrobotics.subsystems.Limelight;
import edu.wpi.first.wpilibj2.command.Command;

public class AimArmSpeakerMT2 extends Command {
private final Arm arm;
private final Limelight ll;

/** Creates a new AimOuttakeSpeaker. */
public AimArmSpeakerMT2(Arm arm, Limelight ll) {
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(this.arm = arm);
this.ll = ll;
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
double goal = ll.getOptimizedArmAngleRadsMT2();
arm.setArmTarget(goal);
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return arm.armAtSetpoint();
}
}
6 changes: 2 additions & 4 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 @@ -5,11 +5,9 @@
package org.carlmontrobotics.commands;

import static org.carlmontrobotics.Constants.Drivetrainc.*;
import static org.carlmontrobotics.Constants.Limelightc.*;

import org.carlmontrobotics.subsystems.Drivetrain;
import org.carlmontrobotics.subsystems.Limelight;
import org.carlmontrobotics.subsystems.LimelightHelpers;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
Expand All @@ -33,7 +31,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 +41,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
62 changes: 62 additions & 0 deletions src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java
View file
Open in desktop
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If it turns out that megatags work just fine, I highly recommend getting rid of old methods to reduce clutter

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

will do after testing

Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package org.carlmontrobotics.commands;

import static org.carlmontrobotics.Constants.Drivetrainc.*;

import org.carlmontrobotics.subsystems.Drivetrain;
import org.carlmontrobotics.subsystems.Limelight;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj2.command.Command;

public class AlignToApriltagMegatag2 extends Command {

public final TeleopDrive teleopDrive;
public final Drivetrain drivetrain;
private Limelight limelight;

public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1],
thetaPIDController[2]);

public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) {
this.limelight = limelight;
this.drivetrain = drivetrain;
this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand();

rotationPID.enableContinuousInput(-180, 180);
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
.plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2()));
stwiggy marked this conversation as resolved.
Show resolved Hide resolved
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
SendableRegistry.addChild(this, rotationPID);
addRequirements(drivetrain);
}

@Override
public void execute() {
Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
.plus(Rotation2d.fromDegrees(limelight.getRotateAngleRadMT2()));
stwiggy marked this conversation as resolved.
Show resolved Hide resolved
rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
if (teleopDrive == null)
drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
else {
double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds();
drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1],
rotationPID.calculate(drivetrain.getHeading()));
}
}

@Override
public boolean isFinished() {
return false;
// SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint());
// SmartDashboard.putNumber("Error", rotationPID.getPositionError());
// return rotationPID.atSetpoint();
}
}
3 changes: 3 additions & 0 deletions src/main/java/org/carlmontrobotics/subsystems/Arm.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -175,6 +175,9 @@ public void setBooleanDrive(boolean climb) {

@Override
public void periodic() {

SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing

babyMode = SmartDashboard.getBoolean("babymode", false);


Expand Down
3 changes: 3 additions & 0 deletions src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1004,5 +1004,8 @@ public void keepRotateMotorsAtDegrees(int angle) {
}
}

public double getGyroRate() {
return gyro.getRate();
}
// #endregion
}
77 changes: 67 additions & 10 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 @@ -3,10 +3,12 @@
import static org.carlmontrobotics.Constants.Limelightc.*;
import static org.carlmontrobotics.Constants.Limelightc.Apriltag.*;

import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

Expand All @@ -18,24 +20,40 @@ public class Limelight extends SubsystemBase {
// private double[] targetPose = null;
private Pose3d botPose;

private final InterpolatingDoubleTreeMap shooterMap;
ProfessorAtomicManiac marked this conversation as resolved.
Show resolved Hide resolved

public Limelight(Drivetrain drivetrain) {
this.drivetrain = drivetrain;
poseEstimator = new SwerveDrivePoseEstimator(
drivetrain.getKinematics(),
Rotation2d.fromDegrees(drivetrain.getHeading()),
drivetrain.getModulePositions(),
new Pose2d());

LimelightHelpers.SetFiducialIDFiltersOverride(SHOOTER_LL_NAME, VALID_IDS);

shooterMap = new InterpolatingDoubleTreeMap(); // add values after testing
// key is distance (meters), value is angle (rads)
shooterMap.put(0.0, 0.0);
}

@Override
public void periodic() {
poseEstimator.update(Rotation2d.fromDegrees(drivetrain.getHeading()), drivetrain.getModulePositions());
updateBotPose3d();
getDistanceToSpeakerMeters();
getCurrentPose();
getDistanceToNoteMeters();
}

updateMT2Odometry();

// 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());

// shooter limelight testing
SmartDashboard.putNumber("distance to speaker (meters)", getDistanceToSpeakerMetersMT2());
SmartDashboard.putNumber("optimized arm angle", getArmAngleToShootSpeakerRad());
}

public void updateBotPose3d() {
botPose = LimelightHelpers.getBotPose3d(SHOOTER_LL_NAME);
Expand All @@ -49,7 +67,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 +78,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() {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this is within the context of this PR, but I'm pretty sure this only calculates the distance between the note and camera in the y direction relative to the robot, it doesn't account for the tx measurement from Limelight.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Similarly for getDistanceToSpeakerMeters. However if it turns out Megatags work just fine, you can probably just get rid of these methods except for the note one (you could make the note method use Megatags)

Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE)
.plus(Rotation2d.fromDegrees(getTYDeg(INTAKE_LL_NAME)));
Expand All @@ -89,16 +103,59 @@ 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());
return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters());
}
Comment on lines +112 to 116
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure if this falls under the scope of this PR, but something's weird with this math.

double cameraLensHorizontalOffset = Deg / Meters;
double realHorizontalOffset = atan(deg/m^2); // Arguments passed to atan should almost always be unitless
return atan(rad/m); // Arguments passed to atan should almost always be unitless

If this is something that would be better addressed in a different PR, feel free to ignore this comment, and I can just open an issue.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yeah, seems like the second involvement of meters should be outside a normal tangent, not an arctan?
Something like that, I second this possible issue.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

if megatags work though, you don't have to do all this math. I'd say we see if megatags work and if so, we can probably scrap the math.


// megatag2

public void updateMT2Odometry() {
boolean rejectVisionUpdate = false;

LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME,
ProfessorAtomicManiac marked this conversation as resolved.
Show resolved Hide resolved
poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers
.getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME);

if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEGSPSEC) { // degrees per second
rejectVisionUpdate = true;
}

if (visionPoseEstimate.tagCount == 0) {
rejectVisionUpdate = true;
}

if (!rejectVisionUpdate) {
poseEstimator
.setVisionMeasurementStdDevs(VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, STD_DEV_HEADING_RADS));
poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds);
}
}

public double getRotateAngleRadMT2() {
Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target
ProfessorAtomicManiac marked this conversation as resolved.
Show resolved Hide resolved

double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the robot and target
double targetY = targetPoseRobotSpace.getY(); // the sideways offset

double targetOffsetRads = Math.atan2(targetY, targetX);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

double check that the signs are correct (to be clear, idk if they are correct, but people not understanding the coordinate system has caused bugs in the past)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what signs?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

on the robot, left and right and forward and backward are sometimes swapped and rotated. Not that I remember which way the signs are, off the top of my head. Ask Big Brettle for that.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i'll flip signs if necessary after testing 🤷


return targetOffsetRads;
}

public double getDistanceToSpeakerMetersMT2() {
return LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME).getX();
stwiggy marked this conversation as resolved.
Show resolved Hide resolved
}

public double getOptimizedArmAngleRadsMT2() {
return shooterMap.get(getDistanceToSpeakerMetersMT2());
}
}
Loading
Loading