commands.autoDrive 

 1import commands2
 2import commands2.cmd
 3import wpilib
 4from subsystem.drivetrain.swerve_drivetrain import SwerveDrivetrain
 5from wpimath.geometry import Pose2d, Transform2d
 6
 7class AutoDrive(commands2.Command):
 8    targetPos = Pose2d()
 9    tolerance = Transform2d(0.5, 0.5, 0)
10    def __init__(self, timeS: float, speedX: float, speedY: float, dir: float, fieldRel: bool, drive: SwerveDrivetrain) -> None:
11        """Creates a new DriveDistance. This command will drive your your robot to a designated cordinate.
12        targetPos: The X and Y pos that the bot will drive to(in meters)
13        drive:  The drivetrain subsystem on which this command will run
14        """
15        super().__init__()
16        self.speedX = speedX
17        self.speedY = speedY
18        self.dir = dir
19        self.drive = drive
20        self.timeS = timeS
21        self.fieldRel = fieldRel
22        self.addRequirements(drive)
23
24    def initialize(self) -> None:
25        """Called when the command is initially scheduled."""
26        self.drive.reset_heading()
27        self.timer = wpilib.Timer()
28        self.timer.start()
29    def execute(self) -> None:
30        """Called every time the scheduler runs while the command is scheduled."""
31        self.drive.drive(self.speedX, self.speedY, self.dir, self.fieldRel)
32        self.drive.update_pose_estimator()
33
34    def end(self, interrupted: bool) -> None:
35        """Called once the command ends or is interrupted."""
36        self.drive.drive(0,0,0,False)
37
38    def isFinished(self) -> bool:
39        """Returns true when the command should end."""
40        # Compare distance travelled from start to desired distance
41        return self.timer.hasElapsed(self.timeS)
class AutoDrive(commands2.command.Command): 
 8class AutoDrive(commands2.Command):
 9    targetPos = Pose2d()
10    tolerance = Transform2d(0.5, 0.5, 0)
11    def __init__(self, timeS: float, speedX: float, speedY: float, dir: float, fieldRel: bool, drive: SwerveDrivetrain) -> None:
12        """Creates a new DriveDistance. This command will drive your your robot to a designated cordinate.
13        targetPos: The X and Y pos that the bot will drive to(in meters)
14        drive:  The drivetrain subsystem on which this command will run
15        """
16        super().__init__()
17        self.speedX = speedX
18        self.speedY = speedY
19        self.dir = dir
20        self.drive = drive
21        self.timeS = timeS
22        self.fieldRel = fieldRel
23        self.addRequirements(drive)
24
25    def initialize(self) -> None:
26        """Called when the command is initially scheduled."""
27        self.drive.reset_heading()
28        self.timer = wpilib.Timer()
29        self.timer.start()
30    def execute(self) -> None:
31        """Called every time the scheduler runs while the command is scheduled."""
32        self.drive.drive(self.speedX, self.speedY, self.dir, self.fieldRel)
33        self.drive.update_pose_estimator()
34
35    def end(self, interrupted: bool) -> None:
36        """Called once the command ends or is interrupted."""
37        self.drive.drive(0,0,0,False)
38
39    def isFinished(self) -> bool:
40        """Returns true when the command should end."""
41        # Compare distance travelled from start to desired distance
42        return self.timer.hasElapsed(self.timeS)

A state machine representing a complete action to be performed by the robot. Commands are run by the commands2.CommandScheduler, and can be composed into CommandGroups to allow users to build complicated multistep actions without the need to roll the state machine logic themselves.

Commands are run synchronously from the main robot loop; no multithreading is used, unless specified explicitly from the command implementation.

AutoDrive( timeS: float, speedX: float, speedY: float, dir: float, fieldRel: bool, drive: subsystem.drivetrain.swerve_drivetrain.SwerveDrivetrain) 
11    def __init__(self, timeS: float, speedX: float, speedY: float, dir: float, fieldRel: bool, drive: SwerveDrivetrain) -> None:
12        """Creates a new DriveDistance. This command will drive your your robot to a designated cordinate.
13        targetPos: The X and Y pos that the bot will drive to(in meters)
14        drive:  The drivetrain subsystem on which this command will run
15        """
16        super().__init__()
17        self.speedX = speedX
18        self.speedY = speedY
19        self.dir = dir
20        self.drive = drive
21        self.timeS = timeS
22        self.fieldRel = fieldRel
23        self.addRequirements(drive)

Creates a new DriveDistance. This command will drive your your robot to a designated cordinate. targetPos: The X and Y pos that the bot will drive to(in meters) drive: The drivetrain subsystem on which this command will run

targetPos = Pose2d(Translation2d(x=0.000000, y=0.000000), Rotation2d(0.000000))
tolerance = Transform2d(Translation2d(x=0.500000, y=0.500000), Rotation2d(0.000000))
speedX
speedY
dir
drive
timeS
fieldRel
def initialize(self) -> None: 
25    def initialize(self) -> None:
26        """Called when the command is initially scheduled."""
27        self.drive.reset_heading()
28        self.timer = wpilib.Timer()
29        self.timer.start()

Called when the command is initially scheduled.

def execute(self) -> None: 
30    def execute(self) -> None:
31        """Called every time the scheduler runs while the command is scheduled."""
32        self.drive.drive(self.speedX, self.speedY, self.dir, self.fieldRel)
33        self.drive.update_pose_estimator()

Called every time the scheduler runs while the command is scheduled.

def end(self, interrupted: bool) -> None: 
35    def end(self, interrupted: bool) -> None:
36        """Called once the command ends or is interrupted."""
37        self.drive.drive(0,0,0,False)

Called once the command ends or is interrupted.

def isFinished(self) -> bool: 
39    def isFinished(self) -> bool:
40        """Returns true when the command should end."""
41        # Compare distance travelled from start to desired distance
42        return self.timer.hasElapsed(self.timeS)

Returns true when the command should end.