Merge pull request #267 from thedropbears/revert-218-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,12 +37,6 @@ class VisibleTag:
     range: float
 
 
-@dataclass
-class ServoOffsets:
-    neutral: Rotation2d
-    full_range: Rotation2d
-
-
 class VisualLocalizer(HasPerLoopCache):
     """
     This localizes the robot from AprilTags on the field,
@@ -55,8 +49,12 @@ 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(
-        68
+        65
     )  # photon vision says 69.8, but we are being conservative
     CAMERA_MAX_RANGE = 4.0  # m
 
@@ -77,46 +75,33 @@ def __init__(
         # The name of the camera in PhotonVision.
         name: str,
         # Position of the camera relative to the center of the robot
-        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,
+        pos: Translation3d,
+        # The camera rotation at its neutral position (ie centred).
+        rot: Rotation3d,
         servo_id: int,
-        servo_offsets: ServoOffsets,
+        servo_offset: Rotation2d,
         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.pi, -math.pi)
+        self.encoder = wpilib.DutyCycleEncoder(encoder_id, math.tau, 0)
         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 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
+        # To find the servo offset, command the servo to neutral in test mode and record the encoder value
+        self.servo_offset = servo_offset
 
         self.servo = wpilib.Servo(servo_id)
-        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.pos = pos
+        self.robot_to_turret = Transform3d(pos, rot)
+        self.robot_to_turret_2d = Transform2d(pos.toTranslation2d(), rot.toRotation2d())
         self.turret_rotation_buffer = TimeInterpolatableRotation2dBuffer(2.0)
 
         self.last_timestamp = -1.0
@@ -176,8 +161,7 @@ def visible_tags(self) -> list[VisibleTag]:
             distance = turret_to_tag.norm()
             relative_facing = tag.pose.toPose2d().rotation() - turret_to_tag.angle()
             if (
-                (relative_bearing - self.min_rotation).radians() >= 0.0
-                and (self.max_rotation - relative_bearing).radians() >= 0.0
+                abs(relative_bearing.radians()) <= self.SERVO_HALF_ANGLE
                 and abs(relative_facing.degrees()) > 100
                 and distance < self.CAMERA_MAX_RANGE
             ):
@@ -192,53 +176,41 @@ 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_offsets.neutral - self.encoder_offset)
+        return turret - (self.servo_offset - 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
-        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(
+            self.robot_to_turret.translation(),
+            Rotation3d(
+                robot_to_turret_rotation.x,
+                robot_to_turret_rotation.y,
+                robot_to_turret_rotation.z + turret_rotation.radians(),
+            ),
         )
-        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(
-            clamp((delta.radians() / half_range.radians() + 1.0) / 2.0, 0.0, 1.0)
+            scale_value(
+                self.turret_to_servo(self.desired_turret_rotation()).radians(),
+                -self.SERVO_HALF_ANGLE,
+                self.SERVO_HALF_ANGLE,
+                0.0,
+                1.0,
+            )
         )
 
         self.turret_rotation_buffer.addSample(

physics.py

@@ -232,13 +232,10 @@ def update_sim(self, now: float, tm_diff: float) -> None:
         self.vision_encoder_sim.set(
             constrain_angle(
                 (
-                    (
-                        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()
+                    (2.0 * self.visual_localiser.servo.getPosition() - 1.0)
+                    * self.visual_localiser.SERVO_HALF_ANGLE
+                )
+                + self.visual_localiser.servo_offset.radians()
             )
         )
 

robot.py

@@ -1,5 +1,3 @@
-import math
-
 import magicbot
 import ntcore
 import wpilib
@@ -17,7 +15,7 @@
 from components.intake import IntakeComponent
 from components.led_component import LightStrip
 from components.shooter import ShooterComponent
-from components.vision import ServoOffsets, VisualLocalizer
+from components.vision import VisualLocalizer
 from components.wrist import WristComponent
 from controllers.algae_measurement import AlgaeMeasurement
 from controllers.algae_shooter import AlgaeShooter
@@ -94,6 +92,7 @@ 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),
@@ -121,14 +120,6 @@ 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),
@@ -152,15 +143,10 @@ def createObjects(self) -> None:
             # metres between centre of front and back wheels
             self.chassis_wheel_base = 0.517
 
-            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.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.coast_button = wpilib.DigitalInput(DioChannel.SWERVE_COAST_SWITCH)
         self.coast_button_pressed_event = wpilib.event.BooleanEvent(
@@ -287,10 +273,8 @@ def testPeriodic(self) -> None:
         # self.coral_placer_component.execute()
         self.shooter_component.execute()
         self.injector_component.execute()
-        if self.gamepad.getLeftStickButton():
+        if self.gamepad.getStartButton():
             self.vision.zero_servo_()
-        elif self.gamepad.getRightStickButton():
-            self.vision.full_range_servo_()
         else:
             self.vision.execute()
         self.wrist.execute()