mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
96 lines
3.5 KiB
Python
96 lines
3.5 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 wpilib
|
|
|
|
import commands2
|
|
import commands2.button
|
|
import commands2.cmd
|
|
|
|
import constants
|
|
|
|
import commands.autos
|
|
|
|
from subsystems.drivesubsystem import DriveSubsystem
|
|
from subsystems.hatchsubsystem import HatchSubsystem
|
|
|
|
|
|
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
|
|
self.driveSubsystem = DriveSubsystem()
|
|
self.hatchSubsystem = HatchSubsystem()
|
|
|
|
# Retained command handles
|
|
|
|
# A simple auto routine that drives forward a specified distance, and then stops.
|
|
self.simpleAuto = commands.autos.Autos.simpleAuto(self.driveSubsystem)
|
|
|
|
# A complex auto routine that drives forward, drops a hatch, and then drives backward.
|
|
self.complexAuto = commands.autos.Autos.complexAuto(
|
|
self.driveSubsystem, self.hatchSubsystem
|
|
)
|
|
|
|
# The driver's controller
|
|
self.driverController = commands2.button.CommandNiDsPS4Controller(
|
|
constants.kDriverControllerPort
|
|
)
|
|
|
|
# Configure the button bindings
|
|
self.configureButtonBindings()
|
|
|
|
# Configure default commands
|
|
# Set the default drive command to split-stick arcade drive
|
|
self.driveSubsystem.setDefaultCommand(
|
|
# A split-stick arcade command, with forward/backward controlled by the left
|
|
# hand, and turning controlled by the right.
|
|
commands2.cmd.run(
|
|
lambda: self.driveSubsystem.arcadeDrive(
|
|
-self.driverController.getLeftY(),
|
|
-self.driverController.getRightX(),
|
|
),
|
|
self.driveSubsystem,
|
|
)
|
|
)
|
|
|
|
# Chooser
|
|
self.chooser = wpilib.SendableChooser()
|
|
|
|
# Add commands to the autonomous command chooser
|
|
self.chooser.setDefaultOption("Simple Auto", self.simpleAuto)
|
|
self.chooser.addOption("Complex Auto", self.complexAuto)
|
|
|
|
# Put the chooser on the dashboard
|
|
wpilib.SmartDashboard.putData("Autonomous", self.chooser)
|
|
|
|
def configureButtonBindings(self):
|
|
"""
|
|
Use this method to define your button->command mappings. Buttons can be created by
|
|
instantiating a wpilib.GenericHID or one of its subclasses (Joystick or XboxController),
|
|
and then passing it to a JoystickButton.
|
|
"""
|
|
|
|
# Grab the hatch when the Circle button is pressed.
|
|
self.driverController.circle().onTrue(self.hatchSubsystem.grabHatch())
|
|
|
|
# Release the hatch when the Square button is pressed.
|
|
self.driverController.square().onTrue(self.hatchSubsystem.releaseHatch())
|
|
|
|
# While holding R1, drive at half speed
|
|
self.driverController.R1().onTrue(
|
|
commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(0.5))
|
|
).onFalse(commands2.cmd.runOnce(lambda: self.driveSubsystem.setMaxOutput(1)))
|
|
|
|
def getAutonomousCommand(self) -> commands2.Command:
|
|
return self.chooser.getSelected()
|