commands.default_swerve_drive

 1# Native imports
 2import math
 3from typing import Callable
 4
 5# Internal imports
 6from constants import SwerveDriveConsts
 7from subsystem.drivetrain.swerve_drivetrain import SwerveDrivetrain
 8
 9# Third-party imports
10import commands2
11
12class DefaultDrive(commands2.Command):
13    """
14    Base command to drive a swerve drive robot using an Xbox controller. This should
15    be given as the default command for the swerve drivetrain subsystem.
16    """
17    def __init__ (
18        self,
19        drivetrain: SwerveDrivetrain,
20        velocity_vector_x: Callable[[], float],
21        velocity_vector_y: Callable[[], float],
22        angular_velocity: Callable[[], float],
23        field: Callable[[], bool]
24    ) -> None:
25        """
26        Store joystick and button inputs within the object, and require the swerve drivetrain
27        subsystem when executing this command.
28
29        Args:
30            drivetrain: the swerve drivetrain subsystem that this command with operate
31            velocity_vector_x: live poll of the percentage of max translation velocity that the
32                drivetrain should go in the X (+ forward-backward -) direction. Has
33                domain [-1, 1]
34            velocity_vector_y: live poll of the percentage of max translation velocity that the
35                drivetrain should go in the Y (+ left-right -) direction. Has
36                domain [-1, 1]
37            angular_velocity: live poll of the percentage of max angular velocity that the drivetrain
38                should go in CCW direction. Has domain [-1, 1]
39            field: live poll. if True, the drivetrain should move in field relative mode. If False,
40                the robot should move in robot relative mode
41
42        Returns:
43            None: class initialization executed upon construction
44        """
45        super().__init__()
46
47        self.drivetrain = drivetrain
48        self.velocity_vector_x = velocity_vector_x
49        self.velocity_vector_y = velocity_vector_y
50        self.angular_velocity = angular_velocity
51        self.field = field
52        self.addRequirements(self.drivetrain)
53
54    def execute(self) -> None:
55        """
56        Take the controller input poll functions and transform them into inputs for the
57        swerve drivetrain's drive interface. Execute drive functionality using those inputs.
58
59        Returns:
60            None: interface eventually passes desired goal states to the swerve modules
61        """
62        self.drivetrain.drive(
63            self.velocity_vector_x() * SwerveDriveConsts.maxTranslationMPS,
64            self.velocity_vector_y() * SwerveDriveConsts.maxTranslationMPS,
65            self.angular_velocity() * math.radians(SwerveDriveConsts.maxAngularDPS),
66            self.field()
67        )