Limelight megatag2 by stwiggy · Pull Request #73 · DeepBlueRobotics/RobotCode2024

stwiggy · 2024-06-12T17:10:17Z

created megatag2 alternatives to existing shooter limelight methods and commands

ProfessorAtomicManiac

very epic, but there are a few issues

ProfessorAtomicManiac · 2024-06-13T04:51:17Z

  private void setBindingsManipulatorNEO() {
-    new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter));
+    new JoystickButton(manipulatorController, EJECT_BUTTON).whileTrue(new AlignToNote(drivetrain)); // test



You don't ever add a binding for the new AimArmSpeaker commands. Before that though, you should probably discuss with manipulator + driver on how they want to be able to auto-aim. IMO, if I was driver, it would be convenient to have a single button that can be held down that simultaneously aims the robot while moving the arm, then LEDs flash to tell me the shooter is ready to shoot, and I press another button.

ProfessorAtomicManiac · 2024-06-13T05:05:05Z

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

will do after testing

ProfessorAtomicManiac · 2024-06-13T05:39:05Z

+    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);


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)

what signs?

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.

i'll flip signs if necessary after testing 🤷

CoolSpy3

Nice work! Just a couple of minor notes.

CoolSpy3 · 2024-06-13T06:39:24Z

+  public double getRotateAngleRad() {
    double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters();
    double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters());
    return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters());
  }


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.

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.

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.

FriedLongJohns · 2024-06-13T12:58:29Z

+  public double getRotateAngleRad() {
    double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters();
    double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters());
    return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters());
  }


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.

FriedLongJohns · 2024-06-13T13:00:18Z

+    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);


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.

ProfessorAtomicManiac

Will double check the math in a bit but overall looks good to me

ProfessorAtomicManiac · 2024-06-13T17:15:11Z

-		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 };


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

ProfessorAtomicManiac · 2024-06-13T17:37:14Z

+    double x = targetPoseRobotSpace.getX();
+    double y = targetPoseRobotSpace.getY();
+
+    return Math.sqrt(x * x + y * y);


Just wanted to mention Math.hypot exists. It's probably the same either way

ProfessorAtomicManiac · 2024-06-13T17:42:11Z

  }

-
  public double getDistanceToNoteMeters() {


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.

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)

ProfessorAtomicManiac · 2024-06-13T17:45:59Z

+  public double getRotateAngleRad() {
    double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters();
    double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters());
    return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters());
  }


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.

CoolSpy3

Approving on behalf of @ProfessorAtomicManiac .

@ProfessorAtomicManiac

@ProfessorAtomicManiac has approved these changes and requested that I merge this on his behalf

stwiggy and others added 5 commits June 9, 2024 16:56

testing

9e563a1

worksession

20d9600

updated constants

e8a613b

updated limelighthelpers

997ab27

incorporated megatag2 into calculations involving shooter limelight

f27ed96

stwiggy requested a review from a team as a code owner June 12, 2024 17:10

stwiggy added 3 commits June 12, 2024 19:32

fixed target angle units

ea164bb

Merge branch 'master' into limelight-megatag2

4fa30b2

Merge branch 'master' into limelight-constants+testing

bd3a169

ProfessorAtomicManiac previously requested changes Jun 13, 2024

View reviewed changes

stwiggy added 3 commits June 12, 2024 22:49

original controls

3ad4780

merged other branch and added comments

514a683

changed distance math

a51458e

CoolSpy3 requested changes Jun 13, 2024

View reviewed changes

FriedLongJohns reviewed Jun 13, 2024

View reviewed changes

ProfessorAtomicManiac reviewed Jun 13, 2024

View reviewed changes

Comment thread src/main/java/org/carlmontrobotics/Constants.java

Comment thread src/main/java/org/carlmontrobotics/subsystems/Limelight.java

Comment thread src/main/java/org/carlmontrobotics/subsystems/Limelight.java

Comment thread src/main/java/org/carlmontrobotics/subsystems/Limelight.java

small things

7d21c52

ProfessorAtomicManiac reviewed Jun 13, 2024

View reviewed changes

CoolSpy3 approved these changes Jun 14, 2024

View reviewed changes

CoolSpy3 merged commit 336ca7c into master Jun 14, 2024

CoolSpy3 deleted the limelight-megatag2 branch June 14, 2024 01:28

Uh oh!

Conversation

stwiggy commented Jun 12, 2024

Uh oh!

ProfessorAtomicManiac left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

CoolSpy3 left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

ProfessorAtomicManiac left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

CoolSpy3 left a comment

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

5 participants