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)))