Merge pull request #218 from thedropbears/vision-limits

Author: LucienMorey

components/vision.py

@@ -22,9 +22,9 @@
 
 from components.chassis import ChassisComponent
 from utilities.caching import HasPerLoopCache, cache_per_loop
+from utilities.functions import clamp
 from utilities.game import APRILTAGS, apriltag_layout
 from utilities.rev import configure_through_bore_encoder
-from utilities.scalers import scale_value
 
 
 @wpiutil.wpistruct.make_wpistruct
@@ -37,6 +37,12 @@ class VisibleTag:
     range: float
 
 
+@dataclass
+class ServoOffsets:
+    neutral: Rotation2d
+    full_range: Rotation2d
+
+
 class VisualLocalizer(HasPerLoopCache):
     """
     This localizes the robot from AprilTags on the field,
@@ -49,12 +55,8 @@ class VisualLocalizer(HasPerLoopCache):
     # Time since the last target sighting we allow before informing drivers
     TIMEOUT = 1.0  # s
 
-    # Calibration constants for servo and encoder
-    # range between +/-135 deg
-    SERVO_HALF_ANGLE = math.radians(135)
-
     CAMERA_FOV = math.radians(
-        65
+        68
     )  # photon vision says 69.8, but we are being conservative
     CAMERA_MAX_RANGE = 4.0  # m
 
@@ -75,33 +77,46 @@ def __init__(
         # The name of the camera in PhotonVision.
         name: str,
         # Position of the camera relative to the center of the robot
-        pos: Translation3d,
-        # The camera rotation at its neutral position (ie centred).
-        rot: Rotation3d,
+        turret_pos: Translation3d,
+        # The turret rotation at its neutral position (ie centred).
+        turret_rot: Rotation2d,
+        # The camera relative to the turret (ie without servo rotation)
+        camera_offset: Translation3d,
+        # The camera pitch on the mount, relative to horizontal
+        camera_pitch: float,
         servo_id: int,
-        servo_offset: Rotation2d,
+        servo_offsets: ServoOffsets,
         encoder_id: int,
         encoder_offset: Rotation2d,
+        # Encoder rotations at min and max of desired rotation range
+        rotation_range: tuple[Rotation2d, Rotation2d],
         field: wpilib.Field2d,
         data_log: wpiutil.log.DataLog,
         chassis: ChassisComponent,
     ) -> None:
         super().__init__()
         self.camera = PhotonCamera(name)
-        self.encoder = wpilib.DutyCycleEncoder(encoder_id, math.tau, 0)
+        self.encoder = wpilib.DutyCycleEncoder(encoder_id, math.pi, -math.pi)
         configure_through_bore_encoder(self.encoder)
         # Offset of encoder in radians when facing forwards (the desired zero)
         # To find this value, manually point the camera forwards and record the encoder value
         # This has nothing to do with the servo - do it by hand!!
         self.encoder_offset = encoder_offset
 
-        # To find the servo offset, command the servo to neutral in test mode and record the encoder value
-        self.servo_offset = servo_offset
+        # To find the servo offsets, command the servo to neutral in test mode and record the encoder value
+        # Repeat for full range
+        self.servo_offsets = servo_offsets
+
+        relative_rotations = [(r - encoder_offset) for r in rotation_range]
+        self.min_rotation, self.max_rotation = relative_rotations
 
         self.servo = wpilib.Servo(servo_id)
-        self.pos = pos
-        self.robot_to_turret = Transform3d(pos, rot)
-        self.robot_to_turret_2d = Transform2d(pos.toTranslation2d(), rot.toRotation2d())
+        self.pos = turret_pos
+        self.robot_to_turret = Transform3d(turret_pos, Rotation3d(turret_rot))
+        self.robot_to_turret_2d = Transform2d(turret_pos.toTranslation2d(), turret_rot)
+        self.turret_to_camera = Transform3d(
+            camera_offset, Rotation3d(roll=0.0, pitch=camera_pitch, yaw=0.0)
+        )
         self.turret_rotation_buffer = TimeInterpolatableRotation2dBuffer(2.0)
 
         self.last_timestamp = -1.0
@@ -161,7 +176,8 @@ def visible_tags(self) -> list[VisibleTag]:
             distance = turret_to_tag.norm()
             relative_facing = tag.pose.toPose2d().rotation() - turret_to_tag.angle()
             if (
-                abs(relative_bearing.radians()) <= self.SERVO_HALF_ANGLE
+                (relative_bearing - self.min_rotation).radians() >= 0.0
+                and (self.max_rotation - relative_bearing).radians() >= 0.0
                 and abs(relative_facing.degrees()) > 100
                 and distance < self.CAMERA_MAX_RANGE
             ):
@@ -176,41 +192,53 @@ def desired_turret_rotation(self) -> Rotation2d:
         # Read encoder angle and account for offset
         return self.relative_bearing_to_best_cluster()
 
+    @feedback
+    def desired_servo_rotation(self) -> Rotation2d:
+        return self.turret_to_servo(self.desired_turret_rotation())
+
     def turret_to_servo(self, turret: Rotation2d) -> Rotation2d:
-        return turret - (self.servo_offset - self.encoder_offset)
+        return turret - (self.servo_offsets.neutral - self.encoder_offset)
 
     @property
     def turret_rotation(self) -> Rotation2d:
         return self.raw_encoder_rotation() - self.encoder_offset
 
     def robot_to_camera(self, timestamp: float) -> Transform3d:
-        robot_to_turret_rotation = self.robot_to_turret.rotation()
         turret_rotation = self.turret_rotation_buffer.sample(timestamp)
         if turret_rotation is None:
             return self.robot_to_turret
-        return Transform3d(
-            self.robot_to_turret.translation(),
-            Rotation3d(
-                robot_to_turret_rotation.x,
-                robot_to_turret_rotation.y,
-                robot_to_turret_rotation.z + turret_rotation.radians(),
-            ),
+        r2t = self.robot_to_turret.translation()
+        t2c = (
+            self.turret_to_camera.translation()
+            .rotateBy(self.turret_to_camera.rotation())
+            .rotateBy(Rotation3d(turret_rotation))
+        )
+        trans = r2t + t2c
+        rot = (
+            self.robot_to_turret.rotation()
+            .rotateBy(Rotation3d(turret_rotation))
+            .rotateBy(self.turret_to_camera.rotation())
         )
+        return Transform3d(trans, rot)
 
     def zero_servo_(self) -> None:
         # ONLY CALL THIS IN TEST MODE!
         # This is used to put the servo in a neutral position to record the encoder value at that point
         self.servo.set(0.5)
 
+    def full_range_servo_(self) -> None:
+        # ONLY CALL THIS IN TEST MODE!
+        # This is used to put the servo to the full range position to record the encoder value at that point
+        self.servo.set(1.0)
+
     def execute(self) -> None:
+        desired = self.turret_to_servo(self.desired_turret_rotation())
+        neutral = self.servo_offsets.neutral
+        full = self.servo_offsets.full_range
+        half_range = full - neutral
+        delta = desired - neutral
         self.servo.set(
-            scale_value(
-                self.turret_to_servo(self.desired_turret_rotation()).radians(),
-                -self.SERVO_HALF_ANGLE,
-                self.SERVO_HALF_ANGLE,
-                0.0,
-                1.0,
-            )
+            clamp((delta.radians() / half_range.radians() + 1.0) / 2.0, 0.0, 1.0)
         )
 
         self.turret_rotation_buffer.addSample(

physics.py

@@ -232,10 +232,13 @@ def update_sim(self, now: float, tm_diff: float) -> None:
         self.vision_encoder_sim.set(
             constrain_angle(
                 (
-                    (2.0 * self.visual_localiser.servo.getPosition() - 1.0)
-                    * self.visual_localiser.SERVO_HALF_ANGLE
-                )
-                + self.visual_localiser.servo_offset.radians()
+                    (
+                        self.visual_localiser.servo_offsets.full_range
+                        - self.visual_localiser.servo_offsets.neutral
+                    )
+                    * (2.0 * self.visual_localiser.servo.getPosition() - 1.0)
+                    + self.visual_localiser.servo_offsets.neutral
+                ).radians()
             )
         )
 

robot.py

@@ -1,3 +1,5 @@
+import math
+
 import magicbot
 import ntcore
 import wpilib
@@ -15,7 +17,7 @@
 from components.intake import IntakeComponent
 from components.led_component import LightStrip
 from components.shooter import ShooterComponent
-from components.vision import VisualLocalizer
+from components.vision import ServoOffsets, VisualLocalizer
 from components.wrist import WristComponent
 from controllers.algae_measurement import AlgaeMeasurement
 from controllers.algae_shooter import AlgaeShooter
@@ -92,7 +94,6 @@ def createObjects(self) -> None:
         self.vision_name = "ardu_cam"
         self.vision_encoder_id = DioChannel.VISION_ENCODER
         self.vision_servo_id = PwmChannel.VISION_SERVO
-
         if wpilib.RobotController.getSerialNumber() == RioSerialNumber.TEST_BOT:
             self.chassis_swerve_config = SwerveConfig(
                 drive_ratio=(14.0 / 50.0) * (25.0 / 19.0) * (15.0 / 45.0),
@@ -120,6 +121,14 @@ def createObjects(self) -> None:
             self.vision_rot = Rotation3d.fromDegrees(0, -20.0, 0)
             self.vision_servo_offset = Rotation2d(3.107)
             self.vision_encoder_offset = Rotation2d(3.052)
+            # TODO set on test bot
+            self.vision_camera_offset = Translation3d(0.05, 0, 0)
+            self.vision_camera_pitch = math.radians(-20.0)
+            self.vision_servo_offsets = ServoOffsets(
+                neutral=Rotation2d(0.5757), full_range=Rotation2d(1.9234)
+            )
+            self.vision_rotation_range = (Rotation2d(1.525), Rotation2d(4.33))
+
         else:
             self.chassis_swerve_config = SwerveConfig(
                 drive_ratio=(14.0 / 50.0) * (27.0 / 17.0) * (15.0 / 45.0),
@@ -143,10 +152,15 @@ def createObjects(self) -> None:
             # metres between centre of front and back wheels
             self.chassis_wheel_base = 0.517
 
-            self.vision_pos = Translation3d(-0.050, -0.300, 0.660)
-            self.vision_rot = Rotation3d.fromDegrees(0, 10.0, -135.0)
-            self.vision_servo_offset = Rotation2d(6.235)
-            self.vision_encoder_offset = Rotation2d(0.183795)
+            self.vision_turret_pos = Translation3d(-0.030, -0.300, 0.660)
+            self.vision_turret_rot = Rotation2d.fromDegrees(-90.0)
+            self.vision_camera_offset = Translation3d(0.021, 0, 0)
+            self.vision_camera_pitch = math.radians(10.0)
+            self.vision_encoder_offset = Rotation2d(0.9796)
+            self.vision_servo_offsets = ServoOffsets(
+                neutral=Rotation2d(0.044), full_range=Rotation2d(1.37)
+            )
+            self.vision_rotation_range = (Rotation2d(4.611), Rotation2d(2.020))
 
         self.coast_button = wpilib.DigitalInput(DioChannel.SWERVE_COAST_SWITCH)
         self.coast_button_pressed_event = wpilib.event.BooleanEvent(
@@ -273,8 +287,10 @@ def testPeriodic(self) -> None:
         # self.coral_placer_component.execute()
         self.shooter_component.execute()
         self.injector_component.execute()
-        if self.gamepad.getStartButton():
+        if self.gamepad.getLeftStickButton():
             self.vision.zero_servo_()
+        elif self.gamepad.getRightStickButton():
+            self.vision.full_range_servo_()
         else:
             self.vision.execute()
         self.wrist.execute()