commands.auto.pid_to_pose

  1# Native imports
  2from math import inf, pi
  3from typing import Callable
  4
  5# Internal imports
  6from config import OperatorRobotConfig
  7from subsystem.drivetrain.swerve_drivetrain import SwerveDrivetrain
  8
  9# Third-party imports
 10from commands2 import Command
 11from wpimath import applyDeadband
 12from wpimath.controller import ProfiledPIDController
 13from wpimath.geometry import Pose2d
 14from wpimath.kinematics import ChassisSpeeds
 15from wpimath.trajectory import TrapezoidProfile
 16
 17
 18class PIDToPose(Command):
 19    def __init__(
 20        self,
 21        drivetrain: SwerveDrivetrain,
 22        target_pose: Callable[[], Pose2d] | None,
 23        translation_pid_config: tuple = OperatorRobotConfig.pid_to_pose_translation_pid_profile,
 24        rotation_pid_config: tuple = OperatorRobotConfig.pid_to_pose_rotation_pid_profile,
 25        setpoint_tolerances: tuple = OperatorRobotConfig.pid_to_pose_setpoint_tolerances
 26    ) -> None:
 27        """
 28        Align to a target pose using PID. In this control system, the objective function is the
 29        absolute error between the current and goal x, y, and omega coordinates, separately. This
 30        objective function is minimized until all errors are within a given tolerance. The decision
 31        variables of the control system are the translational and rotational velocities of the
 32        drivetrain. The contraints are a trapezoidal motion profiles for translational and rotational
 33        motion, where slopes represent max acceleration and the plateaus represent max velocity.
 34
 35        Note that the robot will move in a straight line from its current position to the target
 36        pose and will rotate to the target orientation as quickly as possible.
 37
 38        Args:
 39            drivetrain: the drivetrain subsystem that effects movement from current pose to target pose
 40            target_pose: the desired always-blue position and orientation of the robot
 41            translation_pid_config: the PID and trapezoidal profile constants for translational motion
 42            rotation_pid_config: the PID and trapezoidal profile constants for rotational motion
 43            setpoint_tolerances: the thresholds for x, y, and omega, respectively, below which absolute
 44                positional error is low enough for the robot to be considered arrived at the target pose
 45
 46        Returns:
 47            None: class initialization executed upon construction
 48        """
 49        super().__init__()
 50
 51        self.drivetrain = drivetrain
 52        self.target_pose = target_pose
 53        self.x_translation_pid = ProfiledPIDController(
 54            *translation_pid_config[0:3], TrapezoidProfile.Constraints(*translation_pid_config[3:5])
 55        )
 56        self.y_translation_pid = ProfiledPIDController(
 57            *translation_pid_config[0:3], TrapezoidProfile.Constraints(*translation_pid_config[3:5])
 58        )
 59        self.rotation_pid = ProfiledPIDController(
 60            *rotation_pid_config[0:3], TrapezoidProfile.Constraints(*rotation_pid_config[3:5])
 61        )
 62
 63        self.rotation_pid.enableContinuousInput(-pi, pi)
 64
 65        self.x_translation_pid.setTolerance(setpoint_tolerances[0])
 66        self.y_translation_pid.setTolerance(setpoint_tolerances[1])
 67        self.rotation_pid.setTolerance(setpoint_tolerances[2])
 68
 69        self.addRequirements(self.drivetrain)
 70
 71    def execute(self) -> None:
 72        """
 73        If a target pose is available, run the profiled PID control system to move from the
 74        current pose to the target pose.
 75
 76        Returns:
 77            None: translational and rotational velocities are passed into the drivetrain's drive
 78                method, which runs the motors accordingly
 79        """
 80        if self.target_pose:
 81            current_pose = self.drivetrain.current_pose()
 82            
 83            x_pose_error = self.target_pose().X() - current_pose.X()
 84            y_pose_error = self.target_pose().Y() - current_pose.Y()
 85            rotation_error = (self.target_pose().rotation() - current_pose.rotation()).radians()
 86
 87            x_output = -applyDeadband(self.x_translation_pid.calculate(x_pose_error, 0), 0.04, inf)
 88            y_output = -applyDeadband(self.y_translation_pid.calculate(y_pose_error, 0), 0.04, inf)
 89            rotation_output = -applyDeadband(self.rotation_pid.calculate(rotation_error, 0), 0.04, inf)
 90
 91            drive_speed = ChassisSpeeds.fromFieldRelativeSpeeds(x_output, y_output, rotation_output, current_pose.rotation())
 92            self.drivetrain.drive(drive_speed.vx, drive_speed.vy, drive_speed.omega, False)
 93
 94    def end(self, interrupted: bool) -> None:
 95        """
 96        When this command is over, have the robot stop driving (set all velocities to zero).
 97
 98        Args:
 99            interrupted: whether the command was terminated by way of interruption
100
101        Returns:
102            None: drivetrain subsystem will apply zeroed velocities to motors
103        """
104        self.drivetrain.stop_driving(apply_to_modules=True)
105
106    def isFinished(self) -> bool:
107        """
108        The command is considered complete when the absolute error of each coodinate is within tolerance.
109        If there is no target pose for the robot to navigate to, this command instantly ends.
110
111        Returns:
112            True if the command is complete according to the definition above, False otherwise.
113        """
114        at_setpoints = self.x_translation_pid.atSetpoint() and self.y_translation_pid.atSetpoint() and self.rotation_pid.atSetpoint()
115        no_target_pose = True if not self.target_pose else False
116        return at_setpoints or no_target_pose