Limelight megatag2 #73
Conversation
| @@ -201,7 +201,7 @@ private void setBindingsDriver() { | |||
| .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain)); | |||
| } | |||
| private void setBindingsManipulatorNEO() { | |||
| new JoystickButton(manipulatorController, EJECT_BUTTON).onTrue(new Eject(intakeShooter)); | |||
| new JoystickButton(manipulatorController, EJECT_BUTTON).whileTrue(new AlignToNote(drivetrain)); // test | |||
|
|
|||
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
If it turns out that megatags work just fine, I highly recommend getting rid of old methods to reduce clutter
There was a problem hiding this comment.
will do after testing
| 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); |
There was a problem hiding this comment.
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)
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
i'll flip signs if necessary after testing 🤷
src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java
Outdated
Show resolved
Hide resolved
| public double getRotateAngleRad() { | ||
| double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters(); | ||
| double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); | ||
| return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); | ||
| } |
There was a problem hiding this comment.
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 unitlessIf 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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java
Outdated
Show resolved
Hide resolved
| public double getRotateAngleRad() { | ||
| double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters(); | ||
| double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); | ||
| return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); | ||
| } |
There was a problem hiding this comment.
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.
| 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); |
There was a problem hiding this comment.
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.
| 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 }; |
There was a problem hiding this comment.
To make it clearer, instead of writing 4 use RED_SPEAKER_CENTER_TAG_ID and similarly for 7
| double x = targetPoseRobotSpace.getX(); | ||
| double y = targetPoseRobotSpace.getY(); | ||
|
|
||
| return Math.sqrt(x * x + y * y); |
There was a problem hiding this comment.
Just wanted to mention Math.hypot exists. It's probably the same either way
| else { | ||
| // SmartDashboard.putNumber("limelight distance", -1); | ||
| return -1; | ||
| } | ||
| } | ||
|
|
||
|
|
||
| public double getDistanceToNoteMeters() { |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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)
| public double getRotateAngleRad() { | ||
| double cameraLensHorizontalOffset = getTXDeg(SHOOTER_LL_NAME) / getDistanceToSpeakerMeters(); | ||
| double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters()); | ||
| return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters()); | ||
| } |
There was a problem hiding this comment.
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
dismissed
ProfessorAtomicManiac’s stale review
@ProfessorAtomicManiac has approved these changes and requested that I merge this on his behalf
created megatag2 alternatives to existing shooter limelight methods and commands