class RobotContainer:
"""
This class hosts the bulk of the robot's functions. Little robot logic needs to be
handled here or in the robot periodic methods, as this is a command-based system.
The structure (commands, subsystems, and button mappings) should be done here.
"""
def __init__(self):
# Create the driver's controller.
self.driverController = XboxController(constants.kDriverControllerPort)
# Create an instance of the drivetrain subsystem.
self.robotDrive = Drivetrain()
# Configure and set the button bindings for the driver's controller.
self.configureButtons()
# Set the default command for the drive subsystem. It's default command will allow
# the robot to drive with the controller.
self.robotDrive.setDefaultCommand(
RunCommand(
lambda: self.robotDrive.arcadeDrive(
-self.driverController.getRawAxis(1),
self.driverController.getRawAxis(2) * 0.65,
),
self.robotDrive,
))
def getAutonomousCommand(self):
"""Returns the command to be ran during the autonomous period."""
# Create a voltage constraint to ensure we don't accelerate too fast.
autoVoltageConstraint = DifferentialDriveVoltageConstraint(
SimpleMotorFeedforwardMeters(
constants.ksVolts,
constants.kvVoltSecondsPerMeter,
constants.kaVoltSecondsSquaredPerMeter,
),
constants.kDriveKinematics,
maxVoltage=10, # 10 volts max.
)
# Below will generate the trajectory using a set of programmed configurations
# Create a configuration for the trajctory. This tells the trajectory its constraints
# as well as its resources, such as the kinematics object.
config = TrajectoryConfig(
constants.kMaxSpeedMetersPerSecond,
constants.kMaxAccelerationMetersPerSecondSquared,
)
# Ensures that the max speed is actually obeyed.
config.setKinematics(constants.kDriveKinematics)
# Apply the previously defined voltage constraint.
config.addConstraint(autoVoltageConstraint)
# Start at the origin facing the +x direction.
initialPosition = Pose2d(0, 0, Rotation2d(0))
# Here are the movements we also want to make during this command.
# These movements should make an "S" like curve.
movements = [Translation2d(1, 1), Translation2d(2, -1)]
# End at this position, three meters straight ahead of us, facing forward.
finalPosition = Pose2d(3, 0, Rotation2d(0))
# An example trajectory to follow. All of these units are in meters.
self.exampleTrajectory = TrajectoryGenerator.generateTrajectory(
initialPosition,
movements,
finalPosition,
config,
)
# Below creates the RAMSETE command
ramseteCommand = RamseteCommand(
# The trajectory to follow.
self.exampleTrajectory,
# A reference to a method that will return our position.
self.robotDrive.getPose,
# Our RAMSETE controller.
RamseteController(constants.kRamseteB, constants.kRamseteZeta),
# A feedforward object for the robot.
SimpleMotorFeedforwardMeters(
constants.ksVolts,
constants.kvVoltSecondsPerMeter,
constants.kaVoltSecondsSquaredPerMeter,
),
# Our drive kinematics.
constants.kDriveKinematics,
# A reference to a method which will return a DifferentialDriveWheelSpeeds object.
self.robotDrive.getWheelSpeeds,
# The turn controller for the left side of the drivetrain.
PIDController(constants.kPDriveVel, 0, 0),
# The turn controller for the right side of the drivetrain.
PIDController(constants.kPDriveVel, 0, 0),
# A reference to a method which will set a specified
# voltage to each motor. The command will pass the two parameters.
self.robotDrive.tankDriveVolts,
# The subsystems the command should require.
[self.robotDrive],
)
# Reset the robot's position to the starting position of the trajectory.
self.robotDrive.resetOdometry(self.exampleTrajectory.initialPose())
# Return the command to schedule. The "andThen()" will halt the robot after
# the command finishes.
return ramseteCommand.andThen(
lambda: self.robotDrive.tankDriveVolts(0, 0))
def configureButtons(self):
"""Configure the buttons for the driver's controller"""
# We won't do anything with this button itself, so we don't need to
# define a variable.
(JoystickButton(
self.driverController,
XboxController.Button.kBumperRight.value).whenPressed(
lambda: self.robotDrive.setSlowMaxOutput(0.5)).whenReleased(
lambda: self.robotDrive.setNormalMaxOutput(1)))
