commands.auto.pid_to_angle

  1# Native imports
  2from math import inf, pi
  3from typing import Callable
  4
  5# Internal imports
  6from config import OperatorRobotConfig
  7from constants import SwerveDriveConsts
  8from subsystem.drivetrain.swerve_drivetrain import SwerveDrivetrain
  9
 10# Third-party imports
 11from commands2 import Command
 12from wpimath import applyDeadband
 13from wpimath.controller import ProfiledPIDController
 14from wpimath.geometry import Rotation2d, Translation2d
 15from wpimath.kinematics import ChassisSpeeds
 16from wpimath.trajectory import TrapezoidProfile
 17
 18
 19class PIDAlignToTarget(Command):
 20    def __init__(
 21        self,
 22        drivetrain: SwerveDrivetrain,
 23        target_location: Callable[[], Translation2d] | None,
 24        velocity_vector_x: Callable[[], float],
 25        velocity_vector_y: Callable[[], float],
 26        field: Callable[[], bool],
 27        rotation_pid_config: tuple = OperatorRobotConfig.pid_to_pose_rotation_pid_profile,
 28        setpoint_tolerances: tuple = OperatorRobotConfig.pid_to_pose_setpoint_tolerances,
 29        alignment_angle: Rotation2d = Rotation2d.fromDegrees(0)
 30    ) -> None:
 31        """
 32        Align rotationally to a target location using PID. In this control system, the objective function is the
 33        absolute error between the current and goal omega value. This objective function is minimized
 34        until rotational error is within a given tolerance. The decision variable of the control system
 35        is rotational velocity of the drivetrain. The contraint is a trapezoidal motion profile for
 36        rotational motion, where slopes represent max acceleration and the plateaus represent max
 37        velocity.
 38
 39        Note that the robot will rotate to the target orientation as quickly as possible.
 40
 41        Args:
 42            drivetrain: the drivetrain subsystem that effects movement from current rotation to target rotation
 43            target_location: the desired always-blue location to which the robot's heading should align
 44            velocity_vector_x: live poll of the percentage of max translation velocity that the
 45                drivetrain should go in the X (+ forward-backward -) direction. Has
 46                domain [-1, 1]
 47            velocity_vector_y: live poll of the percentage of max translation velocity that the
 48                drivetrain should go in the Y (+ left-right -) direction. Has
 49                domain [-1, 1]
 50            field: live poll. if True, the drivetrain should move in field relative mode. If False,
 51                the robot should move in robot relative mode
 52            rotation_pid_config: the PID and trapezoidal profile constants for rotational motion
 53            setpoint_tolerances: the threshold for omega below which absolute
 54                rotational error is low enough for the robot to be considered arrived at the target rotation
 55            alignment_angle: which angle, CCW positive, offset the robot's base heading to align
 56                with the target
 57
 58        Returns:
 59            None: class initialization executed upon construction
 60        """
 61        super().__init__()
 62
 63        self.drivetrain = drivetrain
 64        self.target_location = target_location
 65        self.velocity_vector_x = velocity_vector_x
 66        self.velocity_vector_y = velocity_vector_y
 67        self.field = field
 68        self.rotation_pid = ProfiledPIDController(
 69            *rotation_pid_config[0:3], TrapezoidProfile.Constraints(*rotation_pid_config[3:5])
 70        )
 71        self.alignment_angle = alignment_angle
 72
 73        self.rotation_pid.enableContinuousInput(-pi, pi)
 74        self.rotation_pid.setTolerance(setpoint_tolerances[2])
 75
 76        self.target_rotation = None
 77
 78        self.addRequirements(self.drivetrain)
 79
 80    def execute(self) -> None:
 81        """
 82        If a target rotation is available, run the profiled PID control system to move from the
 83        current rotation to the target rotation. When no target is available, pass through
 84        driver translation inputs with zero rotation so the drivetrain is not left uncontrolled.
 85
 86        Returns:
 87            None: translational and rotational velocities are passed into the drivetrain's drive
 88                method, which runs the motors accordingly
 89        """
 90        target = self.target_location() if self.target_location else None
 91        if target is None:
 92            self.target_rotation = None
 93            self.drivetrain.drive(
 94                self.velocity_vector_x() * SwerveDriveConsts.maxTranslationMPS,
 95                self.velocity_vector_y() * SwerveDriveConsts.maxTranslationMPS,
 96                0,
 97                self.field()
 98            )
 99            return
100
101        self.target_rotation = (target - self.drivetrain.current_pose().translation()).angle()
102        current_rotation = self.drivetrain.current_pose().rotation()
103        offset_current_rotation = current_rotation + self.alignment_angle
104
105        rotation_error = (self.target_rotation - offset_current_rotation).radians()
106        rotation_output = -applyDeadband(self.rotation_pid.calculate(rotation_error, 0), 0.04, inf)
107
108        new_rotation_velocity = ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, rotation_output, current_rotation)
109        self.drivetrain.drive(
110            self.velocity_vector_x() * SwerveDriveConsts.maxTranslationMPS,
111            self.velocity_vector_y() * SwerveDriveConsts.maxTranslationMPS,
112            new_rotation_velocity.omega,
113            self.field()
114        )
115
116    def end(self, interrupted: bool) -> None:
117        """
118        When this command is over, have the robot stop driving (set all velocities to zero).
119
120        Args:
121            interrupted: whether the command was terminated by way of interruption
122
123        Returns:
124            None: drivetrain subsystem will apply zeroed velocities to motors
125        """
126        self.drivetrain.stop_driving(apply_to_modules=True)
127
128    def isFinished(self) -> bool:
129        """
130        The command is considered complete when the absolute error of each coodinate is within tolerance.
131        If there is no target rotation for the robot to navigate to, this command instantly ends.
132
133        Returns:
134            True if the command is complete according to the definition above, False otherwise.
135        """
136        at_setpoints = self.rotation_pid.atSetpoint()
137        no_target = self.target_rotation is None
138        return at_setpoints or no_target