:py:mod:`phoenix6.swerve.swerve_drivetrain`
===========================================

.. py:module:: phoenix6.swerve.swerve_drivetrain

Module Contents
---------------


Attributes
~~~~~~~~~~

.. autoapisummary::

   phoenix6.swerve.swerve_drivetrain.USE_WPILIB
   phoenix6.swerve.swerve_drivetrain.DriveMotorT
   phoenix6.swerve.swerve_drivetrain.SteerMotorT
   phoenix6.swerve.swerve_drivetrain.EncoderT


.. py:data:: USE_WPILIB
   :value: True

   

.. py:class:: SwerveControlParameters


   Contains everything the control requests need to calculate the module state.

   .. py:attribute:: drivetrain_id
      :value: 0

      ID of the native drivetrain instance, used for native calls


   .. py:attribute:: kinematics
      :type: wpimath.kinematics.SwerveDrive2Kinematics | wpimath.kinematics.SwerveDrive3Kinematics | wpimath.kinematics.SwerveDrive4Kinematics | wpimath.kinematics.SwerveDrive6Kinematics

      The kinematics object used for control


   .. py:attribute:: module_locations
      :type: list[Translation2d]

      The locations of the swerve modules


   .. py:attribute:: max_speed
      :type: phoenix6.units.meters_per_second
      :value: 0.0

      The max speed of the robot at 12 V output, in m/s


   .. py:attribute:: operator_forward_direction

      The forward direction from the operator perspective


   .. py:attribute:: current_chassis_speed

      The current robot-centric chassis speeds


   .. py:attribute:: current_pose

      The current pose of the robot


   .. py:attribute:: timestamp
      :type: phoenix6.units.second
      :value: 0.0

      The timestamp of the current control apply, in the timebase of utils.get_current_time_seconds()


   .. py:attribute:: update_period
      :type: phoenix6.units.second
      :value: 0.0

      The update period of control apply



.. py:data:: DriveMotorT

   

.. py:data:: SteerMotorT

   

.. py:data:: EncoderT

   

.. py:class:: SwerveDrivetrain(drive_motor_type: type[DriveMotorT], steer_motor_type: type[SteerMotorT], encoder_type: type[EncoderT], drivetrain_constants: phoenix6.swerve.swerve_drivetrain_constants.SwerveDrivetrainConstants, modules: list[phoenix6.swerve.swerve_module_constants.SwerveModuleConstants])
              SwerveDrivetrain(drive_motor_type: type[DriveMotorT], steer_motor_type: type[SteerMotorT], encoder_type: type[EncoderT], drivetrain_constants: phoenix6.swerve.swerve_drivetrain_constants.SwerveDrivetrainConstants, odometry_update_frequency: phoenix6.units.hertz, modules: list[phoenix6.swerve.swerve_module_constants.SwerveModuleConstants])
              SwerveDrivetrain(drive_motor_type: type[DriveMotorT], steer_motor_type: type[SteerMotorT], encoder_type: type[EncoderT], drivetrain_constants: phoenix6.swerve.swerve_drivetrain_constants.SwerveDrivetrainConstants, odometry_update_frequency: phoenix6.units.hertz, odometry_standard_deviation: tuple[float, float, float], vision_standard_deviation: tuple[float, float, float], modules: list[phoenix6.swerve.swerve_module_constants.SwerveModuleConstants])


   Bases: :py:obj:`Generic`\ [\ :py:obj:`DriveMotorT`\ , :py:obj:`SteerMotorT`\ , :py:obj:`EncoderT`\ ]

   Swerve Drive class utilizing CTR Electronics' Phoenix 6 API.

   This class handles the kinematics, configuration, and odometry of a
   swerve drive utilizing CTR Electronics devices. We recommend using
   the Swerve Project Generator in Tuner X to create a template project
   that demonstrates how to use this class.

   This class performs pose estimation internally using a separate odometry
   thread. Vision measurements can be added using add_vision_measurement.
   Other odometry APIs such as reset_pose are also available. The resulting
   pose estimate can be retrieved along with module states and other
   information using get_state. Additionally, the odometry thread synchronously
   provides all new state updates to a telemetry function registered with
   register_telemetry.

   This class will construct the hardware devices internally, so the user
   only specifies the constants (IDs, PID gains, gear ratios, etc).
   Getters for these hardware devices are available.

   If using the generator, the order in which modules are constructed is
   Front Left, Front Right, Back Left, Back Right. This means if you need
   the Back Left module, call get_module(2) to get the third (0-indexed)
   module.

   .. py:class:: SwerveDriveState


      Plain-Old-Data class holding the state of the swerve drivetrain.
      This encapsulates most data that is relevant for telemetry or
      decision-making from the Swerve Drive.

      .. py:attribute:: pose

         The current pose of the robot


      .. py:attribute:: speeds

         The current robot-centric velocity


      .. py:attribute:: module_states
         :type: list[SwerveModuleState]

         The current module states


      .. py:attribute:: module_targets
         :type: list[SwerveModuleState]

         The target module states


      .. py:attribute:: module_positions
         :type: list[SwerveModulePosition]

         The current module positions


      .. py:attribute:: raw_heading

         The raw heading of the robot, unaffected by vision updates and odometry resets


      .. py:attribute:: timestamp
         :type: phoenix6.units.second
         :value: 0.0

         The timestamp of the state capture, in the timebase of utils.get_current_time_seconds()


      .. py:attribute:: odometry_period
         :type: phoenix6.units.second
         :value: 0.0

         The measured odometry update period, in seconds


      .. py:attribute:: successful_daqs
         :value: 0

         Number of successful data acquisitions


      .. py:attribute:: failed_daqs
         :value: 0

         Number of failed data acquisitions



   .. py:class:: OdometryThread(drivetrain: int)


      Performs swerve module updates in a separate thread to minimize latency.

      :param drivetrain: ID of the swerve drivetrain
      :type drivetrain: int

      .. py:method:: start()

         Starts the odometry thread.


      .. py:method:: stop()

         Stops the odometry thread.


      .. py:method:: is_odometry_valid() -> bool

         Check if the odometry is currently valid.

         :returns: True if odometry is valid
         :rtype: bool


      .. py:method:: set_thread_priority(priority: int)

         Sets the odometry thread priority to a real time priority under the specified priority level

         :param priority: Priority level to set the odometry thread to.
                          This is a value between 0 and 99, with 99 indicating higher
                          priority and 0 indicating lower priority.
         :type priority: int



   .. py:property:: odometry_thread
      :type: OdometryThread

      Gets a reference to the odometry thread.

      :returns: Odometry thread
      :rtype: OdometryThread


   .. py:property:: kinematics
      :type: wpimath.kinematics.SwerveDrive2Kinematics | wpimath.kinematics.SwerveDrive3Kinematics | wpimath.kinematics.SwerveDrive4Kinematics | wpimath.kinematics.SwerveDrive6Kinematics

      Gets a reference to the kinematics used for the drivetrain.

      :returns: Swerve kinematics
      :rtype: SwerveDrive2Kinematics | SwerveDrive3Kinematics | SwerveDrive4Kinematics | SwerveDrive6Kinematics


   .. py:property:: modules
      :type: list[phoenix6.swerve.swerve_module.SwerveModule[DriveMotorT, SteerMotorT, EncoderT]]

      Get a reference to the full array of modules.
      The indexes correspond to the module described in the constructor.

      :returns: Reference to the SwerveModule array
      :rtype: list[SwerveModule[DriveMotorT, SteerMotorT, EncoderT]]


   .. py:property:: module_locations
      :type: list[Translation2d]

      Gets the locations of the swerve modules.

      :returns: Reference to the array of swerve module locations
      :rtype: list[Translation2d]


   .. py:property:: pigeon2
      :type: phoenix6.hardware.pigeon2.Pigeon2

      Gets this drivetrain's Pigeon 2 reference.

      This should be used only to access signals and change configurations that the
      swerve drivetrain does not configure itself.

      :returns: This drivetrain's Pigeon 2 reference
      :rtype: Pigeon2


   .. py:method:: close()

      Closes this SwerveDrivetrain instance.


   .. py:method:: update_sim_state(dt: phoenix6.units.second, supply_voltage: phoenix6.units.volt)

      Updates all the simulation state variables for this
      drivetrain class. User provides the update variables for the simulation.
          
      :param dt: time since last update call
      :type dt: second
      :param supply_voltage: voltage as seen at the motor controllers
      :type supply_voltage: volt


   .. py:method:: is_on_can_fd() -> bool

      Gets whether the drivetrain is on a CAN FD bus.

      :returns: True if on CAN FD
      :rtype: bool


   .. py:method:: get_odometry_frequency() -> phoenix6.units.hertz

      Gets the target odometry update frequency.

      :returns: Target odometry update frequency
      :rtype: hertz


   .. py:method:: is_odometry_valid() -> bool

      Check if the odometry is currently valid.

      :returns: True if odometry is valid
      :rtype: bool


   .. py:method:: set_control(request: phoenix6.swerve.requests.SwerveRequest)

      Applies the specified control request to this swerve drivetrain.

      :param request: Request to apply
      :type request: requests.SwerveRequest


   .. py:method:: get_state() -> SwerveDriveState

      Gets the current state of the swerve drivetrain.
      This includes information such as the pose estimate,
      module states, and chassis speeds.

      :returns: Current state of the drivetrain
      :rtype: SwerveDriveState


   .. py:method:: get_state_copy() -> SwerveDriveState

      Gets a copy of the current state of the swerve drivetrain.
      This includes information such as the pose estimate,
      module states, and chassis speeds.

      This can be used to get a thread-safe copy of the state object.

      :returns: Copy of the current state of the drivetrain
      :rtype: SwerveDriveState


   .. py:method:: register_telemetry(telemetry_function: Callable[[SwerveDriveState], None])

      Register the specified lambda to be executed whenever our SwerveDriveState function
      is updated in our odometry thread.

      It is imperative that this function is cheap, as it will be executed along with
      the odometry call, and if this takes a long time, it may negatively impact
      the odometry of this stack.

      This can also be used for logging data if the function performs logging instead of telemetry.
      Additionally, the SwerveDriveState object can be cloned and stored for later processing.

      :param telemetry_function: Function to call for telemetry or logging
      :type telemetry_function: Callable[[SwerveDriveState], None]


   .. py:method:: config_neutral_mode(neutral_mode: phoenix6.signals.spn_enums.NeutralModeValue) -> phoenix6.status_code.StatusCode

      Configures the neutral mode to use for all modules' drive motors.

      :param neutral_mode: The drive motor neutral mode
      :type neutral_mode: NeutralModeValue
      :returns: Status code of the first failed config call, or OK if all succeeded
      :rtype: StatusCode


   .. py:method:: tare_everything()

      Zero's this swerve drive's odometry entirely.

      This will zero the entire odometry, and place the robot at 0,0


   .. py:method:: seed_field_centric()

      Resets the rotation of the robot pose to 0 from the
      ForwardPerspectiveValue.OPERATOR_PERSPECTIVE perspective.
      This makes the current orientation of the robot X forward for
      field-centric maneuvers.

      This is equivalent to calling reset_rotation with the operator
      perspective rotation.


   .. py:method:: reset_pose(pose: Pose2d)

      Resets the pose of the robot. The pose should be from the
      ForwardPerspectiveValue.BLUE_ALLIANCE perspective.

      :param pose: Pose to make the current pose
      :type pose: Pose2d


   .. py:method:: reset_translation(translation: Translation2d)

      Resets the translation of the robot pose without affecting rotation.
      The translation should be from the ForwardPerspectiveValue.BLUE_ALLIANCE
      perspective.

      :param translation: Translation to make the current translation
      :type translation: Translation2d


   .. py:method:: reset_rotation(rotation: Rotation2d)

      Resets the rotation of the robot pose without affecting translation.
      The rotation should be from the ForwardPerspectiveValue.BLUE_ALLIANCE
      perspective.

      :param rotation: Rotation to make the current rotation
      :type rotation: Rotation2d


   .. py:method:: set_operator_perspective_forward(field_direction: Rotation2d)

      Takes the ForwardPerspectiveValue.BLUE_ALLIANCE perpective direction and treats
      it as the forward direction for ForwardPerspectiveValue.OPERATOR_PERSPECTIVE.

      If the operator is in the Blue Alliance Station, this should be 0 degrees.
      If the operator is in the Red Alliance Station, this should be 180 degrees.

      This does not change the robot pose, which is in the
      ForwardPerspectiveValue.BLUE_ALLIANCE perspective. As a result, the robot
      pose may need to be reset using reset_pose.

      :param field_direction: Heading indicating which direction is forward from
                              the ForwardPerspectiveValue.BLUE_ALLIANCE perspective
      :type field_direction: Rotation2d


   .. py:method:: get_operator_forward_direction() -> Rotation2d

      Returns the ForwardPerspectiveValue.BLUE_ALLIANCE perpective direction that is
      treated as the forward direction for ForwardPerspectiveValue.OPERATOR_PERSPECTIVE.

      If the operator is in the Blue Alliance Station, this should be 0 degrees.
      If the operator is in the Red Alliance Station, this should be 180 degrees.

      :returns: Heading indicating which direction is forward from
                the ForwardPerspectiveValue.BLUE_ALLIANCE perspective
      :rtype: Rotation2d


   .. py:method:: add_vision_measurement(vision_robot_pose: Pose2d, timestamp: phoenix6.units.second, vision_measurement_std_devs: tuple[float, float, float] | None = None)

      Adds a vision measurement to the Kalman Filter. This will correct the
      odometry pose estimate while still accounting for measurement noise.

      This method can be called as infrequently as you want.

      To promote stability of the pose estimate and make it robust to bad vision
      data, we recommend only adding vision measurements that are already within
      one meter or so of the current pose estimate.

      Note that the vision measurement standard deviations passed into this method
      will continue to apply to future measurements until a subsequent call to
      set_vision_measurement_std_devs or this method.

      :param vision_robot_pose:           The pose of the robot as measured by the vision
                                          camera.
      :type vision_robot_pose:            Pose2d
      :param timestamp:                   The timestamp of the vision measurement in
                                          seconds. Note that you must use a timestamp with
                                          an epoch since system startup (i.e., the epoch of
                                          this timestamp is the same epoch as
                                          utils.get_current_time_seconds).
                                          This means that you should use
                                          utils.get_current_time_seconds
                                          as your time source or sync the epochs.
                                          An FPGA timestamp can be converted to the correct
                                          timebase using utils.fpga_to_current_time.
      :type timestamp:                    second
      :param vision_measurement_std_devs: Standard deviations of the vision pose
                                          measurement (x position in meters, y
                                          position in meters, and heading in radians).
                                          Increase these numbers to trust the vision
                                          pose measurement less.
      :type vision_measurement_std_devs:  tuple[float, float, float] | None


   .. py:method:: set_vision_measurement_std_devs(vision_measurement_std_devs: tuple[float, float, float])

      Sets the pose estimator's trust of global measurements. This might be used to
      change trust in vision measurements after the autonomous period, or to change
      trust as distance to a vision target increases.

      :param vision_measurement_std_devs: Standard deviations of the vision
                                          measurements. Increase these numbers to
                                          trust global measurements from vision less.
                                          This matrix is in the form [x, y, theta]ᵀ,
                                          with units in meters and radians.
      :type vision_measurement_std_devs:  tuple[float, float, float]


   .. py:method:: set_state_std_devs(state_std_devs: tuple[float, float, float])

      Sets the pose estimator's trust in robot odometry. This might be used to change
      trust in odometry after an impact with the wall or traversing a bump.

      :param state_std_devs: Standard deviations of the pose estimate. Increase these
                             numbers to trust your state estimate less. This matrix is
                             in the form [x, y, theta]ᵀ, with units in meters and radians.
      :type state_std_devs:  tuple[float, float, float]


   .. py:method:: sample_pose_at(timestamp: phoenix6.units.second) -> Pose2d | None

      Return the pose at a given timestamp, if the buffer is not empty.

      :param timestamp: The pose's timestamp. Note that you must use a timestamp
                        with an epoch since system startup (i.e., the epoch of
                        this timestamp is the same epoch as
                        utils.get_current_time_seconds). This means that you
                        should use utils.get_current_time_seconds as your
                        time source in this case.
                        An FPGA timestamp can be converted to the correct
                        timebase using utils.fpga_to_current_time.
      :type timestamp: second
      :returns: The pose at the given timestamp (or None if the buffer is empty).
      :rtype: Pose2d | None


   .. py:method:: get_module(index: int) -> phoenix6.swerve.swerve_module.SwerveModule[DriveMotorT, SteerMotorT, EncoderT]

      Get a reference to the module at the specified index.
      The index corresponds to the module described in the constructor.

      :param index: Which module to get
      :type index: int
      :returns: Reference to SwerveModule
      :rtype: SwerveModule[DriveMotorT, SteerMotorT, EncoderT]


   .. py:method:: get_rotation3d() -> wpimath.geometry.Rotation3d

      Gets the current orientation of the robot as a Rotation3d from
      the Pigeon 2 quaternion values.

      :returns: The robot orientation as a Rotation3d
      :rtype: Rotation3d