class RobotContainer:
"""
This class is where the bulk of the robot should be declared. Since Command-based is a
"declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot`
periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
subsystems, commands, and button mappings) should be declared here.
"""
def __init__(self) -> None:
# The robot's subsystems and commands are defined here...
self.drivetrain = Drivetrain()
self.onboardIO = OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT)
# Assumes a gamepad plugged into channnel 0
self.controller = wpilib.Joystick(0)
# Create SmartDashboard chooser for autonomous routines
self.chooser = wpilib.SendableChooser()
# NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
# that is specified when launching the wpilib-ws server on the Romi raspberry pi.
# By default, the following are available (listed in order from inside of the board to outside):
# - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
# - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
# - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
# - PWM 2 (mapped to Arduino Pin 21)
# - PWM 3 (mapped to Arduino Pin 22)
#
# Your subsystem configuration should take the overlays into account
self._configureButtonBindings()
def _configureButtonBindings(self):
"""Use this method to define your button->command mappings. Buttons can be created by
instantiating a :class:`.GenericHID` or one of its subclasses (:class`.Joystick or
:class:`.XboxController`), and then passing it to a :class:`.JoystickButton`.
"""
# Default command is arcade drive. This will run unless another command
# is scheduler over it
self.drivetrain.setDefaultCommand(self.getArcadeDriveCommand())
# Example of how to use the onboard IO
onboardButtonA = commands2.button.Button(self.onboardIO.getButtonAPressed)
onboardButtonA.whenActive(
commands2.PrintCommand("Button A Pressed")
).whenInactive(commands2.PrintCommand("Button A Released"))
# Setup SmartDashboard options
self.chooser.setDefaultOption(
"Auto Routine Distance", AutonomousDistance(self.drivetrain)
)
self.chooser.addOption("Auto Routine Time", AutonomousTime(self.drivetrain))
wpilib.SmartDashboard.putData(self.chooser)
def getAutonomousCommand(self) -> typing.Optional[commands2.CommandBase]:
return self.chooser.getSelected()
def getArcadeDriveCommand(self) -> ArcadeDrive:
"""Use this to pass the teleop command to the main robot class.
:returns: the command to run in teleop
"""
return ArcadeDrive(
self.drivetrain,
lambda: self.controller.getRawAxis(1),
lambda: self.controller.getRawAxis(2),
)
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)))
