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:
PJ Reiniger
40
robotpyExamples/examples/DriveDistanceOffboard/constants.py
Normal file
40
robotpyExamples/examples/DriveDistanceOffboard/constants.py
Normal file
@@ -0,0 +1,40 @@
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
|
||||
"""
|
||||
A place for the constant values in the code that may be used in more than one place.
|
||||
This offers a convenient resources to teams who need to make both quick and universal
|
||||
changes.
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
|
||||
class DriveConstants:
|
||||
kDt = 0.02
|
||||
kLeftMotor1Port = 0
|
||||
kLeftMotor2Port = 1
|
||||
kRightMotor1Port = 2
|
||||
kRightMotor2Port = 3
|
||||
|
||||
"""
|
||||
These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
These characterization values MUST be determined either experimentally or theoretically
|
||||
for *your* robot's drive.
|
||||
The SysId tool provides a convenient method for obtaining these values for your robot.
|
||||
"""
|
||||
ks = 1.0 # V
|
||||
kv = 0.8 # V/(m/s)
|
||||
ka = 0.15 # V/(m/s²)
|
||||
|
||||
kp = 1.0
|
||||
|
||||
kMaxVelocity = 3.0 # m/s
|
||||
kMaxAcceleration = 3.0 # m/s²
|
||||
|
||||
|
||||
class OIConstants:
|
||||
kDriverControllerPort = 0
|
||||
@@ -0,0 +1,98 @@
|
||||
#
|
||||
# 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 enum
|
||||
|
||||
|
||||
class ExampleSmartMotorController:
|
||||
"""A simplified stub class that simulates the API of a common "smart" motor controller.
|
||||
Has no actual functionality.
|
||||
"""
|
||||
|
||||
class PIDMode(enum.Enum):
|
||||
kPosition = enum.auto()
|
||||
kVelocity = enum.auto()
|
||||
kMovementWitchcraft = enum.auto()
|
||||
|
||||
def __init__(self, port: int) -> None:
|
||||
"""Creates a new ExampleSmartMotorController.
|
||||
|
||||
Args:
|
||||
port: The port for the controller.
|
||||
"""
|
||||
super().__init__()
|
||||
self._velocity = 0.0
|
||||
self._inverted = False
|
||||
self._leader = None
|
||||
|
||||
def setPID(self, kp: float, ki: float, kd: float) -> None:
|
||||
"""Example method for setting the PID gains of the smart controller.
|
||||
|
||||
Args:
|
||||
kp: The proportional gain.
|
||||
ki: The integral gain.
|
||||
kd: The derivative gain.
|
||||
"""
|
||||
pass
|
||||
|
||||
def setSetPoint(
|
||||
self, mode: PIDMode, setpoint: float, arbfeedforward: float
|
||||
) -> None:
|
||||
"""Example method for setting the setpoint of the smart controller in PID mode.
|
||||
|
||||
Args:
|
||||
mode: The mode of the PID controller.
|
||||
setpoint: The controller setpoint.
|
||||
arbfeedforward: An arbitrary feedforward output (from -1 to 1).
|
||||
"""
|
||||
pass
|
||||
|
||||
def follow(self, leader: "ExampleSmartMotorController") -> None:
|
||||
"""Places this motor controller in follower mode.
|
||||
|
||||
Args:
|
||||
leader: The leader to follow.
|
||||
"""
|
||||
self._leader = leader
|
||||
|
||||
def getEncoderDistance(self) -> float:
|
||||
"""Returns the encoder distance.
|
||||
|
||||
Returns:
|
||||
The current encoder distance.
|
||||
"""
|
||||
return 0
|
||||
|
||||
def getEncoderRate(self) -> float:
|
||||
"""Returns the encoder rate.
|
||||
|
||||
Returns:
|
||||
The current encoder rate.
|
||||
"""
|
||||
return 0
|
||||
|
||||
def resetEncoder(self) -> None:
|
||||
"""Resets the encoder to zero distance."""
|
||||
pass
|
||||
|
||||
def set(self, velocity: float) -> None:
|
||||
self._velocity = -velocity if self._inverted else velocity
|
||||
|
||||
def get(self) -> float:
|
||||
return self._velocity
|
||||
|
||||
def setInverted(self, isInverted: bool) -> None:
|
||||
self._inverted = isInverted
|
||||
|
||||
def getInverted(self) -> bool:
|
||||
return self._inverted
|
||||
|
||||
def disable(self) -> None:
|
||||
self._velocity = 0.0
|
||||
|
||||
def stopMotor(self) -> None:
|
||||
self._velocity = 0.0
|
||||
74
robotpyExamples/examples/DriveDistanceOffboard/robot.py
Normal file
74
robotpyExamples/examples/DriveDistanceOffboard/robot.py
Normal file
@@ -0,0 +1,74 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# 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 typing
|
||||
|
||||
import wpilib
|
||||
import commands2
|
||||
import commands2.cmd
|
||||
|
||||
import robotcontainer
|
||||
|
||||
"""
|
||||
The VM is configured to automatically run this class, and to call the functions corresponding to
|
||||
each mode, as described in the TimedRobot documentation. If you change the name of this class or
|
||||
the package after creating this project, you must also update the build.gradle file in the
|
||||
project.
|
||||
"""
|
||||
|
||||
|
||||
class MyRobot(commands2.TimedCommandRobot):
|
||||
"""
|
||||
Command v2 robots are encouraged to inherit from TimedCommandRobot, which
|
||||
has an implementation of robotPeriodic which runs the scheduler for you
|
||||
"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
"""
|
||||
This function is run when the robot is first started up and should be used for any
|
||||
initialization code.
|
||||
"""
|
||||
super().__init__()
|
||||
self.autonomousCommand: typing.Optional[commands2.Command] = None
|
||||
|
||||
# Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
# autonomous chooser on the dashboard.
|
||||
self.container = robotcontainer.RobotContainer()
|
||||
|
||||
def disabledInit(self) -> None:
|
||||
"""This function is called once each time the robot enters Disabled mode."""
|
||||
|
||||
def disabledPeriodic(self) -> None:
|
||||
"""This function is called periodically when disabled"""
|
||||
|
||||
def autonomousInit(self) -> None:
|
||||
"""This autonomous runs the autonomous command selected by your RobotContainer class."""
|
||||
self.autonomousCommand = self.container.getAutonomousCommand()
|
||||
|
||||
# schedule the autonomous command (example)
|
||||
if self.autonomousCommand is not None:
|
||||
self.autonomousCommand.schedule()
|
||||
else:
|
||||
print("no auto command?")
|
||||
|
||||
def autonomousPeriodic(self) -> None:
|
||||
"""This function is called periodically during autonomous"""
|
||||
|
||||
def teleopInit(self) -> None:
|
||||
# This makes sure that the autonomous stops running when
|
||||
# teleop starts running. If you want the autonomous to
|
||||
# continue until interrupted by another command, remove
|
||||
# this line or comment it out.
|
||||
if self.autonomousCommand is not None:
|
||||
self.autonomousCommand.cancel()
|
||||
|
||||
def teleopPeriodic(self) -> None:
|
||||
"""This function is called periodically during operator control"""
|
||||
|
||||
def utilityInit(self) -> None:
|
||||
# Cancels all running commands at the start of utility mode
|
||||
commands2.CommandScheduler.getInstance().cancelAll()
|
||||
@@ -0,0 +1,89 @@
|
||||
#
|
||||
# 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 commands2
|
||||
import commands2.cmd
|
||||
import commands2.button
|
||||
import constants
|
||||
import subsystems.drivesubsystem
|
||||
|
||||
|
||||
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):
|
||||
# The robot's subsystems
|
||||
self.robotDrive = subsystems.drivesubsystem.DriveSubsystem()
|
||||
|
||||
# Retained command references
|
||||
self.driveFullVelocity = commands2.cmd.runOnce(
|
||||
lambda: self.robotDrive.setMaxOutput(1), self.robotDrive
|
||||
)
|
||||
self.driveHalfVelocity = commands2.cmd.runOnce(
|
||||
lambda: self.robotDrive.setMaxOutput(0.5), self.robotDrive
|
||||
)
|
||||
|
||||
# The driver's controller
|
||||
self.driverController = commands2.button.CommandNiDsXboxController(
|
||||
constants.OIConstants.kDriverControllerPort
|
||||
)
|
||||
|
||||
# Configure the button bindings
|
||||
self.configureButtonBindings()
|
||||
|
||||
# Configure default commands
|
||||
# Set the default drive command to split-stick arcade drive
|
||||
self.robotDrive.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.robotDrive.arcadeDrive(
|
||||
-self.driverController.getLeftY(),
|
||||
-self.driverController.getRightX(),
|
||||
),
|
||||
self.robotDrive,
|
||||
)
|
||||
)
|
||||
|
||||
def configureButtonBindings(self):
|
||||
"""
|
||||
Use this method to define your button->command mappings. Buttons can be created via the button
|
||||
factories on commands2.button.CommandGenericHID or one of its
|
||||
subclasses (commands2.button.CommandJoystick or command2.button.CommandNiDsXboxController).
|
||||
"""
|
||||
|
||||
# Configure your button bindings here
|
||||
|
||||
# We can bind commands while retaining references to them in RobotContainer
|
||||
|
||||
# Drive at half velocity when the bumper is held
|
||||
self.driverController.rightBumper().onTrue(self.driveHalfVelocity).onFalse(
|
||||
self.driveFullVelocity
|
||||
)
|
||||
|
||||
# Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
|
||||
self.driverController.a().onTrue(
|
||||
self.robotDrive.profiledDriveDistance(3).withTimeout(10)
|
||||
)
|
||||
|
||||
# Do the same thing as above when the 'B' button is pressed, but defined inline
|
||||
self.driverController.b().onTrue(
|
||||
self.robotDrive.dynamicProfiledDriveDistance(3).withTimeout(10)
|
||||
)
|
||||
|
||||
def getAutonomousCommand(self) -> commands2.Command:
|
||||
"""
|
||||
Use this to pass the autonomous command to the main :class:`.Robot` class.
|
||||
|
||||
:returns: the command to run in autonomous
|
||||
"""
|
||||
return commands2.cmd.none()
|
||||
@@ -0,0 +1,223 @@
|
||||
#
|
||||
# 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 constants
|
||||
import examplesmartmotorcontroller
|
||||
import wpimath
|
||||
|
||||
|
||||
class DriveSubsystem(commands2.Subsystem):
|
||||
def __init__(self) -> None:
|
||||
"""Creates a new DriveSubsystem"""
|
||||
super().__init__()
|
||||
|
||||
# The motors on the left side of the drive.
|
||||
self.leftLeader = examplesmartmotorcontroller.ExampleSmartMotorController(
|
||||
constants.DriveConstants.kLeftMotor1Port
|
||||
)
|
||||
|
||||
self.leftFollower = examplesmartmotorcontroller.ExampleSmartMotorController(
|
||||
constants.DriveConstants.kLeftMotor2Port
|
||||
)
|
||||
|
||||
# The motors on the right side of the drive.
|
||||
self.rightLeader = examplesmartmotorcontroller.ExampleSmartMotorController(
|
||||
constants.DriveConstants.kRightMotor1Port
|
||||
)
|
||||
|
||||
self.rightFollower = examplesmartmotorcontroller.ExampleSmartMotorController(
|
||||
constants.DriveConstants.kRightMotor1Port
|
||||
)
|
||||
|
||||
# We need to invert one side of the drivetrain so that positive voltages
|
||||
# result in both sides moving forward. Depending on how your robot's
|
||||
# gearbox is constructed, you might have to invert the left side instead.
|
||||
self.rightLeader.setInverted(True)
|
||||
|
||||
# You might need to not do this depending on the specific motor controller
|
||||
# that you are using -- contact the respective vendor's documentation for
|
||||
# more details.
|
||||
self.rightFollower.setInverted(True)
|
||||
|
||||
self.leftFollower.follow(self.leftLeader)
|
||||
self.rightFollower.follow(self.rightLeader)
|
||||
|
||||
self.leftLeader.setPID(constants.DriveConstants.kp, 0, 0)
|
||||
self.rightLeader.setPID(constants.DriveConstants.kp, 0, 0)
|
||||
|
||||
# The feedforward controller (note that these are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!)
|
||||
# check DriveConstants for more information.
|
||||
self.feedforward = wpimath.SimpleMotorFeedforwardMeters(
|
||||
constants.DriveConstants.ks,
|
||||
constants.DriveConstants.kv,
|
||||
constants.DriveConstants.ka,
|
||||
)
|
||||
|
||||
# The robot's drive
|
||||
self.drive = wpilib.DifferentialDrive(self.leftLeader.set, self.rightLeader.set)
|
||||
|
||||
self.profile = wpimath.TrapezoidProfile(
|
||||
wpimath.TrapezoidProfile.Constraints(
|
||||
constants.DriveConstants.kMaxVelocity,
|
||||
constants.DriveConstants.kMaxAcceleration,
|
||||
)
|
||||
)
|
||||
self.timer = wpilib.Timer()
|
||||
self._initialLeftDistance = 0.0
|
||||
self._initialRightDistance = 0.0
|
||||
|
||||
def arcadeDrive(self, fwd: float, rot: float):
|
||||
"""
|
||||
Drives the robot using arcade controls.
|
||||
|
||||
:param fwd: the commanded forward movement
|
||||
:param rot: the commanded rotation
|
||||
"""
|
||||
self.drive.arcadeDrive(fwd, rot)
|
||||
|
||||
def setDriveStates(
|
||||
self,
|
||||
currentLeft: wpimath.TrapezoidProfile.State,
|
||||
currentRight: wpimath.TrapezoidProfile.State,
|
||||
nextLeft: wpimath.TrapezoidProfile.State,
|
||||
nextRight: wpimath.TrapezoidProfile.State,
|
||||
):
|
||||
"""
|
||||
Attempts to follow the given drive states using offboard PID.
|
||||
|
||||
:param currentLeft: The current left wheel state.
|
||||
:param currentRight: The current right wheel state.
|
||||
:param nextLeft: The next left wheel state.
|
||||
:param nextRight: The next right wheel state.
|
||||
"""
|
||||
battery_voltage = wpilib.RobotController.getBatteryVoltage()
|
||||
left_feedforward = self.feedforward.calculate(
|
||||
currentLeft.velocity,
|
||||
(nextLeft.velocity - currentLeft.velocity) / constants.DriveConstants.kDt,
|
||||
)
|
||||
right_feedforward = self.feedforward.calculate(
|
||||
currentRight.velocity,
|
||||
(nextRight.velocity - currentRight.velocity) / constants.DriveConstants.kDt,
|
||||
)
|
||||
self.leftLeader.setSetPoint(
|
||||
examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
|
||||
currentLeft.position,
|
||||
left_feedforward / battery_voltage,
|
||||
)
|
||||
|
||||
self.rightLeader.setSetPoint(
|
||||
examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
|
||||
currentRight.position,
|
||||
right_feedforward / battery_voltage,
|
||||
)
|
||||
|
||||
def getLeftEncoderDistance(self) -> float:
|
||||
"""
|
||||
Returns the left drive encoder distance.
|
||||
|
||||
:returns: the left drive encoder distance
|
||||
"""
|
||||
return self.leftLeader.getEncoderDistance()
|
||||
|
||||
def getRightEncoderDistance(self) -> float:
|
||||
"""
|
||||
Returns the right drive encoder distance.
|
||||
|
||||
:returns: the right drive encoder distance
|
||||
"""
|
||||
return self.rightLeader.getEncoderDistance()
|
||||
|
||||
def resetEncoders(self):
|
||||
"""Resets the drive encoders"""
|
||||
self.leftLeader.resetEncoder()
|
||||
self.rightLeader.resetEncoder()
|
||||
|
||||
def setMaxOutput(self, maxOutput: float):
|
||||
"""
|
||||
Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
|
||||
|
||||
:param maxOutput: the maximum output to which the drive will be constrained
|
||||
"""
|
||||
self.drive.setMaxOutput(maxOutput)
|
||||
|
||||
def profiledDriveDistance(self, distance: float) -> commands2.Command:
|
||||
def on_init():
|
||||
self.timer.restart()
|
||||
self.resetEncoders()
|
||||
|
||||
def on_execute():
|
||||
current_time = self.timer.get()
|
||||
current_setpoint = self.profile.calculate(
|
||||
current_time,
|
||||
wpimath.TrapezoidProfile.State(),
|
||||
wpimath.TrapezoidProfile.State(distance, 0),
|
||||
)
|
||||
next_setpoint = self.profile.calculate(
|
||||
current_time + constants.DriveConstants.kDt,
|
||||
wpimath.TrapezoidProfile.State(),
|
||||
wpimath.TrapezoidProfile.State(distance, 0),
|
||||
)
|
||||
self.setDriveStates(
|
||||
current_setpoint, current_setpoint, next_setpoint, next_setpoint
|
||||
)
|
||||
|
||||
def on_end(interrupted: bool):
|
||||
self.leftLeader.set(0)
|
||||
self.rightLeader.set(0)
|
||||
|
||||
def is_finished() -> bool:
|
||||
return self.profile.isFinished(0)
|
||||
|
||||
return commands2.FunctionalCommand(
|
||||
on_init, on_execute, on_end, is_finished, self
|
||||
)
|
||||
|
||||
def dynamicProfiledDriveDistance(self, distance: float) -> commands2.Command:
|
||||
def on_init():
|
||||
self.timer.restart()
|
||||
self._initialLeftDistance = self.getLeftEncoderDistance()
|
||||
self._initialRightDistance = self.getRightEncoderDistance()
|
||||
|
||||
def on_execute():
|
||||
current_time = self.timer.get()
|
||||
current_left = self.profile.calculate(
|
||||
current_time,
|
||||
wpimath.TrapezoidProfile.State(self._initialLeftDistance, 0),
|
||||
wpimath.TrapezoidProfile.State(self._initialLeftDistance + distance, 0),
|
||||
)
|
||||
current_right = self.profile.calculate(
|
||||
current_time,
|
||||
wpimath.TrapezoidProfile.State(self._initialRightDistance, 0),
|
||||
wpimath.TrapezoidProfile.State(
|
||||
self._initialRightDistance + distance, 0
|
||||
),
|
||||
)
|
||||
next_left = self.profile.calculate(
|
||||
current_time + constants.DriveConstants.kDt,
|
||||
wpimath.TrapezoidProfile.State(self._initialLeftDistance, 0),
|
||||
wpimath.TrapezoidProfile.State(self._initialLeftDistance + distance, 0),
|
||||
)
|
||||
next_right = self.profile.calculate(
|
||||
current_time + constants.DriveConstants.kDt,
|
||||
wpimath.TrapezoidProfile.State(self._initialRightDistance, 0),
|
||||
wpimath.TrapezoidProfile.State(
|
||||
self._initialRightDistance + distance, 0
|
||||
),
|
||||
)
|
||||
self.setDriveStates(current_left, current_right, next_left, next_right)
|
||||
|
||||
def on_end(interrupted: bool):
|
||||
self.leftLeader.set(0)
|
||||
self.rightLeader.set(0)
|
||||
|
||||
def is_finished() -> bool:
|
||||
return self.profile.isFinished(0)
|
||||
|
||||
return commands2.FunctionalCommand(
|
||||
on_init, on_execute, on_end, is_finished, self
|
||||
)
|
||||
Reference in New Issue
Block a user