mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
dca59147e1
This also updates the bazel scripts to behave more like the C++ and Java examples, and updates the copybara scripts to be able to sync up `mostrobotpy`
76 lines
3.1 KiB
Python
76 lines
3.1 KiB
Python
#
|
|
# Copyright (c) FIRST and other WPILib contributors.
|
|
# Open Source Software; you can modify and/or share it under the terms of
|
|
# the WPILib BSD license file in the root directory of this project.
|
|
#
|
|
|
|
import commands2
|
|
import commands2.cmd
|
|
|
|
import subsystems.drivesubsystem
|
|
import subsystems.hatchsubsystem
|
|
|
|
import constants
|
|
|
|
|
|
class Autos:
|
|
"""Container for auto command factories."""
|
|
|
|
def __init__(self) -> None:
|
|
raise Exception("This is a utility class!")
|
|
|
|
@staticmethod
|
|
def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem):
|
|
"""A simple auto routine that drives forward a specified distance, and then stops."""
|
|
return commands2.FunctionalCommand(
|
|
# Reset encoders on command start
|
|
drive.resetEncoders,
|
|
# Drive forward while the command is executing,
|
|
lambda: drive.arcadeDrive(constants.kAutoDriveVelocity, 0),
|
|
# Stop driving at the end of the command
|
|
lambda interrupt: drive.arcadeDrive(0, 0),
|
|
# End the command when the robot's driven distance exceeds the desired value
|
|
lambda: drive.getAverageEncoderDistance()
|
|
>= constants.kAutoDriveDistanceInches,
|
|
# Require the drive subsystem
|
|
)
|
|
|
|
@staticmethod
|
|
def complexAuto(
|
|
driveSubsystem: subsystems.drivesubsystem.DriveSubsystem,
|
|
hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem,
|
|
):
|
|
"""A complex auto routine that drives forward, drops a hatch, and then drives backward."""
|
|
return commands2.cmd.sequence(
|
|
# Drive forward up to the front of the cargo ship
|
|
commands2.FunctionalCommand(
|
|
# Reset encoders on command start
|
|
driveSubsystem.resetEncoders,
|
|
# Drive forward while the command is executing
|
|
lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveVelocity, 0),
|
|
# Stop driving at the end of the command
|
|
lambda interrupt: driveSubsystem.arcadeDrive(0, 0),
|
|
# End the command when the robot's driven distance exceeds the desired value
|
|
lambda: driveSubsystem.getAverageEncoderDistance()
|
|
>= constants.kAutoDriveDistanceInches,
|
|
# Require the drive subsystem
|
|
driveSubsystem,
|
|
),
|
|
# Release the hatch
|
|
hatchSubsystem.releaseHatch(),
|
|
# Drive backward the specified distance
|
|
commands2.FunctionalCommand(
|
|
# Reset encoders on command start
|
|
driveSubsystem.resetEncoders,
|
|
# Drive backwards while the command is executing
|
|
lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveVelocity, 0),
|
|
# Stop driving at the end of the command
|
|
lambda interrupt: driveSubsystem.arcadeDrive(0, 0),
|
|
# End the command when the robot's driven distance exceeds the desired value
|
|
lambda: abs(driveSubsystem.getAverageEncoderDistance())
|
|
>= constants.kAutoBackupDistanceInches,
|
|
# Require the drive subsystem
|
|
driveSubsystem,
|
|
),
|
|
)
|