Note detection #57
Note detection #57
Conversation
commands/autonotepickup.py
Outdated
|
|
||
| def execute(self): | ||
| self.drive.arcadeDriveWithFactors( | ||
| 0, 0, self.vision.dRobotAngle, self.drive.CoordinateMode.RobotRelative |
There was a problem hiding this comment.
driveWithFactors gives the rotation in radians / second, so you have to PID to get to the correct angle
as implemented its a PID loop with P gain set to 1
commands/autonotepickup.py
Outdated
|
|
||
| def execute(self): | ||
| self.drive.arcadeDriveWithFactors( | ||
| 0.5, 0, 0, self.drive.CoordinateMode.RobotRelative |
There was a problem hiding this comment.
should still be actively trying to rotate here to ensure that if the note skids we still follow
constants.py
Outdated
| kNoteCameraYaw = 30 * kRadiansPerDegree | ||
| kNoteCameraHeight = 0.5 # meters | ||
| kRobotToNoteCameraTransform = Transform3d( | ||
| Pose3d(), Pose3d(0.1, -0.1, 0.5, Rotation3d(0, kNoteCameraPitch, kNoteCameraYaw)) |
There was a problem hiding this comment.
what Euler order axis is Rotation3d? if its not yaw then pitch then the rotation won't do what you expect
is the note camera position proper? please take from cad
There was a problem hiding this comment.
roll pitch yaw, all the numbers rn are placeholder
constants.py
Outdated
| @@ -236,6 +237,16 @@ | |||
| kPhotonvisionBackRightCameraKey, | |||
| ] | |||
|
|
|||
| kPhotonvisionNoteCameraKey = "noteCamera" | |||
| kNoteInViewKey = "noteInView" | |||
There was a problem hiding this comment.
update these to use folders
also use the OptionalValueKeys class we already established for this type of purpose
subsystems/visionsubsystem.py
Outdated
| @@ -121,6 +154,15 @@ def updateAdvantagescopePose( | |||
| ) | |||
| SmartDashboard.putNumberArray(cameraKey, cameraPose) | |||
|
|
|||
| def getNoteToCamera(self, note: PhotonTrackedTarget) -> Pose2d: | |||
| x = constants.kNoteCameraHeight / tan( | |||
There was a problem hiding this comment.
these constants for camera height should be able to come from the transform's components themselves
subsystems/visionsubsystem.py
Outdated
| dist = (constants.kNoteCameraHeight**2 + x**2) ** 0.5 | ||
| y = dist * tan(-note.getYaw() * constants.kRadiansPerDegree) | ||
|
|
||
| return Pose2d(x, y, atan(y / x)) |
There was a problem hiding this comment.
since the function says note --> camera it should be a transform2d, not a pose2d
commands/autonotepickup.py
Outdated
| < constants.kAutoNotePickupAngleTolerance.radians() | ||
| ): | ||
| self.drive.arcadeDriveWithFactors( | ||
| 0.5, 0, angleOutput, self.drive.CoordinateMode.RobotRelative |
There was a problem hiding this comment.
move the 0.5 to a constant
There was a problem hiding this comment.
or better, have a drive controller that starts with the current speed of the robot so we don't immediately come to a screeching halt with speed the second we say auto intake
subsystems/visionsubsystem.py
Outdated
| SmartDashboard.putBoolean(constants.kNoteInViewKey, True) | ||
| else: | ||
| # rotate around if no note in vision | ||
| self.dRobotAngle = Rotation2d(0.1) |
There was a problem hiding this comment.
would be better to just finish silently if nothing detected or go to a standard driving instead of "search"
lmaxwell24
force-pushed
the
note-detection
branch
from
fb8c9a6 to
cccbd78
Compare
No description provided.