Note detection (#57)

* note positions from note cam

hopefully correct

* calculate necessary robot rotation

* auto note pickup command

* fix stuff

* merge commands

* make types compatible

* real numbers

* stop command when no note in vision, drive pid

* fix pylint

---------

Co-authored-by: Luke Maxwell <[email protected]>

.pylintrc

@@ -296,7 +296,7 @@ indent-string='    '
 max-line-length=100
 
 # Maximum number of lines in a module.
-max-module-lines=1000
+max-module-lines=1100
 
 # Allow the body of a class to be on the same line as the declaration if body
 # contains single statement.

commands/autonotepickup.py

@@ -0,0 +1,73 @@
+from commands2 import Command
+from wpilib import SmartDashboard
+from wpimath.controller import PIDController
+from wpimath.filter import Debouncer
+from subsystems.drivesubsystem import DriveSubsystem
+from subsystems.intakesubsystem import IntakeSubsystem
+from subsystems.elevatorsubsystem import ElevatorSubsystem
+from subsystems.visionsubsystem import VisionSubsystem
+
+
+import constants
+
+
+class AutoNotePickup(Command):
+    def __init__(
+        self,
+        drive: DriveSubsystem,
+        vision: VisionSubsystem,
+        intake: IntakeSubsystem,
+        elevator: ElevatorSubsystem,
+    ) -> None:
+        Command.__init__(self)
+        self.setName(__class__.__name__)
+        self.drive = drive
+        self.vision = vision
+        self.intake = intake
+        self.elevator = elevator
+        self.addRequirements(self.drive, self.vision, self.intake, self.elevator)
+        self.rotationPID = PIDController(
+            constants.kRotationPGain, constants.kRotationIGain, constants.kRotationDGain
+        )
+        self.drivePID = PIDController(
+            constants.kAutoNotePickupPGain,
+            constants.kAutoNotePickupIGain,
+            constants.kAutoNotePickupDGain,
+        )
+        self.debouncer = Debouncer(
+            constants.kNoteCameraDebounceTime, Debouncer.DebounceType.kFalling
+        )
+
+    def execute(self):
+        self.intake.setIntaking()
+        self.elevator.setBottomPosition()
+
+        angleOutput = self.rotationPID.calculate(self.vision.dRobotAngle)
+        driveOutput = self.drivePID.calculate(
+            (
+                self.drive.getRobotRelativeSpeeds().vx ** 2
+                + self.drive.getRobotRelativeSpeeds().vy ** 2
+            )
+            ** (1 / 2)
+            / constants.kMaxWheelLinearVelocity,
+            constants.kMaxAutoNotePickupSpeed,
+        )
+
+        if (
+            abs(self.vision.dRobotAngle.radians())
+            < constants.kAutoNotePickupAngleTolerance.radians()
+        ):
+            self.drive.arcadeDriveWithFactors(
+                driveOutput, 0, angleOutput, self.drive.CoordinateMode.RobotRelative
+            )
+        else:
+            self.drive.arcadeDriveWithFactors(
+                0, 0, angleOutput, self.drive.CoordinateMode.RobotRelative
+            )
+
+    def isFinished(self) -> bool:
+        return SmartDashboard.getBoolean(
+            constants.kIntakeHasNoteKey, False
+        ) or self.debouncer.calculate(
+            SmartDashboard.getBoolean(constants.kNoteInViewKey, False)
+        )

constants.py

@@ -31,6 +31,7 @@
     Rotation2d,
     Rotation3d,
     Transform3d,
+    Transform2d,
     Translation2d,
 )
 from wpimath.system.plant import DCMotor
@@ -238,6 +239,18 @@
     kPhotonvisionBackRightCameraKey,
 ]
 
+kPhotonvisionNoteCameraKey = "noteCamera"
+kNoteInViewKey = OptionalValueKeys("noteInView")
+
+kNoteCameraPitch = 30 * kRadiansPerDegree  # below horizontal
+kNoteCameraYaw = 20 * kRadiansPerDegree
+kRobotToNoteCameraTransform = Transform3d(
+    Pose3d(),
+    Pose3d(
+        0.330296, -0.333443, 0.570646, Rotation3d(0, kNoteCameraPitch, kNoteCameraYaw)
+    ),
+)
+
 kCameraFOVHorizontal = 75.9  # degrees
 kCameraFOVVertical = 47.4  # degrees
 
@@ -974,3 +987,19 @@
     -2 * math.pi / 3,
 )
 kWaypointsRed = [kAmpWaypointRed, kSpeakerWaypointRed, kSourceWaypointRed]
+
+kRobotToIntakePickupTransform = Transform2d(
+    Pose2d(),
+    Pose2d(
+        0.446088,
+        0.222375 - 0.342900 / 2,
+        Rotation2d.fromDegrees(math.tan((0.222375 - 0.342900 / 2) / 0.446088)),
+    ),
+)
+kAutoNotePickupAngleTolerance = Rotation2d.fromDegrees(5)
+kNoteCameraDebounceTime = 1  # seconds
+kMaxAutoNotePickupSpeed = 0.5  # 0 to 1
+
+kAutoNotePickupPGain = 0.001
+kAutoNotePickupIGain = 0
+kAutoNotePickupDGain = 0

subsystems/visionsubsystem.py

@@ -1,10 +1,11 @@
 from collections import deque
-from math import hypot, sin
+from math import hypot, sin, tan, atan
 
 # import numpy as np
 
 from commands2 import Subsystem
 from photonlibpy.photonCamera import PhotonCamera
+from photonlibpy.photonTrackedTarget import PhotonTrackedTarget
 from wpilib import SmartDashboard
 from wpilib import RobotBase, Timer
 from wpimath.geometry import (
@@ -13,6 +14,7 @@
     Pose2d,
     Rotation2d,
     Translation3d,
+    Transform2d,
     Rotation3d,
 )
 
@@ -64,7 +66,14 @@ def __init__(self) -> None:
             PhotonCamera(camera) for camera in constants.kPhotonvisionCameraArray
         ]
         self.robotToTags = []
+
+        self.noteCamera = PhotonCamera(constants.kPhotonvisionNoteCameraKey)
+
         self.poseList = deque([])
+
+        self.dRobotAngle = Rotation2d()
+
+        SmartDashboard.putBoolean(constants.kNoteInViewKey, False)
         # if RobotBase.isSimulation():
         #     inst = NetworkTableInstance.getDefault()
         #     inst.stopServer()
@@ -80,6 +89,30 @@ def periodic(self) -> None:
         robotPose = SmartDashboard.getNumberArray(
             constants.kRobotPoseArrayKeys.valueKey, [0, 0, 0]
         )
+
+        noteResult = self.noteCamera.getLatestResult()
+        if noteResult.hasTargets():
+            notes = noteResult.getTargets()
+            notePositions = [
+                Pose3d(robotPose[0], robotPose[1], 0, Rotation3d(0, 0, robotPose[2]))
+                + constants.kRobotToNoteCameraTransform
+                + self.getCameraToNote(note)
+                for note in notes
+            ]
+            closestNote = Pose2d(*robotPose).nearest(notePositions)
+            intakePickupPosition = robotPose + constants.kRobotToIntakePickupTransform
+
+            # angle robot needs to rotate by to pick up note by driving forward
+            self.dRobotAngle = (
+                Rotation2d(robotPose[2])
+                - Transform2d(intakePickupPosition, closestNote).rotation()
+            )
+
+            SmartDashboard.putBoolean(constants.kNoteInViewKey, True)
+        else:
+            # rotate around if no note in vision
+            SmartDashboard.putBoolean(constants.kNoteInViewKey, False)
+
         combinedPose = pose3dFrom2d(Pose2d(visionPose[0], visionPose[1], robotPose[2]))
         self.robotToTags = []
         for camera in self.cameras:
@@ -147,6 +180,15 @@ def updateAdvantagescopePose(
         )
         SmartDashboard.putNumberArray(cameraKey, cameraPose)
 
+    def getCameraToNote(self, note: PhotonTrackedTarget) -> Transform3d:
+        x = constants.kRobotToNoteCameraTransform.Z() / tan(
+            constants.kNoteCameraPitch - note.getPitch() * constants.kRadiansPerDegree
+        )
+        dist = (constants.kRobotToNoteCameraTransform.Z() ** 2 + x**2) ** 0.5
+        y = dist * tan(-note.getYaw() * constants.kRadiansPerDegree)
+
+        return Transform3d(x, y, 0, Rotation3d(0, 0, atan(y / x)))
+
 
 class CameraTargetRelation:
     def __init__(self, cameraPose: Pose3d, targetPose: Pose3d) -> None: