mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
94 lines
3.2 KiB
Python
94 lines
3.2 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 constants
|
||
|
|
|
||
|
|
from commands.complexauto import ComplexAuto
|
||
|
|
from commands.drivedistance import DriveDistance
|
||
|
|
from commands.defaultdrive import DefaultDrive
|
||
|
|
from commands.grabhatch import GrabHatch
|
||
|
|
from commands.halvedrivespeed import HalveDriveSpeed
|
||
|
|
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.kAutoDriveSpeed, 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(
|
||
|
|
HalveDriveSpeed(self.drive)
|
||
|
|
)
|
||
|
|
|
||
|
|
def getAutonomousCommand(self) -> commands2.Command:
|
||
|
|
return self.chooser.getSelected()
|