mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[robotpy][examples] Split examples and snippets (#8944)
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`
This commit is contained in:
@@ -0,0 +1,93 @@
|
||||
#
|
||||
# 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 constants
|
||||
|
||||
from commands.complexauto import ComplexAuto
|
||||
from commands.drivedistance import DriveDistance
|
||||
from commands.defaultdrive import DefaultDrive
|
||||
from commands.grabhatch import GrabHatch
|
||||
from commands.halvedrivevelocity import HalveDriveVelocity
|
||||
from commands.releasehatch import ReleaseHatch
|
||||
|
||||
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 driver's controller
|
||||
# self.driverController = wpilib.NiDsXboxController(constants.kDriverControllerPort)
|
||||
self.driverController = wpilib.Joystick(constants.kDriverControllerPort)
|
||||
|
||||
# The robot's subsystems
|
||||
self.drive = DriveSubsystem()
|
||||
self.hatch = HatchSubsystem()
|
||||
|
||||
# Autonomous routines
|
||||
|
||||
# A simple auto routine that drives forward a specified distance, and then stops.
|
||||
self.simpleAuto = DriveDistance(
|
||||
constants.kAutoDriveDistanceInches, constants.kAutoDriveVelocity, self.drive
|
||||
)
|
||||
|
||||
# A complex auto routine that drives forward, drops a hatch, and then drives backward.
|
||||
self.complexAuto = ComplexAuto(self.drive, self.hatch)
|
||||
|
||||
# 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)
|
||||
|
||||
self.configureButtonBindings()
|
||||
|
||||
# set up default drive command
|
||||
self.drive.setDefaultCommand(
|
||||
DefaultDrive(
|
||||
self.drive,
|
||||
lambda: -self.driverController.getY(),
|
||||
lambda: self.driverController.getX(),
|
||||
)
|
||||
)
|
||||
|
||||
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.
|
||||
"""
|
||||
|
||||
commands2.button.JoystickButton(self.driverController, 1).onTrue(
|
||||
GrabHatch(self.hatch)
|
||||
)
|
||||
|
||||
commands2.button.JoystickButton(self.driverController, 2).onTrue(
|
||||
ReleaseHatch(self.hatch)
|
||||
)
|
||||
|
||||
commands2.button.JoystickButton(self.driverController, 3).whileTrue(
|
||||
HalveDriveVelocity(self.drive)
|
||||
)
|
||||
|
||||
def getAutonomousCommand(self) -> commands2.Command:
|
||||
return self.chooser.getSelected()
|
||||
Reference in New Issue
Block a user