diff --git a/.github/workflows/dist.yml b/.github/workflows/dist.yml index 7c88b48af..74deeb99e 100644 --- a/.github/workflows/dist.yml +++ b/.github/workflows/dist.yml @@ -261,6 +261,10 @@ jobs: run: | python -m devtools ci install-test-pure-wheels + - name: Test examples + run: | + python -m devtools test-examples + - name: Ensure all headers are accounted for run: | python -m devtools ci scan-headers diff --git a/devtools/__main__.py b/devtools/__main__.py index 58aa50492..89af9212a 100644 --- a/devtools/__main__.py +++ b/devtools/__main__.py @@ -6,6 +6,7 @@ from .ctx import Context from . import ci +from . import examples from . import update_pyproject @@ -34,6 +35,7 @@ def main(ctx: click.Context, verbose: bool): main.add_command(ci.ci) +main.add_command(examples.test_examples) main.add_command(update_pyproject.update_pyproject) diff --git a/devtools/examples.py b/devtools/examples.py new file mode 100644 index 000000000..85ef46614 --- /dev/null +++ b/devtools/examples.py @@ -0,0 +1,105 @@ +import dataclasses +import os +import pathlib +import subprocess +import sys +import typing as T + +import click +import tomlkit + +from .util import parse_input, run_cmd + + +def _validate_example_list(root: pathlib.Path, expected_dirs: T.Sequence[str]) -> None: + expected = sorted(f"{name}/robot.py" for name in expected_dirs) + actual = sorted(p.relative_to(root).as_posix() for p in root.rglob("robot.py")) + + if expected == actual: + return + + missing = sorted(set(expected) - set(actual)) + extra = sorted(set(actual) - set(expected)) + for path in missing: + print(f"Missing: {path}") + for path in extra: + print(f"Extra: {path}") + + if not os.environ.get("FORCE_ANYWAYS"): + print("ERROR: Not every robot.py file is in the list of tests!") + sys.exit(1) + + +@dataclasses.dataclass +class ExamplesTests: + base: T.List[str] + ignored: T.List[str] + + +@dataclasses.dataclass +class ExamplesConfig: + tests: ExamplesTests + + +def _load_tests_config(config_path: pathlib.Path) -> ExamplesConfig: + try: + data = tomlkit.parse(config_path.read_text(encoding="utf-8")) + except FileNotFoundError: + raise click.ClickException(f"Missing tests config: {config_path}") + except Exception as exc: + raise click.ClickException(f"Invalid tests config: {config_path}: {exc}") + + try: + return parse_input(data, ExamplesConfig, config_path) + except Exception as exc: + raise click.ClickException(str(exc)) + + +@click.command(name="test-examples") +@click.argument("test_name", required=False) +@click.option("-x", "--exitfirst", is_flag=True, help="Exit on first failed test.") +def test_examples(test_name: str | None, exitfirst: bool) -> None: + """Run tests on robot examples.""" + root = pathlib.Path(__file__).parent.parent / "examples" / "robot" + config_path = root / "examples.toml" + + cfg = _load_tests_config(config_path) + base_tests = cfg.tests.base + ignored_tests = cfg.tests.ignored + + every_tests = [*base_tests, *ignored_tests] + _validate_example_list(root, every_tests) + + tests_to_run = base_tests + if test_name: + if test_name not in every_tests: + raise click.BadParameter(f"unknown example {test_name}") + tests_to_run = [test_name] + + failed_tests = [] + + for example_name in tests_to_run: + test_dir = root / example_name + print(test_dir.resolve()) + try: + run_cmd( + sys.executable, + "-m", + "robotpy", + "test", + "--builtin", + cwd=test_dir, + ) + except subprocess.CalledProcessError: + print(f"Test in {test_dir.resolve()} failed") + failed_tests.append(example_name) + if exitfirst: + break + + if failed_tests: + print("Failed tests:") + for name in failed_tests: + print(f"- {name}") + sys.exit(1) + + print("All tests successful!") diff --git a/examples/robot/.gitignore b/examples/robot/.gitignore new file mode 100644 index 000000000..cb80cf5f4 --- /dev/null +++ b/examples/robot/.gitignore @@ -0,0 +1,22 @@ +*.py[ocd] +__pycache__ +.coverage +.cache + +.vscode/ +.deploy_cfg +deploy.json + +.project +.pydevproject + +pyproject.toml +wpilib_preferences.json + +imgui.ini +simgui*.json + +opkg_cache +pip_cache + +networktables.* \ No newline at end of file diff --git a/examples/robot/AddressableLED/robot.py b/examples/robot/AddressableLED/robot.py new file mode 100644 index 000000000..1bb7a18ac --- /dev/null +++ b/examples/robot/AddressableLED/robot.py @@ -0,0 +1,45 @@ +#!/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 wpilib +import wpimath.units + + +class MyRobot(wpilib.TimedRobot): + def __init__(self) -> None: + super().__init__() + + # SmartIO port 1 + self.led = wpilib.AddressableLED(1) + + # Reuse buffer + # Default to a length of 60 + self.ledData = [wpilib.AddressableLED.LEDData() for _ in range(60)] + self.led.setLength(len(self.ledData)) + + # Set the data + self.led.setData(self.ledData) + + # Create an LED pattern that will display a rainbow across + # all hues at maximum saturation and half brightness + self.rainbow = wpilib.LEDPattern.rainbow(255, 128) + + # Our LED strip has a density of 120 LEDs per meter + self.kLedSpacing = 1 / 120.0 + + # Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a + # speed of 1 meter per second. + self.scrollingRainbow = self.rainbow.scrollAtAbsoluteSpeed( + 1, + self.kLedSpacing, + ) + + def robotPeriodic(self) -> None: + # Update the buffer with the rainbow animation + self.scrollingRainbow.applyTo(self.ledData) + # Set the LEDs + self.led.setData(self.ledData) diff --git a/examples/robot/AprilTagsVision/robot.py b/examples/robot/AprilTagsVision/robot.py new file mode 100644 index 000000000..ac7f0c936 --- /dev/null +++ b/examples/robot/AprilTagsVision/robot.py @@ -0,0 +1,26 @@ +#!/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 wpilib +import wpilib.cameraserver + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing the detection of AprilTags. The image is acquired from the USB + camera, then any detected AprilTags are marked up on the image and sent to the dashboard. + Be aware that the performance on this is much worse than a coprocessor solution! + """ + + def __init__(self): + super().__init__() + # Your image processing code will be launched via a stub that will set up logging and initialize NetworkTables + # to talk to your robot code. + # https://robotpy.readthedocs.io/en/stable/vision/roborio.html#important-notes + + wpilib.CameraServer.launch("vision.py:main") diff --git a/examples/robot/AprilTagsVision/vision.py b/examples/robot/AprilTagsVision/vision.py new file mode 100644 index 000000000..5fd7d6de3 --- /dev/null +++ b/examples/robot/AprilTagsVision/vision.py @@ -0,0 +1,152 @@ +# +# 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 ntcore +import robotpy_apriltag +from cscore import CameraServer + +import cv2 +import numpy as np + + +# +# This code will work both on a RoboRIO and on other platforms. The exact mechanism +# to run it differs depending on whether you’re on a RoboRIO or a coprocessor +# +# https://robotpy.readthedocs.io/en/stable/vision/code.html + + +def main(): + detector = robotpy_apriltag.AprilTagDetector() + + # look for tag36h11, correct 1 error bit (hamming distance 1) + # hamming 1 allocates 781KB, 2 allocates 27.4 MB, 3 allocates 932 MB + # max of 1 recommended for RoboRIO 1, while hamming 2 is feasible on the RoboRIO 2 + detector.addFamily("tag36h11", 1) + + # Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000 + # (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21) + poseEstConfig = robotpy_apriltag.AprilTagPoseEstimator.Config( + 0.1651, + 699.3778103158814, + 677.7161226393544, + 345.6059345433618, + 207.12741326228522, + ) + estimator = robotpy_apriltag.AprilTagPoseEstimator(poseEstConfig) + + # Get the UsbCamera from CameraServer + camera = CameraServer.startAutomaticCapture() + + # Set the resolution + camera.setResolution(640, 480) + + # Get a CvSink. This will capture Mats from the camera + cvSink = CameraServer.getVideo() + + # Set up a CvSource. This will send images back to the Dashboard + outputStream = CameraServer.putVideo("Detected", 640, 480) + + # Mats are very memory expensive. Let's reuse these. + mat = np.zeros((480, 640, 3), dtype=np.uint8) + grayMat = np.zeros(shape=(480, 640), dtype=np.uint8) + + # Instantiate once + tags = [] + outlineColor = (0, 255, 0) + crossColor = (0, 0, 255) + + # Output the list to Network Tables + tagsTable = ntcore.NetworkTableInstance.getDefault().getTable("apriltags") + pubTags = tagsTable.getIntegerArrayTopic("tags").publish() + + try: + while True: + # Tell the CvSink to grab a frame from the camera and put it + # in the source mat. If there is an error notify the output. + if cvSink.grabFrame(mat) == 0: + # Send the output frame the error + outputStream.notifyError(cvSink.getError()) + + # Skip the rest of the current iteration + continue + + cv2.cvtColor(mat, cv2.COLOR_RGB2GRAY, dst=grayMat) + + detections = detector.detect(grayMat) + + # have not seen any tags yet + tags.clear() + + for detection in detections: + # remember we saw this tag + tags.append(detection.getId()) + + # draw lines around the tag + for i in range(4): + j = (i + 1) % 4 + point1 = ( + int(detection.getCorner(i).x), + int(detection.getCorner(i).y), + ) + point2 = ( + int(detection.getCorner(j).x), + int(detection.getCorner(j).y), + ) + mat = cv2.line(mat, point1, point2, outlineColor, 2) + + # mark the center of the tag + cx = int(detection.getCenter().x) + cy = int(detection.getCenter().y) + ll = 10 + mat = cv2.line( + mat, + (cx - ll, cy), + (cx + ll, cy), + crossColor, + 2, + ) + mat = cv2.line( + mat, + (cx, cy - ll), + (cx, cy + ll), + crossColor, + 2, + ) + + # identify the tag + mat = cv2.putText( + mat, + str(detection.getId()), + (cx + ll, cy), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + crossColor, + 3, + ) + + # determine pose + pose = estimator.estimate(detection) + + # put pose into dashboard + rot = pose.rotation() + tagsTable.getEntry(f"pose_{detection.getId()}").setDoubleArray( + [pose.X(), pose.Y(), pose.Z(), rot.X(), rot.Y(), rot.Z()] + ) + + # put list of tags onto dashboard + pubTags.set(tags) + + # Give output stream a new image to display + outputStream.putFrame(mat) + finally: + pubTags.close() + detector.close() + + # The camera code will be killed when the robot.py program exits. If you wish to perform cleanup, + # you should register an atexit handler. The child process will NOT be launched when running the robot code in + # simulation or unit testing mode diff --git a/examples/robot/ArcadeDrive/robot.py b/examples/robot/ArcadeDrive/robot.py new file mode 100755 index 000000000..03ee5f028 --- /dev/null +++ b/examples/robot/ArcadeDrive/robot.py @@ -0,0 +1,35 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing the use of the DifferentialDrive class. + Runs the motors with arcade steering. + """ + + def __init__(self): + """Robot initialization function""" + super().__init__() + + leftMotor = wpilib.PWMSparkMax(0) + rightMotor = wpilib.PWMSparkMax(1) + self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor) + self.stick = wpilib.Joystick(0) + + # 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. + rightMotor.setInverted(True) + + def teleopPeriodic(self): + # Drive with arcade drive. + # That means that the Y axis drives forward + # and backward, and the X turns left and right. + self.robotDrive.arcadeDrive(-self.stick.getY(), -self.stick.getX()) diff --git a/examples/robot/ArcadeDriveXboxController/robot.py b/examples/robot/ArcadeDriveXboxController/robot.py new file mode 100755 index 000000000..e1b83ec63 --- /dev/null +++ b/examples/robot/ArcadeDriveXboxController/robot.py @@ -0,0 +1,37 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing the use of the DifferentialDrive class. + Runs the motors with split arcade steering and an Xbox controller. + """ + + def __init__(self): + """Robot initialization function""" + super().__init__() + + leftMotor = wpilib.PWMSparkMax(0) + rightMotor = wpilib.PWMSparkMax(1) + self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor) + self.driverController = wpilib.XboxController(0) + + # 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. + rightMotor.setInverted(True) + + def teleopPeriodic(self): + # Drive with split arcade style + # That means that the Y axis of the left stick moves forward + # and backward, and the X of the right stick turns left and right. + self.robotDrive.arcadeDrive( + -self.driverController.getLeftY(), -self.driverController.getRightX() + ) diff --git a/examples/robot/ArmSimulation/constants.py b/examples/robot/ArmSimulation/constants.py new file mode 100644 index 000000000..025dcc391 --- /dev/null +++ b/examples/robot/ArmSimulation/constants.py @@ -0,0 +1,33 @@ +# +# 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 math + +from wpimath import units + + +class Constants: + kMotorPort = 0 + kEncoderAChannel = 0 + kEncoderBChannel = 1 + kJoystickPort = 0 + + kArmPositionKey = "ArmPosition" + kArmPKey = "ArmP" + + # The P gain for the PID controller that drives this arm. + kDefaultArmKp = 50.0 + kDefaultArmSetpointDegrees = 75.0 + + # distance per pulse = (angle per revolution) / (pulses per revolution) + # = (2 * PI rads) / (4096 pulses) + kArmEncoderDistPerPulse = 2.0 * math.pi / 4096 + + kArmReduction = 200 + kArmMass = 8.0 # Kilograms + kArmLength = units.inchesToMeters(30) + kMinAngleRads = units.degreesToRadians(-75) + kMaxAngleRads = units.degreesToRadians(255) diff --git a/examples/robot/ArmSimulation/robot.py b/examples/robot/ArmSimulation/robot.py new file mode 100755 index 000000000..0997db7e0 --- /dev/null +++ b/examples/robot/ArmSimulation/robot.py @@ -0,0 +1,38 @@ +#!/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 wpilib + +from constants import Constants +from subsystems.arm import Arm + + +class MyRobot(wpilib.TimedRobot): + """This is a sample program to demonstrate the use of arm simulation with existing code.""" + + def __init__(self) -> None: + super().__init__() + self.arm = Arm() + self.joystick = wpilib.Joystick(Constants.kJoystickPort) + + def simulationPeriodic(self) -> None: + self.arm.simulationPeriodic() + + def teleopInit(self) -> None: + self.arm.loadPreferences() + + def teleopPeriodic(self) -> None: + if self.joystick.getTrigger(): + # Here, we run PID control like normal. + self.arm.reachSetpoint() + else: + # Otherwise, we disable the motor. + self.arm.stop() + + def disabledInit(self) -> None: + # This just makes sure that our simulation code knows that the motor's off. + self.arm.stop() diff --git a/examples/robot/ArmSimulation/subsystems/arm.py b/examples/robot/ArmSimulation/subsystems/arm.py new file mode 100644 index 000000000..c4c9cac66 --- /dev/null +++ b/examples/robot/ArmSimulation/subsystems/arm.py @@ -0,0 +1,114 @@ +# +# 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 wpilib.simulation +import wpimath +import wpimath.units +import wpiutil + +from constants import Constants + + +class Arm: + def __init__(self) -> None: + # The P gain for the PID controller that drives this arm. + self.armKp = Constants.kDefaultArmKp + self.armSetpointDegrees = Constants.kDefaultArmSetpointDegrees + + # The arm gearbox represents a gearbox containing two Vex 775pro motors. + self.armGearbox = wpimath.DCMotor.vex775Pro(2) + + # Standard classes for controlling our arm + self.controller = wpimath.PIDController(self.armKp, 0, 0) + self.encoder = wpilib.Encoder( + Constants.kEncoderAChannel, Constants.kEncoderBChannel + ) + self.motor = wpilib.PWMSparkMax(Constants.kMotorPort) + + # Simulation classes help us simulate what's going on, including gravity. + # This arm sim represents an arm that can travel from -75 degrees (rotated down front) + # to 255 degrees (rotated down in the back). + self.armSim = wpilib.simulation.SingleJointedArmSim( + self.armGearbox, + Constants.kArmReduction, + wpilib.simulation.SingleJointedArmSim.estimateMOI( + Constants.kArmLength, Constants.kArmMass + ), + Constants.kArmLength, + Constants.kMinAngleRads, + Constants.kMaxAngleRads, + True, + 0, + [Constants.kArmEncoderDistPerPulse, 0.0], + ) + self.encoderSim = wpilib.simulation.EncoderSim(self.encoder) + + # Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm. + self.mech2d = wpilib.Mechanism2d(60, 60) + self.armPivot = self.mech2d.getRoot("ArmPivot", 30, 30) + self.armTower = self.armPivot.appendLigament("ArmTower", 30, -90) + self.armLigament = self.armPivot.appendLigament( + "Arm", + 30, + wpimath.units.radiansToDegrees(self.armSim.getAngle()), + 6, + wpiutil.Color8Bit(wpiutil.Color.kYellow), + ) + + # Subsystem constructor. + self.encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse) + + # Put Mechanism 2d to SmartDashboard + wpilib.SmartDashboard.putData("Arm Sim", self.mech2d) + self.armTower.setColor(wpiutil.Color8Bit(wpiutil.Color.kBlue)) + + # Set the Arm position setpoint and P constant to Preferences if the keys don't already exist + wpilib.Preferences.initDouble( + Constants.kArmPositionKey, self.armSetpointDegrees + ) + wpilib.Preferences.initDouble(Constants.kArmPKey, self.armKp) + + def simulationPeriodic(self) -> None: + # In this method, we update our simulation of what our arm is doing + # First, we set our "inputs" (voltages) + self.armSim.setInput( + [self.motor.get() * wpilib.RobotController.getBatteryVoltage()] + ) + + # Next, we update it. The standard loop time is 20ms. + self.armSim.update(0.020) + + # Finally, we set our simulated encoder's readings and simulated battery voltage + self.encoderSim.setDistance(self.armSim.getAngle()) + # SimBattery estimates loaded battery voltages + wpilib.simulation.RoboRioSim.setVInVoltage( + wpilib.simulation.BatterySim.calculate([self.armSim.getCurrentDraw()]) + ) + + # Update the Mechanism Arm angle based on the simulated arm angle + self.armLigament.setAngle( + wpimath.units.radiansToDegrees(self.armSim.getAngle()) + ) + + def loadPreferences(self) -> None: + # Read Preferences for Arm setpoint and kP on entering Teleop + self.armSetpointDegrees = wpilib.Preferences.getDouble( + Constants.kArmPositionKey, self.armSetpointDegrees + ) + if self.armKp != wpilib.Preferences.getDouble(Constants.kArmPKey, self.armKp): + self.armKp = wpilib.Preferences.getDouble(Constants.kArmPKey, self.armKp) + self.controller.setP(self.armKp) + + def reachSetpoint(self) -> None: + pidOutput = self.controller.calculate( + self.encoder.getDistance(), + wpimath.units.degreesToRadians(self.armSetpointDegrees), + ) + self.motor.setVoltage(pidOutput) + + def stop(self) -> None: + self.motor.set(0.0) diff --git a/examples/robot/CANPDP/robot.py b/examples/robot/CANPDP/robot.py new file mode 100755 index 000000000..c99334494 --- /dev/null +++ b/examples/robot/CANPDP/robot.py @@ -0,0 +1,56 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program showing how to retrieve information from the Power + Distribution Panel via CAN. The information will be displayed under variables + through the SmartDashboard. + """ + + def __init__(self): + """Robot initialization function""" + super().__init__() + + # Object for dealing with the Power Distribution Panel (PDP). + self.pdp = wpilib.PowerDistribution(0) + + # Put the PDP itself to the dashboard + wpilib.SmartDashboard.putData("PDP", self.pdp) + + def robotPeriodic(self): + # Get the current going through channel 7, in Amperes. + # The PDP returns the current in increments of 0.125A. + # At low currents the current readings tend to be less accurate. + current7 = self.pdp.getCurrent(7) + wpilib.SmartDashboard.putNumber("Current Channel 7", current7) + + # Get the voltage going into the PDP, in Volts. + # The PDP returns the voltage in increments of 0.05 Volts. + voltage = self.pdp.getVoltage() + wpilib.SmartDashboard.putNumber("Voltage", voltage) + + # Retrieves the temperature of the PDP, in degrees Celsius. + temperatureCelsius = self.pdp.getTemperature() + wpilib.SmartDashboard.putNumber("Temperature", temperatureCelsius) + + # Get the total current of all channels. + totalCurrent = self.pdp.getTotalCurrent() + wpilib.SmartDashboard.putNumber("Total Current", totalCurrent) + + # Get the total power of all channels. + # Power is the bus voltage multiplied by the current with the units Watts. + totalPower = self.pdp.getTotalPower() + wpilib.SmartDashboard.putNumber("Total Power", totalPower) + + # Get the total energy of all channels. + # Energy is the power summed over time with units Joules. + totalEnergy = self.pdp.getTotalEnergy() + wpilib.SmartDashboard.putNumber("Total Energy", totalEnergy) diff --git a/examples/robot/CONTRIBUTING.md b/examples/robot/CONTRIBUTING.md new file mode 100644 index 000000000..a79b7dbbe --- /dev/null +++ b/examples/robot/CONTRIBUTING.md @@ -0,0 +1,174 @@ +Guidelines for porting WPILib examples to Python +================================================ + +To ensure that our examples are helpful and accurate for those learning how to +use RobotPy, we have a set of guidelines for adding new examples to the project. +These guidelines are not strictly enforced, but we do ask that you follow them +when submitting pull requests with new examples. This will make the review +process easier for everyone involved. + +In general, our examples are based on the Java examples from allwpilib, as Java +is often easier to translate to Python. However, not all of our existing +examples adhere to all of these guidelines. If you see an opportunity to improve +an existing example, feel free to make the necessary changes. + +Shorter thoughts +---------------- + +Testing: + +* New examples must run! You *must* test your code on either a robot or in + simulation. If there's something broken in RobotPy, file an issue to get it + fixed + * You can find instructions on how to test a vision file [here](https://robotpy.readthedocs.io/en/stable/vision/other.html#vision-other-runcustom)! +* Format your code with black + +General: + +* We always try to stay as close to the original examples as possible +* `Main.java` is never needed +* Don't ever check in files for your IDE (.vscode, .idea, etc) +* Copy over the copyright statement from the original file + +Naming: + +* Filenames should always be all lowercase +* Function names are camelCase +* Class names start with a capital letter +* Class method names are camelCase +* Class member variables such as `m_name` should be `self.name` in Python +* Protected/private methods/members can optionally be prefixed with `_` + +Misc conversion thoughts + +* Comparisons to null such as `foo == null` become `foo is None` +* Single-line lambdas can be converted to python lambda statements. Anything + longer needs to be a separate function somewhere +* Never modify `sys.path` directly! + +Longer thoughts +--------------- + +Never initialize anything other than constants at class/global scope. Here are +a few examples: + +```python + +# OK: just a constant +MY_CONSTANT = 42 + +# BAD: at global scope +motor = wpilib.Talon(1) + +class MyRobot: + # BAD: at class scope + motor = wpilib.Talon(1) + + def __init__(self): + # OK: variable assigned to `self` + self.motor = wpilib.Talon(1) +``` + +--- + +Import order doesn't really matter, but we prefer the following convention: + +```python + +# Import things from the python standard library first +import os +import typing + +# Import things from robotpy in a second group +import wpilib +import commands2 + +# Import things from the other files in the example last +import constants +import subsystems.drivetrain + +``` + +--- + +The `pass` statement is only required for empty functions: + + +```python +# OK +def empty_function(): + pass + +def has_docstring(): + """Some docstring""" + pass # NOT NEEDED + +class C: + def __init__(self): + super().__init__() + pass # NOT NEEDED +``` + + +Include all the comments +------------------------ + +**IMPORTANT**: Include all the comments from the existing examples. These +comments provide helpful explanations and context for the code. + +Converting Java comments to Python docstrings can be tedious and error prone. We +have a tool at https://github.com/robotpy/devtools/blob/main/sphinxify_server.py +that launches an HTML page that you can just paste doxygen or javadoc comments +into and it will convert it to a mostly usable docstring. + +```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. + +""" +Some docstring describing what this file does +""" + +class SomeClass: + """ + This describes what this class does + """ + + def __init__(self): + """ + This describes what the constructor does + """ + + def myFunction(self, a: int) -> int: + """ + This function is great. + + :param a: Input parameter a + """ + +``` + +Command-based robot specific things +----------------------------------- + +We use `commands2.TimedCommandRobot` instead of TimedRobot. It provides a +`robotPeriodic` method for you, so it doesn't need to be included from +the java code unless robotPeriodic function does something other than +run the command scheduler. + +Java examples will often have a `Constants.java` file with a bunch of constants +in it. RobotPy examples will put those constants in a `constants.py` as globals. +To group constants sometimes it makes sense to put each group in its own class, +but a single `Constants` class should be avoided. + +Final thoughts +-------------- + +Before translating WPILib Java code to RobotPy's WPILib, first take some time +and read through the existing RobotPy code to get a feel for the style of the +code. Try to keep it Pythonic and yet true to the original spirit of the code. +Style *does* matter, as students will be reading through this code and it will +potentially influence their decisions in the future. + +Remember, all contributions are welcome, no matter how big or small! diff --git a/examples/robot/DifferentialDriveBot/drivetrain.py b/examples/robot/DifferentialDriveBot/drivetrain.py new file mode 100755 index 000000000..8abf8a561 --- /dev/null +++ b/examples/robot/DifferentialDriveBot/drivetrain.py @@ -0,0 +1,107 @@ +# +# 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 math + +import wpilib +import wpimath + + +class Drivetrain: + """Represents a differential drive style drivetrain.""" + + kMaxSpeed = 3.0 # meters per second + kMaxAngularSpeed = 2 * math.pi # one rotation per second + + kTrackwidth = 0.381 * 2 # meters + kWheelRadius = 0.0508 # meters + kEncoderResolution = 4096 # counts per revolution + + def __init__(self) -> None: + self.leftLeader = wpilib.PWMSparkMax(1) + self.leftFollower = wpilib.PWMSparkMax(2) + self.rightLeader = wpilib.PWMSparkMax(3) + self.rightFollower = wpilib.PWMSparkMax(4) + + # Make sure both motors for each side are in the same group + self.leftLeader.addFollower(self.leftFollower) + self.rightLeader.addFollower(self.rightFollower) + + # 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) + + self.leftEncoder = wpilib.Encoder(0, 1) + self.rightEncoder = wpilib.Encoder(2, 3) + + self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.kFlat) + + self.leftPIDController = wpimath.PIDController(1.0, 0.0, 0.0) + self.rightPIDController = wpimath.PIDController(1.0, 0.0, 0.0) + + self.kinematics = wpimath.DifferentialDriveKinematics(self.kTrackwidth) + + # Gains are for example purposes only - must be determined for your own robot! + self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3) + + self.imu.resetYaw() + + # Set the distance per pulse for the drive encoders. We can simply use the + # distance traveled for one rotation of the wheel divided by the encoder + # resolution. + self.leftEncoder.setDistancePerPulse( + 2 * math.pi * self.kWheelRadius / self.kEncoderResolution + ) + self.rightEncoder.setDistancePerPulse( + 2 * math.pi * self.kWheelRadius / self.kEncoderResolution + ) + + self.leftEncoder.reset() + self.rightEncoder.reset() + + self.odometry = wpimath.DifferentialDriveOdometry( + self.imu.getRotation2d(), + self.leftEncoder.getDistance(), + self.rightEncoder.getDistance(), + ) + + def setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None: + """Sets the desired wheel speeds. + + :param speeds: The desired wheel speeds. + """ + leftFeedforward = self.feedforward.calculate(speeds.left) + rightFeedforward = self.feedforward.calculate(speeds.right) + + leftOutput = self.leftPIDController.calculate( + self.leftEncoder.getRate(), speeds.left + ) + rightOutput = self.rightPIDController.calculate( + self.rightEncoder.getRate(), speeds.right + ) + + # Controls the left and right sides of the robot using the calculated outputs + self.leftLeader.setVoltage(leftOutput + leftFeedforward) + self.rightLeader.setVoltage(rightOutput + rightFeedforward) + + def drive(self, xSpeed: float, rot: float) -> None: + """Drives the robot with the given linear velocity and angular velocity. + + :param xSpeed: Linear velocity in m/s. + :param rot: Angular velocity in rad/s. + """ + wheelSpeeds = self.kinematics.toWheelSpeeds( + wpimath.ChassisSpeeds(xSpeed, 0.0, rot) + ) + self.setSpeeds(wheelSpeeds) + + def updateOdometry(self) -> None: + """Updates the field-relative position.""" + self.odometry.update( + self.imu.getRotation2d(), + self.leftEncoder.getDistance(), + self.rightEncoder.getDistance(), + ) diff --git a/examples/robot/DifferentialDriveBot/robot.py b/examples/robot/DifferentialDriveBot/robot.py new file mode 100755 index 000000000..90a538be7 --- /dev/null +++ b/examples/robot/DifferentialDriveBot/robot.py @@ -0,0 +1,46 @@ +#!/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 wpimath +import wpilib + +from drivetrain import Drivetrain + + +class MyRobot(wpilib.TimedRobot): + def __init__(self) -> None: + super().__init__() + + self.controller = wpilib.XboxController(0) + self.drive = Drivetrain() + + # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + self.speedLimiter = wpimath.SlewRateLimiter(3) + self.rotLimiter = wpimath.SlewRateLimiter(3) + + def autonomousPeriodic(self) -> None: + self.teleopPeriodic() + self.drive.updateOdometry() + + def teleopPeriodic(self) -> None: + # Get the x speed. We are inverting this because Xbox controllers return + # negative values when we push forward. + xSpeed = ( + -self.speedLimiter.calculate(self.controller.getLeftY()) + * Drivetrain.kMaxSpeed + ) + + # Get the rate of angular rotation. We are inverting this because we want a + # positive value when we pull to the left (remember, CCW is positive in + # mathematics). Xbox controllers return positive values when you pull to + # the right by default. + rot = ( + -self.rotLimiter.calculate(self.controller.getRightX()) + * Drivetrain.kMaxAngularSpeed + ) + + self.drive.drive(xSpeed, rot) diff --git a/examples/robot/DigitalCommunication/robot.py b/examples/robot/DigitalCommunication/robot.py new file mode 100755 index 000000000..2922ad994 --- /dev/null +++ b/examples/robot/DigitalCommunication/robot.py @@ -0,0 +1,49 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program demonstrating how to communicate to a light controller from the robot + code using the roboRIO's DIO ports. + """ + + # define ports for digitalcommunication with light controller + kAlliancePort = 0 + kEnabledPort = 1 + kAutonomousPort = 2 + kAlertPort = 3 + + def __init__(self): + """Robot initialization function""" + super().__init__() + + self.allianceOutput = wpilib.DigitalOutput(self.kAlliancePort) + self.enabledOutput = wpilib.DigitalOutput(self.kEnabledPort) + self.autonomousOutput = wpilib.DigitalOutput(self.kAutonomousPort) + self.alertOutput = wpilib.DigitalOutput(self.kAlertPort) + + def robotPeriodic(self): + setAlliance = False + alliance = wpilib.DriverStation.getAlliance() + if alliance: + setAlliance = alliance == wpilib.DriverStation.Alliance.kRed + + # pull alliance port high if on red alliance, pull low if on blue alliance + self.allianceOutput.set(setAlliance) + + # pull enabled port high if enabled, low if disabled + self.enabledOutput.set(wpilib.DriverStation.isEnabled()) + + # pull auto port high if in autonomous, low if in teleop (or disabled) + self.autonomousOutput.set(wpilib.DriverStation.isAutonomous()) + + # pull alert port high if match time remaining is between 30 and 25 seconds + matchTime = wpilib.DriverStation.getMatchTime() + self.alertOutput.set(30 >= matchTime >= 25) diff --git a/examples/robot/DriveDistanceOffboard/constants.py b/examples/robot/DriveDistanceOffboard/constants.py new file mode 100644 index 000000000..353682280 --- /dev/null +++ b/examples/robot/DriveDistanceOffboard/constants.py @@ -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 + + kMaxSpeed = 3.0 # m/s + kMaxAcceleration = 3.0 # m/s² + + +class OIConstants: + kDriverControllerPort = 0 diff --git a/examples/robot/DriveDistanceOffboard/examplesmartmotorcontroller.py b/examples/robot/DriveDistanceOffboard/examplesmartmotorcontroller.py new file mode 100644 index 000000000..dd4445ae5 --- /dev/null +++ b/examples/robot/DriveDistanceOffboard/examplesmartmotorcontroller.py @@ -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(wpilib.MotorController): + """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._speed = 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, speed: float) -> None: + self._speed = -speed if self._inverted else speed + + def get(self) -> float: + return self._speed + + def setInverted(self, isInverted: bool) -> None: + self._inverted = isInverted + + def getInverted(self) -> bool: + return self._inverted + + def disable(self) -> None: + self._speed = 0.0 + + def stopMotor(self) -> None: + self._speed = 0.0 diff --git a/examples/robot/DriveDistanceOffboard/robot.py b/examples/robot/DriveDistanceOffboard/robot.py new file mode 100644 index 000000000..f221c8fc5 --- /dev/null +++ b/examples/robot/DriveDistanceOffboard/robot.py @@ -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 testInit(self) -> None: + # Cancels all running commands at the start of test mode + commands2.CommandScheduler.getInstance().cancelAll() diff --git a/examples/robot/DriveDistanceOffboard/robotcontainer.py b/examples/robot/DriveDistanceOffboard/robotcontainer.py new file mode 100644 index 000000000..c6fd7fe54 --- /dev/null +++ b/examples/robot/DriveDistanceOffboard/robotcontainer.py @@ -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.driveFullSpeed = commands2.cmd.runOnce( + lambda: self.robotDrive.setMaxOutput(1), self.robotDrive + ) + self.driveHalfSpeed = commands2.cmd.runOnce( + lambda: self.robotDrive.setMaxOutput(0.5), self.robotDrive + ) + + # The driver's controller + self.driverController = commands2.button.CommandXboxController( + 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.CommandXboxController). + """ + + # Configure your button bindings here + + # We can bind commands while retaining references to them in RobotContainer + + # Drive at half speed when the bumper is held + self.driverController.rightBumper().onTrue(self.driveHalfSpeed).onFalse( + self.driveFullSpeed + ) + + # 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() diff --git a/examples/robot/DriveDistanceOffboard/subsystems/drivesubsystem.py b/examples/robot/DriveDistanceOffboard/subsystems/drivesubsystem.py new file mode 100644 index 000000000..7c15b41a3 --- /dev/null +++ b/examples/robot/DriveDistanceOffboard/subsystems/drivesubsystem.py @@ -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, self.rightLeader) + + self.profile = wpimath.TrapezoidProfile( + wpimath.TrapezoidProfile.Constraints( + constants.DriveConstants.kMaxSpeed, + 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 + ) diff --git a/examples/robot/DutyCycleEncoder/robot.py b/examples/robot/DutyCycleEncoder/robot.py new file mode 100755 index 000000000..c462fc44b --- /dev/null +++ b/examples/robot/DutyCycleEncoder/robot.py @@ -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. +# + +""" +This example shows how to use a duty cycle encoder for devices such as +an arm or elevator. +""" + +import wpilib +import wpimath + +FULL_RANGE = 1.3 +EXPECTED_ZERO = 0.0 + + +class MyRobot(wpilib.TimedRobot): + def __init__(self): + """Called once at the beginning of the robot program.""" + super().__init__() + + # 2nd parameter is the range of values. This sensor will output between + # 0 and the passed in value. + # 3rd parameter is the the physical value where you want "0" to be. How + # to measure this is fairly easy. Set the value to 0, place the mechanism + # where you want "0" to be, and observe the value on the dashboard, That + # is the value to enter for the 3rd parameter. + self.dutyCycleEncoder = wpilib.DutyCycleEncoder(0, FULL_RANGE, EXPECTED_ZERO) + + # If you know the frequency of your sensor, uncomment the following + # method, and set the method to the frequency of your sensor. + # This will result in more stable readings from the sensor. + # Do note that occasionally the datasheet cannot be trusted + # and you should measure this value. You can do so with either + # an oscilloscope, or by observing the "Frequency" output + # on the dashboard while running this sample. If you find + # the value jumping between the 2 values, enter halfway between + # those values. This number doesn't have to be perfect, + # just having a fairly close value will make the output readings + # much more stable. + self.dutyCycleEncoder.setAssumedFrequency(967.8) + + def robotPeriodic(self): + # Connected can be checked, and uses the frequency of the encoder + connected = self.dutyCycleEncoder.isConnected() + + # Duty Cycle Frequency in Hz + frequency = self.dutyCycleEncoder.getFrequency() + + # Output of encoder + output = self.dutyCycleEncoder.get() + + # By default, the output will wrap around to the full range value + # when the sensor goes below 0. However, for moving mechanisms this + # is not usually ideal, as if 0 is set to a hard stop, its still + # possible for the sensor to move slightly past. If this happens + # The sensor will assume its now at the furthest away position, + # which control algorithms might not handle correctly. Therefore + # it can be a good idea to slightly shift the output so the sensor + # can go a bit negative before wrapping. Usually 10% or so is fine. + # This does not change where "0" is, so no calibration numbers need + # to be changed. + percentOfRange = FULL_RANGE * 0.1 + shiftedOutput = wpimath.inputModulus( + output, 0 - percentOfRange, FULL_RANGE - percentOfRange + ) + + wpilib.SmartDashboard.putBoolean("Connected", connected) + wpilib.SmartDashboard.putNumber("Frequency", frequency) + wpilib.SmartDashboard.putNumber("Output", output) + wpilib.SmartDashboard.putNumber("ShiftedOutput", shiftedOutput) diff --git a/examples/robot/DutyCycleInput/robot.py b/examples/robot/DutyCycleInput/robot.py new file mode 100755 index 000000000..9d3624824 --- /dev/null +++ b/examples/robot/DutyCycleInput/robot.py @@ -0,0 +1,27 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + def __init__(self): + """Robot initialization function""" + super().__init__() + + self.dutyCycle = wpilib.DutyCycle(0) + + def robotPeriodic(self): + # Duty Cycle Frequency in Hz + frequency = self.dutyCycle.getFrequency() + + # Output of duty cycle, ranging from 0 to 1 + # 1 is fully on, 0 is fully off + output = self.dutyCycle.getOutput() + + wpilib.SmartDashboard.putNumber("Frequency", frequency) + wpilib.SmartDashboard.putNumber("Duty Cycle", output) diff --git a/examples/robot/ElevatorProfiledPID/robot.py b/examples/robot/ElevatorProfiledPID/robot.py new file mode 100644 index 000000000..da54dee9e --- /dev/null +++ b/examples/robot/ElevatorProfiledPID/robot.py @@ -0,0 +1,53 @@ +#!/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 math + +import wpilib +import wpimath + + +class MyRobot(wpilib.TimedRobot): + kDt = 0.02 + kMaxVelocity = 1.75 + kMaxAcceleration = 0.75 + kP = 1.3 + kI = 0.0 + kD = 0.7 + kS = 1.1 + kG = 1.2 + kV = 1.3 + + def __init__(self) -> None: + super().__init__() + self.joystick = wpilib.Joystick(1) + self.encoder = wpilib.Encoder(1, 2) + self.motor = wpilib.PWMSparkMax(1) + + # Create a PID controller whose setpoint's change is subject to maximum + # velocity and acceleration constraints. + self.constraints = wpimath.TrapezoidProfile.Constraints( + self.kMaxVelocity, self.kMaxAcceleration + ) + self.controller = wpimath.ProfiledPIDController( + self.kP, self.kI, self.kD, self.constraints, self.kDt + ) + self.feedforward = wpimath.ElevatorFeedforward(self.kS, self.kG, self.kV) + + self.encoder.setDistancePerPulse(1 / 360 * 2 * math.pi * 1.5) + + def teleopPeriodic(self) -> None: + if self.joystick.getRawButtonPressed(2): + self.controller.setGoal(5) + elif self.joystick.getRawButtonPressed(3): + self.controller.setGoal(0) + + # Run controller and update motor output + self.motor.setVoltage( + self.controller.calculate(self.encoder.getDistance()) + + self.feedforward.calculate(self.controller.getSetpoint().velocity) + ) diff --git a/examples/robot/ElevatorSimulation/constants.py b/examples/robot/ElevatorSimulation/constants.py new file mode 100644 index 000000000..58a6db9d2 --- /dev/null +++ b/examples/robot/ElevatorSimulation/constants.py @@ -0,0 +1,36 @@ +# +# 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 math + +import wpimath.units + +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 + +kElevatorKp = 5.0 +kElevatorKi = 0.0 +kElevatorKd = 0.0 + +kElevatorkS = 0.0 # volts (V) +kElevatorkG = 0.762 # volts (V) +kElevatorkV = 0.762 # volt per velocity (V/(m/s)) +kElevatorkA = 0.0 # volt per acceleration (V/(m/s^2)) + +kElevatorGearing = 10.0 +kElevatorDrumRadius = wpimath.units.inchesToMeters(2.0) +kCarriageMass = 4.0 # kg + +kSetpoint = 0.75 # m +# Encoder is reset to measure 0 at the bottom, so minimum height is 0. +kMinElevatorHeight = 0.0 # m +kMaxElevatorHeight = 1.25 # m + +# distance per pulse = (distance per revolution) / (pulses per revolution) +# = (Pi * D) / ppr +kElevatorEncoderDistPerPulse = 2.0 * math.pi * kElevatorDrumRadius / 4096 diff --git a/examples/robot/ElevatorSimulation/robot.py b/examples/robot/ElevatorSimulation/robot.py new file mode 100755 index 000000000..8a9a6ae45 --- /dev/null +++ b/examples/robot/ElevatorSimulation/robot.py @@ -0,0 +1,40 @@ +#!/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 wpilib + +import constants +from subsystems.elevator import Elevator + + +class MyRobot(wpilib.TimedRobot): + """This is a sample program to demonstrate the use of elevator simulation.""" + + def __init__(self) -> None: + super().__init__() + self.joystick = wpilib.Joystick(constants.kJoystickPort) + self.elevator = Elevator() + + def robotPeriodic(self) -> None: + # Update the telemetry, including mechanism visualization, regardless of mode. + self.elevator.updateTelemetry() + + def simulationPeriodic(self) -> None: + # Update the simulation model. + self.elevator.simulationPeriodic() + + def teleopPeriodic(self) -> None: + if self.joystick.getTrigger(): + # Here, we set the constant setpoint of 0.75 meters. + self.elevator.reachGoal(constants.kSetpoint) + else: + # Otherwise, we update the setpoint to 0. + self.elevator.reachGoal(0.0) + + def disabledInit(self) -> None: + # This just makes sure that our simulation code knows that the motor's off. + self.elevator.stop() diff --git a/examples/robot/ElevatorSimulation/subsystems/elevator.py b/examples/robot/ElevatorSimulation/subsystems/elevator.py new file mode 100644 index 000000000..620a56c49 --- /dev/null +++ b/examples/robot/ElevatorSimulation/subsystems/elevator.py @@ -0,0 +1,107 @@ +# +# 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 wpilib.simulation +import wpimath + +import constants + + +class Elevator: + """Represents the elevator subsystem.""" + + def __init__(self) -> None: + # This gearbox represents a gearbox containing 4 Vex 775pro motors. + self.elevatorGearbox = wpimath.DCMotor.vex775Pro(4) + + # Standard classes for controlling our elevator + self.controller = wpimath.ProfiledPIDController( + constants.kElevatorKp, + constants.kElevatorKi, + constants.kElevatorKd, + wpimath.TrapezoidProfile.Constraints(2.45, 2.45), + ) + self.feedforward = wpimath.ElevatorFeedforward( + constants.kElevatorkS, + constants.kElevatorkG, + constants.kElevatorkV, + constants.kElevatorkA, + ) + self.encoder = wpilib.Encoder( + constants.kEncoderAChannel, constants.kEncoderBChannel + ) + self.motor = wpilib.PWMSparkMax(constants.kMotorPort) + + # Simulation classes help us simulate what's going on, including gravity. + self.elevatorSim = wpilib.simulation.ElevatorSim( + self.elevatorGearbox, + constants.kElevatorGearing, + constants.kCarriageMass, + constants.kElevatorDrumRadius, + constants.kMinElevatorHeight, + constants.kMaxElevatorHeight, + True, + 0, + [0.01, 0.0], + ) + self.encoderSim = wpilib.simulation.EncoderSim(self.encoder) + self.motorSim = wpilib.simulation.PWMMotorControllerSim(self.motor) + + # Create a Mechanism2d visualization of the elevator + self.mech2d = wpilib.Mechanism2d(20, 50) + self.mech2dRoot = self.mech2d.getRoot("Elevator Root", 10, 0) + self.elevatorMech2d = self.mech2dRoot.appendLigament( + "Elevator", self.elevatorSim.getPosition(), 90 + ) + + self.encoder.setDistancePerPulse(constants.kElevatorEncoderDistPerPulse) + + # Publish Mechanism2d to SmartDashboard + # To view the Elevator visualization, select Network Tables -> SmartDashboard -> Elevator Sim + wpilib.SmartDashboard.putData("Elevator Sim", self.mech2d) + + def simulationPeriodic(self) -> None: + # In this method, we update our simulation of what our elevator is doing + # First, we set our "inputs" (voltages) + self.elevatorSim.setInputVoltage( + self.motorSim.getSpeed() * wpilib.RobotController.getBatteryVoltage() + ) + + # Next, we update it. The standard loop time is 20ms. + self.elevatorSim.update(0.020) + + # Finally, we set our simulated encoder's readings and simulated battery voltage + self.encoderSim.setDistance(self.elevatorSim.getPosition()) + # SimBattery estimates loaded battery voltages + wpilib.simulation.RoboRioSim.setVInVoltage( + wpilib.simulation.BatterySim.calculate([self.elevatorSim.getCurrentDraw()]) + ) + + def reachGoal(self, goal: float) -> None: + """Run control loop to reach and maintain goal. + + :param goal: the position to maintain + """ + + self.controller.setGoal(goal) + + # With the setpoint value we run PID control like normal + pidOutput = self.controller.calculate(self.encoder.getDistance()) + feedforwardOutput = self.feedforward.calculate( + self.controller.getSetpoint().velocity + ) + self.motor.setVoltage(pidOutput + feedforwardOutput) + + def stop(self) -> None: + """Stop the control loop and motor output.""" + self.controller.setGoal(0.0) + self.motor.set(0.0) + + def updateTelemetry(self) -> None: + """Update telemetry, including the mechanism visualization.""" + # Update elevator visualization with position + self.elevatorMech2d.setLength(self.encoder.getDistance()) diff --git a/examples/robot/ElevatorTrapezoidProfile/examplesmartmotorcontroller.py b/examples/robot/ElevatorTrapezoidProfile/examplesmartmotorcontroller.py new file mode 100644 index 000000000..dd4445ae5 --- /dev/null +++ b/examples/robot/ElevatorTrapezoidProfile/examplesmartmotorcontroller.py @@ -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(wpilib.MotorController): + """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._speed = 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, speed: float) -> None: + self._speed = -speed if self._inverted else speed + + def get(self) -> float: + return self._speed + + def setInverted(self, isInverted: bool) -> None: + self._inverted = isInverted + + def getInverted(self) -> bool: + return self._inverted + + def disable(self) -> None: + self._speed = 0.0 + + def stopMotor(self) -> None: + self._speed = 0.0 diff --git a/examples/robot/ElevatorTrapezoidProfile/robot.py b/examples/robot/ElevatorTrapezoidProfile/robot.py new file mode 100644 index 000000000..b147646b6 --- /dev/null +++ b/examples/robot/ElevatorTrapezoidProfile/robot.py @@ -0,0 +1,50 @@ +#!/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 wpilib +from wpimath import TrapezoidProfile + +import examplesmartmotorcontroller +import wpimath + + +class MyRobot(wpilib.TimedRobot): + kDt = 0.02 + + def __init__(self): + super().__init__() + self.joystick = wpilib.Joystick(1) + self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1) + # Note: These gains are fake, and will have to be tuned for your robot. + self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 1.5) + + # Create a motion profile with the given maximum velocity and maximum + # acceleration constraints for the next setpoint. + self.profile = TrapezoidProfile(TrapezoidProfile.Constraints(1.75, 0.75)) + + self.goal = TrapezoidProfile.State() + self.setpoint = TrapezoidProfile.State() + + # Note: These gains are fake, and will have to be tuned for your robot. + self.motor.setPID(1.3, 0.0, 0.7) + + def teleopPeriodic(self): + if self.joystick.getRawButtonPressed(2): + self.goal = TrapezoidProfile.State(5, 0) + elif self.joystick.getRawButtonPressed(3): + self.goal = TrapezoidProfile.State(0, 0) + + # Retrieve the profiled setpoint for the next timestep. This setpoint moves + # toward the goal while obeying the constraints. + self.setpoint = self.profile.calculate(self.kDt, self.setpoint, self.goal) + + # Send setpoint to offboard controller PID + self.motor.setSetPoint( + examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, + self.setpoint.position, + self.feedforward.calculate(self.setpoint.velocity) / 12, + ) diff --git a/examples/robot/Encoder/robot.py b/examples/robot/Encoder/robot.py new file mode 100755 index 000000000..04941eda0 --- /dev/null +++ b/examples/robot/Encoder/robot.py @@ -0,0 +1,54 @@ +#!/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 wpilib + +import math + + +class MyRobot(wpilib.TimedRobot): + """ + Sample program displaying the value of a quadrature encoder on the SmartDashboard. Quadrature + Encoders are digital sensors which can detect the amount the encoder has rotated since starting + as well as the direction in which the encoder shaft is rotating. However, encoders can not tell + you the absolute position of the encoder shaft (ie, it considers where it starts to be the zero + position, no matter where it starts), and so can only tell you how much the encoder has rotated + since starting. Depending on the precision of an encoder, it will have fewer or greater ticks per + revolution; the number of ticks per revolution will affect the conversion between ticks and + distance, as specified by DistancePerPulse. One of the most common uses of encoders is in the + drivetrain, so that the distance that the robot drives can be precisely controlled during the + autonomous mode. + """ + + def __init__(self): + """Robot initialization function""" + super().__init__() + + self.encoder = wpilib.Encoder(1, 2, False, wpilib.Encoder.EncodingType.k4X) + + # Defines the number of samples to average when determining the rate. + # On a quadrature encoder, values range from 1-255; + # larger values result in smoother but potentially + # less accurate rates than lower values. + self.encoder.setSamplesToAverage(5) + + # Defines how far the mechanism attached to the encoder moves per pulse. In + # this case, we assume that a 360 count encoder is directly + # attached to a 3 inch diameter (1.5inch radius) wheel, + # and that we want to measure distance in inches. + self.encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * math.pi * 1.5) + + # Defines the lowest rate at which the encoder will + # not be considered stopped, for the purposes of + # the GetStopped() method. Units are in distance / second, + # where distance refers to the units of distance + # that you are using, in this case inches. + self.encoder.setMinRate(1.0) + + def teleopPeriodic(self): + wpilib.SmartDashboard.putNumber("Encoder Distance", self.encoder.getDistance()) + wpilib.SmartDashboard.putNumber("Encoder Rate", self.encoder.getRate()) diff --git a/examples/robot/EventLoop/robot.py b/examples/robot/EventLoop/robot.py new file mode 100644 index 000000000..be7ccbc03 --- /dev/null +++ b/examples/robot/EventLoop/robot.py @@ -0,0 +1,87 @@ +#!/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 wpilib +import wpimath + +SHOT_VELOCITY = 200.0 # rpm +TOLERANCE = 8.0 # rpm + + +class MyRobot(wpilib.TimedRobot): + def __init__(self) -> None: + super().__init__() + + self.shooter = wpilib.PWMSparkMax(0) + self.shooterEncoder = wpilib.Encoder(0, 1) + self.controller = wpimath.PIDController(0.3, 0, 0) + self.feedforward = wpimath.SimpleMotorFeedforwardMeters(0.1, 0.065) + + self.kicker = wpilib.PWMSparkMax(1) + + self.intake = wpilib.PWMSparkMax(2) + + self.loop = wpilib.EventLoop() + self.joystick = wpilib.Joystick(0) + + # Called once at the beginning of the robot program. + self.controller.setTolerance(TOLERANCE) + + isBallAtKickerEvent = wpilib.BooleanEvent( + self.loop, lambda: False + ) # self.kickerSensor.getRange() < KICKER_THRESHOLD + intakeButton = wpilib.BooleanEvent( + self.loop, lambda: self.joystick.getRawButton(2) + ) + + # if the thumb button is held + intakeButton.and_(isBallAtKickerEvent.negate()).ifHigh( + # and there is not a ball at the kicker + # activate the intake + lambda: self.intake.set(0.5) + ) + + # if the thumb button is not held + intakeButton.negate().or_(isBallAtKickerEvent).ifHigh( + # or there is a ball in the kicker + # stop the intake + self.intake.stopMotor + ) + + shootTrigger = wpilib.BooleanEvent(self.loop, self.joystick.getTrigger) + + # if the trigger is held + shootTrigger.ifHigh( + # accelerate the shooter wheel + lambda: self.shooter.setVoltage( + self.controller.calculate(self.shooterEncoder.getRate(), SHOT_VELOCITY) + + self.feedforward.calculate(SHOT_VELOCITY) + ) + ) + + # if not, stop + shootTrigger.negate().ifHigh(self.shooter.stopMotor) + + atTargetVelocity = wpilib.BooleanEvent( + self.loop, self.controller.atSetpoint + ).debounce( + # debounce for more stability + 0.2 + ) + + # if we're at the target velocity, kick the ball into the shooter wheel + atTargetVelocity.ifHigh(lambda: self.kicker.set(0.7)) + + # when we stop being at the target velocity, it means the ball was shot + atTargetVelocity.falling().ifHigh( + # so stop the kicker + self.kicker.stopMotor + ) + + def robotPeriodic(self) -> None: + # poll all the bindings + self.loop.poll() diff --git a/examples/robot/FlywheelBangBangController/robot.py b/examples/robot/FlywheelBangBangController/robot.py new file mode 100755 index 000000000..da6aef569 --- /dev/null +++ b/examples/robot/FlywheelBangBangController/robot.py @@ -0,0 +1,108 @@ +#!/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 wpilib +import math + +import wpilib.simulation +import wpimath +import wpimath.units + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program to demonstrate the use of a BangBangController with a flywheel to + control RPM. + """ + + kMotorPort = 0 + kEncoderAChannel = 0 + kEncoderBChannel = 1 + + # Max setpoint for joystick control in RPM + kMaxSetpointValue = 6000.0 + + # Gains are for example purposes only - must be determined for your own robot! + kFlywheelKs = 0.0001 # V + kFlywheelKv = 0.000195 # V/RPM + kFlywheelKa = 0.0003 # V/(RPM/s) + + # Reduction between motors and encoder, as output over input. If the flywheel + # spins slower than the motors, this number should be greater than one. + kFlywheelGearing = 1.0 + + # 1/2 MR² + kFlywheelMomentOfInertia = ( + 0.5 + * wpimath.units.lbsToKilograms(1.5) + * math.pow(wpimath.units.inchesToMeters(4), 2) + ) + + def __init__(self) -> None: + """Robot initialization function""" + super().__init__() + + self.flywheelMotor = wpilib.PWMSparkMax(self.kMotorPort) + self.encoder = wpilib.Encoder(self.kEncoderAChannel, self.kEncoderBChannel) + + self.bangBangControler = wpimath.BangBangController() + + # Gains are for example purposes only - must be determined for your own robot! + self.feedforward = wpimath.SimpleMotorFeedforwardMeters( + self.kFlywheelKs, self.kFlywheelKv, self.kFlywheelKa + ) + + # Joystick to control setpoint + self.joystick = wpilib.Joystick(0) + + # Simulation classes help us simulate our robot + + self.gearbox = wpimath.DCMotor.NEO(1) + + self.plant = wpimath.LinearSystemId.flywheelSystem( + self.gearbox, self.kFlywheelGearing, self.kFlywheelMomentOfInertia + ) + + self.flywheelSim = wpilib.simulation.FlywheelSim(self.plant, self.gearbox) + self.encoderSim = wpilib.simulation.EncoderSim(self.encoder) + + # Add bang-bang controller to SmartDashboard and networktables. + wpilib.SmartDashboard.putData(self.bangBangControler) + + def teleopPeriodic(self) -> None: + """Controls flywheel to a set speed (RPM) controlled by a joystick.""" + + # Scale setpoint value between 0 and maxSetpointValue + setpoint = max( + 0.0, + self.joystick.getRawAxis(0) + * wpimath.units.rotationsPerMinuteToRadiansPerSecond( + self.kMaxSetpointValue + ), + ) + + # Set setpoint and measurement of the bang-bang controller + bangOutput = ( + self.bangBangControler.calculate(self.encoder.getRate(), setpoint) * 12.0 + ) + + # Controls a motor with the output of the BangBang controller and a + # feedforward. The feedforward is reduced slightly to avoid overspeeding + # the shooter. + self.flywheelMotor.setVoltage( + bangOutput + 0.9 * self.feedforward.calculate(setpoint) + ) + + def simulationPeriodic(self) -> None: + """Update our simulation. This should be run every robot loop in simulation.""" + # To update our simulation, we set motor voltage inputs, update the + # simulation, and write the simulated velocities to our simulated encoder + self.flywheelSim.setInputVoltage( + self.flywheelMotor.get() * wpilib.RobotController.getInputVoltage() + ) + self.flywheelSim.update(0.02) + self.encoderSim.setRate(self.flywheelSim.getAngularVelocity()) diff --git a/examples/robot/GettingStarted/robot.py b/examples/robot/GettingStarted/robot.py new file mode 100755 index 000000000..abcdc9325 --- /dev/null +++ b/examples/robot/GettingStarted/robot.py @@ -0,0 +1,56 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + def __init__(self): + """ + This function is called upon program startup and + should be used for any initialization code. + """ + super().__init__() + self.leftDrive = wpilib.PWMSparkMax(0) + self.rightDrive = wpilib.PWMSparkMax(1) + self.robotDrive = wpilib.DifferentialDrive(self.leftDrive, self.rightDrive) + self.controller = wpilib.XboxController(0) + self.timer = wpilib.Timer() + + # 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.rightDrive.setInverted(True) + + def autonomousInit(self): + """This function is run once each time the robot enters autonomous mode.""" + self.timer.restart() + + def autonomousPeriodic(self): + """This function is called periodically during autonomous.""" + + # Drive for two seconds + if self.timer.get() < 2.0: + # Drive forwards half speed, make sure to turn input squaring off + self.robotDrive.arcadeDrive(0.5, 0, squareInputs=False) + else: + self.robotDrive.stopMotor() # Stop robot + + def teleopInit(self): + """This function is called once each time the robot enters teleoperated mode.""" + + def teleopPeriodic(self): + """This function is called periodically during teleoperated mode.""" + self.robotDrive.arcadeDrive( + -self.controller.getLeftY(), -self.controller.getRightX() + ) + + def testInit(self): + """This function is called once each time the robot enters test mode.""" + + def testPeriodic(self): + """This function is called periodically during test mode.""" diff --git a/examples/robot/Gyro/robot.py b/examples/robot/Gyro/robot.py new file mode 100755 index 000000000..aad7eff2a --- /dev/null +++ b/examples/robot/Gyro/robot.py @@ -0,0 +1,47 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program to demonstrate how to use a gyro sensor to make a robot drive straight. + This program uses a joystick to drive forwards and backwards while the gyro is used for direction + keeping. + """ + + kAngleSetpoint = 0.0 + kP = 0.005 # proportional turning constant + + kLeftMotorPort = 0 + kRightMotorPort = 1 + kIMUMountOrientation = wpilib.OnboardIMU.MountOrientation.kFlat + kJoystickPort = 0 + + def __init__(self) -> None: + """Robot initialization function""" + super().__init__() + + self.leftDrive = wpilib.PWMSparkMax(self.kLeftMotorPort) + self.rightDrive = wpilib.PWMSparkMax(self.kRightMotorPort) + self.myRobot = wpilib.DifferentialDrive(self.leftDrive, self.rightDrive) + self.imu = wpilib.OnboardIMU(self.kIMUMountOrientation) + self.joystick = wpilib.Joystick(self.kJoystickPort) + + # 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.rightDrive.setInverted(True) + + def teleopPeriodic(self) -> None: + # The motor speed is set from the joystick while the DifferentialDrive turning value is assigned + # from the error between the setpoint and the gyro angle. + turningValue = ( + self.kAngleSetpoint - self.imu.getRotation2d().degrees() + ) * self.kP + self.myRobot.arcadeDrive(-self.joystick.getY(), -turningValue) diff --git a/examples/robot/GyroMecanum/robot.py b/examples/robot/GyroMecanum/robot.py new file mode 100755 index 000000000..bb06d6d3d --- /dev/null +++ b/examples/robot/GyroMecanum/robot.py @@ -0,0 +1,50 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation vectors + in relation to the starting orientation of the robot (field-oriented controls). + """ + + kFrontLeftChannel = 0 + kRearLeftChannel = 1 + kFrontRightChannel = 2 + kRearRightChannel = 3 + kIMUMountOrientation = wpilib.OnboardIMU.MountOrientation.kFlat + kJoystickPort = 0 + + def __init__(self) -> None: + """Robot initialization function""" + super().__init__() + + self.imu = wpilib.OnboardIMU(self.kIMUMountOrientation) + self.joystick = wpilib.Joystick(self.kJoystickPort) + + frontLeft = wpilib.PWMSparkMax(self.kFrontLeftChannel) + rearLeft = wpilib.PWMSparkMax(self.kRearLeftChannel) + frontRight = wpilib.PWMSparkMax(self.kFrontRightChannel) + rearRight = wpilib.PWMSparkMax(self.kRearRightChannel) + + frontRight.setInverted(True) + rearRight.setInverted(True) + + self.robotDrive = wpilib.MecanumDrive( + frontLeft, rearLeft, frontRight, rearRight + ) + + def teleopPeriodic(self) -> None: + self.robotDrive.driveCartesian( + -self.joystick.getY(), + -self.joystick.getX(), + -self.joystick.getZ(), + self.imu.getRotation2d(), + ) diff --git a/examples/robot/HatchbotInlined/commands/autos.py b/examples/robot/HatchbotInlined/commands/autos.py new file mode 100644 index 000000000..f888d2176 --- /dev/null +++ b/examples/robot/HatchbotInlined/commands/autos.py @@ -0,0 +1,75 @@ +# +# 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 subsystems.drivesubsystem +import subsystems.hatchsubsystem + +import constants + + +class Autos: + """Container for auto command factories.""" + + def __init__(self) -> None: + raise Exception("This is a utility class!") + + @staticmethod + def simpleAuto(drive: subsystems.drivesubsystem.DriveSubsystem): + """A simple auto routine that drives forward a specified distance, and then stops.""" + return commands2.FunctionalCommand( + # Reset encoders on command start + drive.resetEncoders, + # Drive forward while the command is executing, + lambda: drive.arcadeDrive(constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: drive.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: drive.getAverageEncoderDistance() + >= constants.kAutoDriveDistanceInches, + # Require the drive subsystem + ) + + @staticmethod + def complexAuto( + driveSubsystem: subsystems.drivesubsystem.DriveSubsystem, + hatchSubsystem: subsystems.hatchsubsystem.HatchSubsystem, + ): + """A complex auto routine that drives forward, drops a hatch, and then drives backward.""" + return commands2.cmd.sequence( + # Drive forward up to the front of the cargo ship + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders, + # Drive forward while the command is executing + lambda: driveSubsystem.arcadeDrive(constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: driveSubsystem.getAverageEncoderDistance() + >= constants.kAutoDriveDistanceInches, + # Require the drive subsystem + driveSubsystem, + ), + # Release the hatch + hatchSubsystem.releaseHatch(), + # Drive backward the specified distance + commands2.FunctionalCommand( + # Reset encoders on command start + driveSubsystem.resetEncoders, + # Drive backwards while the command is executing + lambda: driveSubsystem.arcadeDrive(-constants.kAutoDriveSpeed, 0), + # Stop driving at the end of the command + lambda interrupt: driveSubsystem.arcadeDrive(0, 0), + # End the command when the robot's driven distance exceeds the desired value + lambda: abs(driveSubsystem.getAverageEncoderDistance()) + >= constants.kAutoBackupDistanceInches, + # Require the drive subsystem + driveSubsystem, + ), + ) diff --git a/examples/robot/HatchbotInlined/constants.py b/examples/robot/HatchbotInlined/constants.py new file mode 100644 index 000000000..82dfdf212 --- /dev/null +++ b/examples/robot/HatchbotInlined/constants.py @@ -0,0 +1,51 @@ +# +# 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. +# + +# +# The constants module is a convenience place for teams to hold robot-wide +# numerical or boolean constants. Don't use this for any other purpose! +# + +import math +import wpilib + +# Motors +kLeftMotor1Port = 0 +kLeftMotor2Port = 1 +kRightMotor1Port = 2 +kRightMotor2Port = 3 + +# Encoders +kLeftEncoderPorts = (0, 1) +kRightEncoderPorts = (2, 3) +kLeftEncoderReversed = False +kRightEncoderReversed = True + +kEncoderCPR = 1024 +kWheelDiameterInches = 6 +# Assumes the encoders are directly mounted on the wheel shafts +kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR + +# Hatch +kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM +kHatchSolenoidModule = 0 +kHatchSolenoidPorts = (0, 1) + +# Autonomous +kAutoDriveDistanceInches = 60 +kAutoBackupDistanceInches = 20 +kAutoDriveSpeed = 0.5 + +# Operator Interface +kDriverControllerPort = 0 + +# Physical parameters +kDriveTrainMotorCount = 2 +kTrackWidth = 0.381 * 2 +kGearingRatio = 8 +kWheelRadius = 0.0508 + +# kEncoderResolution = - diff --git a/examples/robot/HatchbotInlined/robot.py b/examples/robot/HatchbotInlined/robot.py new file mode 100644 index 000000000..bbb9f5b3d --- /dev/null +++ b/examples/robot/HatchbotInlined/robot.py @@ -0,0 +1,71 @@ +#!/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 commands2 +import wpilib + +from robotcontainer import RobotContainer + + +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 + """ + + autonomousCommand: typing.Optional[commands2.Command] = None + + 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__() + + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = RobotContainer() + + # Start recording to data log + wpilib.DataLogManager.start() + + # Record DS control and joystick data. + # Change to `false` to not record joystick data. + wpilib.DriverStation.startDataLog(wpilib.DataLogManager.getLog(), True) + + 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() + + if self.autonomousCommand: + self.autonomousCommand.schedule() + + 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: + self.autonomousCommand.cancel() + + def teleopPeriodic(self) -> None: + """This function is called periodically during operator control""" + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode + commands2.CommandScheduler.getInstance().cancelAll() diff --git a/examples/robot/HatchbotInlined/robotcontainer.py b/examples/robot/HatchbotInlined/robotcontainer.py new file mode 100644 index 000000000..6712f5cb0 --- /dev/null +++ b/examples/robot/HatchbotInlined/robotcontainer.py @@ -0,0 +1,95 @@ +# +# 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.CommandPS4Controller( + 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() diff --git a/examples/robot/HatchbotInlined/subsystems/__init__.py b/examples/robot/HatchbotInlined/subsystems/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/robot/HatchbotInlined/subsystems/drivesubsystem.py b/examples/robot/HatchbotInlined/subsystems/drivesubsystem.py new file mode 100644 index 000000000..bab17b6a4 --- /dev/null +++ b/examples/robot/HatchbotInlined/subsystems/drivesubsystem.py @@ -0,0 +1,68 @@ +# +# 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 wpilib +import constants + + +class DriveSubsystem(commands2.Subsystem): + def __init__(self) -> None: + + self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port) + self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port) + self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) + self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) + + self.left1.addFollower(self.left2) + self.right1.addFollower(self.right2) + + # We need to invert one side of the drivetrain so that positive speeds + # result in both sides moving forward. Depending on how your robot's + # drivetrain is constructed, you might have to invert the left side instead. + self.right1.setInverted(True) + + # The robot's drive + self.drive = wpilib.DifferentialDrive(self.left1, self.right1) + + # The left-side drive encoder + self.leftEncoder = wpilib.Encoder( + *constants.kLeftEncoderPorts, + reverseDirection=constants.kLeftEncoderReversed + ) + + # The right-side drive encoder + self.rightEncoder = wpilib.Encoder( + *constants.kRightEncoderPorts, + reverseDirection=constants.kRightEncoderReversed + ) + + # Sets the distance per pulse for the encoders + self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + + def arcadeDrive(self, fwd: float, rot: float) -> None: + """ + Drives the robot using arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + self.drive.arcadeDrive(fwd, rot) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + + def getAverageEncoderDistance(self) -> float: + """Gets the average distance of the TWO encoders.""" + return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 + + def setMaxOutput(self, maxOutput: float): + """ + Sets the max output of the drive. Useful for scaling the + drive to drive more slowly. + """ + self.drive.setMaxOutput(maxOutput) diff --git a/examples/robot/HatchbotInlined/subsystems/hatchsubsystem.py b/examples/robot/HatchbotInlined/subsystems/hatchsubsystem.py new file mode 100644 index 000000000..55f05e156 --- /dev/null +++ b/examples/robot/HatchbotInlined/subsystems/hatchsubsystem.py @@ -0,0 +1,33 @@ +# +# 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.cmd + +import constants + + +class HatchSubsystem(commands2.Subsystem): + def __init__(self) -> None: + + self.hatchSolenoid = wpilib.DoubleSolenoid( + constants.kHatchSolenoidModule, + constants.kHatchSolenoidModuleType, + *constants.kHatchSolenoidPorts + ) + + def grabHatch(self) -> commands2.Command: + """Grabs the hatch""" + return commands2.cmd.runOnce( + lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward), self + ) + + def releaseHatch(self) -> commands2.Command: + """Releases the hatch""" + return commands2.cmd.runOnce( + lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), self + ) diff --git a/examples/robot/HatchbotTraditional/commands/complexauto.py b/examples/robot/HatchbotTraditional/commands/complexauto.py new file mode 100644 index 000000000..398c680a6 --- /dev/null +++ b/examples/robot/HatchbotTraditional/commands/complexauto.py @@ -0,0 +1,35 @@ +# +# 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 constants + +from .drivedistance import DriveDistance +from .releasehatch import ReleaseHatch + +from subsystems.drivesubsystem import DriveSubsystem +from subsystems.hatchsubsystem import HatchSubsystem + + +class ComplexAuto(commands2.SequentialCommandGroup): + """ + A complex auto command that drives forward, releases a hatch, and then drives backward. + """ + + def __init__(self, drive: DriveSubsystem, hatch: HatchSubsystem): + super().__init__( + # Drive forward the specified distance + DriveDistance( + constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, drive + ), + # Release the hatch + ReleaseHatch(hatch), + # Drive backward the specified distance + DriveDistance( + constants.kAutoBackupDistanceInches, -constants.kAutoDriveSpeed, drive + ), + ) diff --git a/examples/robot/HatchbotTraditional/commands/defaultdrive.py b/examples/robot/HatchbotTraditional/commands/defaultdrive.py new file mode 100644 index 000000000..707cda000 --- /dev/null +++ b/examples/robot/HatchbotTraditional/commands/defaultdrive.py @@ -0,0 +1,27 @@ +# +# 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 commands2 +from subsystems.drivesubsystem import DriveSubsystem + + +class DefaultDrive(commands2.Command): + def __init__( + self, + drive: DriveSubsystem, + forward: typing.Callable[[], float], + rotation: typing.Callable[[], float], + ) -> None: + + self.drive = drive + self.forward = forward + self.rotation = rotation + + self.addRequirements(self.drive) + + def execute(self) -> None: + self.drive.arcadeDrive(self.forward(), self.rotation()) diff --git a/examples/robot/HatchbotTraditional/commands/drivedistance.py b/examples/robot/HatchbotTraditional/commands/drivedistance.py new file mode 100644 index 000000000..2201b3ffb --- /dev/null +++ b/examples/robot/HatchbotTraditional/commands/drivedistance.py @@ -0,0 +1,30 @@ +# +# 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 + +from subsystems.drivesubsystem import DriveSubsystem + + +class DriveDistance(commands2.Command): + def __init__(self, inches: float, speed: float, drive: DriveSubsystem) -> None: + self.distance = inches + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + self.drive.resetEncoders() + self.drive.arcadeDrive(self.speed, 0) + + def execute(self) -> None: + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + return abs(self.drive.getAverageEncoderDistance()) >= self.distance diff --git a/examples/robot/HatchbotTraditional/commands/grabhatch.py b/examples/robot/HatchbotTraditional/commands/grabhatch.py new file mode 100644 index 000000000..41fe9ef8e --- /dev/null +++ b/examples/robot/HatchbotTraditional/commands/grabhatch.py @@ -0,0 +1,20 @@ +# +# 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 +from subsystems.hatchsubsystem import HatchSubsystem + + +class GrabHatch(commands2.Command): + def __init__(self, hatch: HatchSubsystem) -> None: + self.hatch = hatch + self.addRequirements(hatch) + + def initialize(self) -> None: + self.hatch.grabHatch() + + def isFinished(self) -> bool: + return True diff --git a/examples/robot/HatchbotTraditional/commands/halvedrivespeed.py b/examples/robot/HatchbotTraditional/commands/halvedrivespeed.py new file mode 100644 index 000000000..54198fac2 --- /dev/null +++ b/examples/robot/HatchbotTraditional/commands/halvedrivespeed.py @@ -0,0 +1,20 @@ +# +# 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 + +from subsystems.drivesubsystem import DriveSubsystem + + +class HalveDriveSpeed(commands2.Command): + def __init__(self, drive: DriveSubsystem) -> None: + self.drive = drive + + def initialize(self) -> None: + self.drive.setMaxOutput(0.5) + + def end(self, interrupted: bool) -> None: + self.drive.setMaxOutput(1.0) diff --git a/examples/robot/HatchbotTraditional/commands/releasehatch.py b/examples/robot/HatchbotTraditional/commands/releasehatch.py new file mode 100644 index 000000000..3d9718f5a --- /dev/null +++ b/examples/robot/HatchbotTraditional/commands/releasehatch.py @@ -0,0 +1,14 @@ +# +# 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 + +from subsystems.hatchsubsystem import HatchSubsystem + + +class ReleaseHatch(commands2.InstantCommand): + def __init__(self, hatch: HatchSubsystem) -> None: + super().__init__(hatch.releaseHatch, hatch) diff --git a/examples/robot/HatchbotTraditional/constants.py b/examples/robot/HatchbotTraditional/constants.py new file mode 100644 index 000000000..82dfdf212 --- /dev/null +++ b/examples/robot/HatchbotTraditional/constants.py @@ -0,0 +1,51 @@ +# +# 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. +# + +# +# The constants module is a convenience place for teams to hold robot-wide +# numerical or boolean constants. Don't use this for any other purpose! +# + +import math +import wpilib + +# Motors +kLeftMotor1Port = 0 +kLeftMotor2Port = 1 +kRightMotor1Port = 2 +kRightMotor2Port = 3 + +# Encoders +kLeftEncoderPorts = (0, 1) +kRightEncoderPorts = (2, 3) +kLeftEncoderReversed = False +kRightEncoderReversed = True + +kEncoderCPR = 1024 +kWheelDiameterInches = 6 +# Assumes the encoders are directly mounted on the wheel shafts +kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR + +# Hatch +kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM +kHatchSolenoidModule = 0 +kHatchSolenoidPorts = (0, 1) + +# Autonomous +kAutoDriveDistanceInches = 60 +kAutoBackupDistanceInches = 20 +kAutoDriveSpeed = 0.5 + +# Operator Interface +kDriverControllerPort = 0 + +# Physical parameters +kDriveTrainMotorCount = 2 +kTrackWidth = 0.381 * 2 +kGearingRatio = 8 +kWheelRadius = 0.0508 + +# kEncoderResolution = - diff --git a/examples/robot/HatchbotTraditional/robot.py b/examples/robot/HatchbotTraditional/robot.py new file mode 100644 index 000000000..bbb9f5b3d --- /dev/null +++ b/examples/robot/HatchbotTraditional/robot.py @@ -0,0 +1,71 @@ +#!/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 commands2 +import wpilib + +from robotcontainer import RobotContainer + + +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 + """ + + autonomousCommand: typing.Optional[commands2.Command] = None + + 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__() + + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = RobotContainer() + + # Start recording to data log + wpilib.DataLogManager.start() + + # Record DS control and joystick data. + # Change to `false` to not record joystick data. + wpilib.DriverStation.startDataLog(wpilib.DataLogManager.getLog(), True) + + 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() + + if self.autonomousCommand: + self.autonomousCommand.schedule() + + 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: + self.autonomousCommand.cancel() + + def teleopPeriodic(self) -> None: + """This function is called periodically during operator control""" + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode + commands2.CommandScheduler.getInstance().cancelAll() diff --git a/examples/robot/HatchbotTraditional/robotcontainer.py b/examples/robot/HatchbotTraditional/robotcontainer.py new file mode 100644 index 000000000..07dd6d0dc --- /dev/null +++ b/examples/robot/HatchbotTraditional/robotcontainer.py @@ -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.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.XboxController(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() diff --git a/examples/robot/HatchbotTraditional/subsystems/__init__.py b/examples/robot/HatchbotTraditional/subsystems/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/robot/HatchbotTraditional/subsystems/drivesubsystem.py b/examples/robot/HatchbotTraditional/subsystems/drivesubsystem.py new file mode 100644 index 000000000..09bd8f132 --- /dev/null +++ b/examples/robot/HatchbotTraditional/subsystems/drivesubsystem.py @@ -0,0 +1,65 @@ +# +# 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 wpilib +import constants + + +class DriveSubsystem(commands2.Subsystem): + def __init__(self) -> None: + + self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port) + self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port) + self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) + self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) + + # We need to invert one side of the drivetrain so that positive speeds + # result in both sides moving forward. Depending on how your robot's + # drivetrain is constructed, you might have to invert the left side instead. + self.right1.setInverted(True) + + # The robot's drive + self.drive = wpilib.DifferentialDrive(self.left1, self.right1) + + # The left-side drive encoder + self.leftEncoder = wpilib.Encoder( + *constants.kLeftEncoderPorts, + reverseDirection=constants.kLeftEncoderReversed + ) + + # The right-side drive encoder + self.rightEncoder = wpilib.Encoder( + *constants.kRightEncoderPorts, + reverseDirection=constants.kRightEncoderReversed + ) + + # Sets the distance per pulse for the encoders + self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) + + def arcadeDrive(self, fwd: float, rot: float) -> None: + """ + Drives the robot using arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + self.drive.arcadeDrive(fwd, rot) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + + def getAverageEncoderDistance(self) -> float: + """Gets the average distance of the TWO encoders.""" + return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 + + def setMaxOutput(self, maxOutput: float): + """ + Sets the max output of the drive. Useful for scaling the + drive to drive more slowly. + """ + self.drive.setMaxOutput(maxOutput) diff --git a/examples/robot/HatchbotTraditional/subsystems/hatchsubsystem.py b/examples/robot/HatchbotTraditional/subsystems/hatchsubsystem.py new file mode 100644 index 000000000..3070e730c --- /dev/null +++ b/examples/robot/HatchbotTraditional/subsystems/hatchsubsystem.py @@ -0,0 +1,28 @@ +# +# 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 + + +class HatchSubsystem(commands2.Subsystem): + def __init__(self) -> None: + + self.hatchSolenoid = wpilib.DoubleSolenoid( + constants.kHatchSolenoidModule, + constants.kHatchSolenoidModuleType, + *constants.kHatchSolenoidPorts + ) + + def grabHatch(self) -> None: + """Grabs the hatch""" + self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward) + + def releaseHatch(self) -> None: + """Releases the hatch""" + self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse) diff --git a/examples/robot/HidRumble/robot.py b/examples/robot/HidRumble/robot.py new file mode 100755 index 000000000..0e7cf6f99 --- /dev/null +++ b/examples/robot/HidRumble/robot.py @@ -0,0 +1,30 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing the use of GenericHID's rumble feature. + """ + + def __init__(self): + """Robot initialization function""" + super().__init__() + + self.hid = wpilib.XboxController(0) + + def autonomousInit(self): + # Turn on rumble at the start of auto + self.hid.setRumble(wpilib.XboxController.RumbleType.kLeftRumble, 1.0) + self.hid.setRumble(wpilib.XboxController.RumbleType.kRightRumble, 1.0) + + def disabledInit(self): + # Stop the rumble when entering disabled + self.hid.setRumble(wpilib.XboxController.RumbleType.kLeftRumble, 0.0) + self.hid.setRumble(wpilib.XboxController.RumbleType.kRightRumble, 0.0) diff --git a/examples/robot/HttpCamera/robot.py b/examples/robot/HttpCamera/robot.py new file mode 100644 index 000000000..0efdc84b3 --- /dev/null +++ b/examples/robot/HttpCamera/robot.py @@ -0,0 +1,20 @@ +#!/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 wpilib +import wpilib.cameraserver + + +class MyRobot(wpilib.TimedRobot): + def __init__(self): + super().__init__() + # Your image processing code will be launched via a stub that will set up logging and initialize NetworkTables + # to talk to your robot code. + # https://robotpy.readthedocs.io/en/stable/vision/roborio.html#important-notes + + wpilib.CameraServer.launch("vision.py:main") diff --git a/examples/robot/HttpCamera/vision.py b/examples/robot/HttpCamera/vision.py new file mode 100644 index 000000000..c3fc6d8af --- /dev/null +++ b/examples/robot/HttpCamera/vision.py @@ -0,0 +1,72 @@ +# +# 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. +# + +""" +This is a demo program showing the use of OpenCV to do vision processing. The image is acquired +from an HTTP camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has +many methods for different types of processing. +""" + +import numpy +import cscore +from cscore import CameraServer +import cv2 + + +# +# This code will work both on a RoboRIO and on other platforms. The exact mechanism +# to run it differs depending on whether you’re on a RoboRIO or a coprocessor +# +# https://robotpy.readthedocs.io/en/stable/vision/code.html + + +def main(): + # Create an HTTP camera. The address will need to be modified to have the + # correct team number. The exact path will depend on the source. + camera = cscore.HttpCamera("HTTP Camera", "http://10.x.y.11/video/stream.mjpg") + # Start capturing images + CameraServer.startAutomaticCapture(camera) + # Set the resolution + camera.setResolution(640, 480) + + # Get a CvSink. This will capture Mats from the camera + cvSink = CameraServer.getVideo() + + # Setup a CvSource. This will send images back to the Dashboard + outputStream = CameraServer.putVideo("Rectangle", 640, 480) + + # Mats are very memory expensive. Lets reuse this Mat. + mat = numpy.zeros((480, 640, 3), dtype="uint8") + + # Declare the color of the rectangle + rectColor = (255, 255, 255) + + # The camera code will be killed when the robot.py program exits. If you wish to perform cleanup, + # you should register an atexit handler. The child process will NOT be launched when running the robot code in + # simulation or unit testing mode + + while True: + # Tell the CvSink to grab a frame from the camera and put it in the source mat. If there is an error notify the + # output. + + if cvSink.grabFrame(mat) == 0: + # Send the output the error. + outputStream.notifyError(cvSink.getError()) + + # skip the rest of the current iteration + continue + + # Put a rectangle on the image + mat = cv2.rectangle( + img=mat, + pt1=(100, 100), + pt2=(400, 400), + color=rectColor, + lineType=5, + ) + + # Give the output stream a new image to display + outputStream.putFrame(mat) diff --git a/examples/robot/I2CCommunication/robot.py b/examples/robot/I2CCommunication/robot.py new file mode 100755 index 000000000..8c56394c9 --- /dev/null +++ b/examples/robot/I2CCommunication/robot.py @@ -0,0 +1,56 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program demonstrating how to communicate to a light controller from the robot + code using the roboRIO's I2C port. + """ + + PORT = wpilib.I2C.Port.kPort0 + DEVICE_ADDRESS = 4 + + def __init__(self): + """Robot initialization function""" + super().__init__() + + self.arduino = wpilib.I2C(self.PORT, self.DEVICE_ADDRESS) + + def writeString(self, s: str): + # Creates a char array from the input string + chars = s.encode("ascii") + + # Writes bytes over I2C + self.arduino.writeBulk(chars) + + def robotPeriodic(self): + # Creates a string to hold current robot state information, including + # alliance, enabled state, operation mode, and match time. The message + # is sent in format "AEM###" where A is the alliance color, (R)ed or + # (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the + # operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded + # time remaining in the match. + # + # For example, "RET043" would indicate that the robot is on the red + # alliance, enabled in teleop mode, with 43 seconds left in the match. + allianceString = "U" + alliance = wpilib.DriverStation.getAlliance() + if alliance is not None: + allianceString = ( + "R" if alliance == wpilib.DriverStation.Alliance.kRed else "B" + ) + + enabledString = "E" if wpilib.DriverStation.isEnabled() else "D" + autoString = "A" if wpilib.DriverStation.isAutonomous() else "T" + matchTime = wpilib.DriverStation.getMatchTime() + + stateMessage = f"{allianceString}{enabledString}{autoString}{matchTime:03f}" + + self.writeString(stateMessage) diff --git a/examples/robot/IntermediateVision/robot.py b/examples/robot/IntermediateVision/robot.py new file mode 100755 index 000000000..a40fa6044 --- /dev/null +++ b/examples/robot/IntermediateVision/robot.py @@ -0,0 +1,20 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing the use of OpenCV to do vision processing. The image is acquired + from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has + many methods for different types of processing. + """ + + def __init__(self): + super().__init__() + wpilib.CameraServer.launch("vision.py:main") diff --git a/examples/robot/IntermediateVision/vision.py b/examples/robot/IntermediateVision/vision.py new file mode 100644 index 000000000..d473b288f --- /dev/null +++ b/examples/robot/IntermediateVision/vision.py @@ -0,0 +1,53 @@ +# +# 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. +# + +# +# This is a demo program showing the use of OpenCV to do vision processing. The image is acquired +# from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has +# many methods for different types of processing. +# +# NOTE: This code runs in its own process, so we cannot access the robot here, +# nor can we create/use/see wpilib objects +# +# To try this code out locally (if you have robotpy-cscore installed), you +# can execute `python3 -m cscore vision.py:main` +# + +import cv2 +import numpy as np + +from cscore import CameraServer as CS + + +def main(): + # Get the UsbCamera from CameraServer + camera = CS.startAutomaticCapture() + # Set the resolution + camera.setResolution(640, 480) + + # Get a CvSink. This will capture images from the camera + cvSink = CS.getVideo() + # Setup a CvSource. This will send images back to the Dashboard + outputStream = CS.putVideo("Rectangle", 640, 480) + + # Mats are very memory expensive. Lets reuse this Mat. + mat = np.zeros(shape=(480, 640, 3), dtype=np.uint8) + + while True: + # Tell the CvSink to grab a frame from the camera and put it + # in the source image. If there is an error notify the output. + time, mat = cvSink.grabFrame(mat) + if time == 0: + # Send the output the error. + outputStream.notifyError(cvSink.getError()) + # skip the rest of the current iteration + continue + + # Put a rectangle on the image + cv2.rectangle(mat, (100, 100), (400, 400), (255, 255, 255), 5) + + # Give the output stream a new image to display + outputStream.putFrame(mat) diff --git a/examples/robot/MecanumBot/drivetrain.py b/examples/robot/MecanumBot/drivetrain.py new file mode 100755 index 000000000..09f059119 --- /dev/null +++ b/examples/robot/MecanumBot/drivetrain.py @@ -0,0 +1,127 @@ +# +# 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 math + +import wpilib +import wpimath + + +class Drivetrain: + """Represents a mecanum drive style drivetrain.""" + + kMaxSpeed = 3.0 # 3 meters per second + kMaxAngularSpeed = math.pi # 1/2 rotation per second + + def __init__(self) -> None: + self.frontLeftMotor = wpilib.PWMSparkMax(1) + self.frontRightMotor = wpilib.PWMSparkMax(2) + self.backLeftMotor = wpilib.PWMSparkMax(3) + self.backRightMotor = wpilib.PWMSparkMax(4) + + self.frontLeftEncoder = wpilib.Encoder(0, 1) + self.frontRightEncoder = wpilib.Encoder(2, 3) + self.backLeftEncoder = wpilib.Encoder(4, 5) + self.backRightEncoder = wpilib.Encoder(6, 7) + + self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.Translation2d(-0.381, -0.381) + + self.frontLeftPIDController = wpimath.PIDController(1, 0, 0) + self.frontRightPIDController = wpimath.PIDController(1, 0, 0) + self.backLeftPIDController = wpimath.PIDController(1, 0, 0) + self.backRightPIDController = wpimath.PIDController(1, 0, 0) + + self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.kFlat) + + self.kinematics = wpimath.MecanumDriveKinematics( + self.frontLeftLocation, + self.frontRightLocation, + self.backLeftLocation, + self.backRightLocation, + ) + + self.odometry = wpimath.MecanumDriveOdometry( + self.kinematics, self.imu.getRotation2d(), self.getCurrentDistances() + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3) + + self.imu.resetYaw() + + # 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.frontRightMotor.setInverted(True) + self.backRightMotor.setInverted(True) + + def getCurrentState(self) -> wpimath.MecanumDriveWheelSpeeds: + """Returns the current state of the drivetrain.""" + return wpimath.MecanumDriveWheelSpeeds( + self.frontLeftEncoder.getRate(), + self.frontRightEncoder.getRate(), + self.backLeftEncoder.getRate(), + self.backRightEncoder.getRate(), + ) + + def getCurrentDistances(self) -> wpimath.MecanumDriveWheelPositions: + """Returns the current distances measured by the drivetrain.""" + positions = wpimath.MecanumDriveWheelPositions() + positions.frontLeft = self.frontLeftEncoder.getDistance() + positions.frontRight = self.frontRightEncoder.getDistance() + positions.rearLeft = self.backLeftEncoder.getDistance() + positions.rearRight = self.backRightEncoder.getDistance() + return positions + + def setSpeeds(self, speeds: wpimath.MecanumDriveWheelSpeeds) -> None: + """Sets the desired speeds for each wheel.""" + frontLeftFeedforward = self.feedforward.calculate(speeds.frontLeft) + frontRightFeedforward = self.feedforward.calculate(speeds.frontRight) + backLeftFeedforward = self.feedforward.calculate(speeds.rearLeft) + backRightFeedforward = self.feedforward.calculate(speeds.rearRight) + + frontLeftOutput = self.frontLeftPIDController.calculate( + self.frontLeftEncoder.getRate(), speeds.frontLeft + ) + frontRightOutput = self.frontRightPIDController.calculate( + self.frontRightEncoder.getRate(), speeds.frontRight + ) + backLeftOutput = self.frontLeftPIDController.calculate( + self.backLeftEncoder.getRate(), speeds.rearLeft + ) + backRightOutput = self.frontRightPIDController.calculate( + self.backRightEncoder.getRate(), speeds.rearRight + ) + + self.frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward) + self.frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward) + self.backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward) + self.backRightMotor.setVoltage(backRightOutput + backRightFeedforward) + + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + periodSeconds: float, + ) -> None: + """Method to drive the robot using joystick info.""" + chassisSpeeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot) + if fieldRelative: + chassisSpeeds = chassisSpeeds.toRobotRelative(self.imu.getRotation2d()) + + self.setSpeeds( + self.kinematics.toWheelSpeeds( + chassisSpeeds.discretize(periodSeconds) + ).desaturate(self.kMaxSpeed) + ) + + def updateOdometry(self) -> None: + """Updates the field-relative position.""" + self.odometry.update(self.imu.getRotation2d(), self.getCurrentDistances()) diff --git a/examples/robot/MecanumBot/robot.py b/examples/robot/MecanumBot/robot.py new file mode 100755 index 000000000..27b68e7cd --- /dev/null +++ b/examples/robot/MecanumBot/robot.py @@ -0,0 +1,58 @@ +#!/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 wpimath +import wpilib + +from drivetrain import Drivetrain + + +class MyRobot(wpilib.TimedRobot): + def __init__(self) -> None: + super().__init__() + + self.controller = wpilib.XboxController(0) + self.mecanum = Drivetrain() + + # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + self.xspeedLimiter = wpimath.SlewRateLimiter(3) + self.yspeedLimiter = wpimath.SlewRateLimiter(3) + self.rotLimiter = wpimath.SlewRateLimiter(3) + + def autonomousPeriodic(self) -> None: + self.driveWithJoystick(False) + self.mecanum.updateOdometry() + + def teleopPeriodic(self) -> None: + self.driveWithJoystick(True) + + def driveWithJoystick(self, fieldRelative: bool) -> None: + # Get the x speed. We are inverting this because Xbox controllers return + # negative values when we push forward. + xSpeed = ( + -self.xspeedLimiter.calculate(self.controller.getLeftY()) + * Drivetrain.kMaxSpeed + ) + + # Get the y speed or sideways/strafe speed. We are inverting this because + # we want a positive value when we pull to the left. Xbox controllers + # return positive values when you pull to the right by default. + ySpeed = ( + -self.yspeedLimiter.calculate(self.controller.getLeftX()) + * Drivetrain.kMaxSpeed + ) + + # Get the rate of angular rotation. We are inverting this because we want a + # positive value when we pull to the left (remember, CCW is positive in + # mathematics). Xbox controllers return positive values when you pull to + # the right by default. + rot = ( + -self.rotLimiter.calculate(self.controller.getRightX()) + * Drivetrain.kMaxAngularSpeed + ) + + self.mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) diff --git a/examples/robot/MecanumDrive/robot.py b/examples/robot/MecanumDrive/robot.py new file mode 100755 index 000000000..fb75395fd --- /dev/null +++ b/examples/robot/MecanumDrive/robot.py @@ -0,0 +1,51 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing how to use Mecanum control with the + MecanumDrive class. + """ + + # Channels on the roboRIO that the motor controllers are plugged in to + kFrontLeftChannel = 2 + kRearLeftChannel = 3 + kFrontRightChannel = 1 + kRearRightChannel = 0 + + # The channel on the driver station that the joystick is connected to + kJoystickChannel = 0 + + def __init__(self): + super().__init__() + self.frontLeft = wpilib.PWMSparkMax(self.kFrontLeftChannel) + self.rearLeft = wpilib.PWMSparkMax(self.kRearLeftChannel) + self.frontRight = wpilib.PWMSparkMax(self.kFrontRightChannel) + self.rearRight = wpilib.PWMSparkMax(self.kRearRightChannel) + + # invert the right side motors + # you may need to change or remove this to match your robot + self.frontRight.setInverted(True) + self.rearRight.setInverted(True) + + self.robotDrive = wpilib.MecanumDrive( + self.frontLeft, self.rearLeft, self.frontRight, self.rearRight + ) + + self.stick = wpilib.Joystick(self.kJoystickChannel) + + def teleopPeriodic(self): + # Use the joystick X axis for lateral movement, Y axis for forward + # movement, and Z axis for rotation. + self.robotDrive.driveCartesian( + -self.stick.getY(), + -self.stick.getX(), + -self.stick.getZ(), + ) diff --git a/examples/robot/Mechanism2d/robot.py b/examples/robot/Mechanism2d/robot.py new file mode 100755 index 000000000..76f46566f --- /dev/null +++ b/examples/robot/Mechanism2d/robot.py @@ -0,0 +1,60 @@ +#!/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 wpilib +import wpiutil + + +class MyRobot(wpilib.TimedRobot): + """ + This sample program shows how to use Mechanism2d - a visual representation of arms, elevators, + and other mechanisms on dashboards; driven by a node-based API. + + Ligaments are based on other ligaments or roots, and roots are contained in the base + Mechanism2d object. + """ + + kMetersPerPulse = 0.01 + kElevatorMinimumLength = 0.5 + + def __init__(self): + super().__init__() + self.elevatorMotor = wpilib.PWMSparkMax(0) + self.wristMotor = wpilib.PWMSparkMax(1) + self.wristPot = wpilib.AnalogPotentiometer(1, 90) + self.elevatorEncoder = wpilib.Encoder(0, 1) + self.joystick = wpilib.Joystick(0) + + self.elevatorEncoder.setDistancePerPulse(self.kMetersPerPulse) + + # the main mechanism object + self.mech = wpilib.Mechanism2d(3, 3) + # the mechanism root node + self.root = self.mech.getRoot("climber", 2, 0) + + # MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based + # off the root node or another ligament object + self.elevator = self.root.appendLigament( + "elevator", self.kElevatorMinimumLength, 90 + ) + self.wrist = self.elevator.appendLigament( + "wrist", 0.5, 90, 6, wpiutil.Color8Bit(wpiutil.Color.kPurple) + ) + + # post the mechanism to the dashboard + wpilib.SmartDashboard.putData("Mech2d", self.mech) + + def robotPeriodic(self): + # update the dashboard mechanism's state + self.elevator.setLength( + self.kElevatorMinimumLength + self.elevatorEncoder.getDistance() + ) + self.wrist.setAngle(self.wristPot.get()) + + def teleopPeriodic(self): + self.elevatorMotor.set(self.joystick.getRawAxis(0)) + self.wristMotor.set(self.joystick.getRawAxis(1)) diff --git a/examples/robot/MotorControl/robot.py b/examples/robot/MotorControl/robot.py new file mode 100755 index 000000000..9b63940d5 --- /dev/null +++ b/examples/robot/MotorControl/robot.py @@ -0,0 +1,46 @@ +#!/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 wpilib + +import math + + +class MyRobot(wpilib.TimedRobot): + """ + This sample program shows how to control a motor using a joystick. In the operator control part + of the program, the joystick is read and the value is written to the motor. + + Joystick analog values range from -1 to 1 and motor controller inputs also range from -1 to 1 + making it easy to work together. + + In addition, the encoder value of an encoder connected to ports 0 and 1 is consistently sent + to the Dashboard. + """ + + kMotorPort = 0 + kJoystickPort = 0 + kEncoderPortA = 0 + kEncoderPortB = 1 + + def __init__(self): + """Robot initialization function""" + super().__init__() + + self.motor = wpilib.PWMSparkMax(self.kMotorPort) + self.joystick = wpilib.Joystick(self.kJoystickPort) + self.encoder = wpilib.Encoder(self.kEncoderPortA, self.kEncoderPortB) + # Use SetDistancePerPulse to set the multiplier for GetDistance + # This is set up assuming a 6 inch wheel with a 360 CPR encoder. + self.encoder.setDistancePerPulse((math.pi * 6) / 360.0) + + def robotPeriodic(self): + """The RobotPeriodic function is called every control packet no matter the robot mode.""" + wpilib.SmartDashboard.putNumber("Encoder", self.encoder.getDistance()) + + def teleopPeriodic(self): + self.motor.set(self.joystick.getY()) diff --git a/examples/robot/PotentiometerPID/robot.py b/examples/robot/PotentiometerPID/robot.py new file mode 100644 index 000000000..bf3ec4b11 --- /dev/null +++ b/examples/robot/PotentiometerPID/robot.py @@ -0,0 +1,66 @@ +#!/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 wpilib +import wpimath + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program to demonstrate how to use a soft potentiometer and a PID controller to + reach and maintain position setpoints on an elevator mechanism. + """ + + kPotChannel = 1 + kMotorChannel = 7 + kJoystickChannel = 3 + + # The elevator can move 1.5 meters from top to bottom + kFullHeightMeters = 1.5 + + # Bottom, middle, and top elevator setpoints + kSetpointMeters = [0.2, 0.8, 1.4] + + # proportional, integral, and derivative speed constants + # DANGER: when tuning PID constants, high/inappropriate values for kP, kI, + # and kD may cause dangerous, uncontrollable, or undesired behavior! + kP = 0.7 + kI = 0.35 + kD = 0.25 + + def __init__(self): + super().__init__() + + self.pidController = wpimath.PIDController(self.kP, self.kI, self.kD) + # Scaling is handled internally + self.potentiometer = wpilib.AnalogPotentiometer( + self.kPotChannel, self.kFullHeightMeters + ) + self.elevatorMotor = wpilib.PWMSparkMax(self.kMotorChannel) + self.joystick = wpilib.Joystick(self.kJoystickChannel) + + def teleopInit(self): + # Move to the bottom setpoint when teleop starts + self.index = 0 + self.pidController.setSetpoint(self.kSetpointMeters[self.index]) + + def teleopPeriodic(self): + # Read from the sensor + position = self.potentiometer.get() + + # Run the PID Controller + pidOut = self.pidController.calculate(position) + + # Apply PID output + self.elevatorMotor.set(pidOut) + + # when the button is pressed once, the selected elevator setpoint is incremented + if self.joystick.getTriggerPressed(): + # index of the elevator setpoint wraps around. + self.index = (self.index + 1) % len(self.kSetpointMeters) + print(f"index = {self.index}") + self.pidController.setSetpoint(self.kSetpointMeters[self.index]) diff --git a/examples/robot/QuickVision/robot.py b/examples/robot/QuickVision/robot.py new file mode 100644 index 000000000..00d05968f --- /dev/null +++ b/examples/robot/QuickVision/robot.py @@ -0,0 +1,21 @@ +#!/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 wpilib +from wpilib.cameraserver import CameraServer + + +class MyRobot(wpilib.TimedRobot): + """ + Uses the CameraServer class to automatically capture video from a USB webcam and send it to the + FRC dashboard without doing any vision processing. This is the easiest way to get camera images + to the dashboard. Just add this to the robotInit() method in your program. + """ + + def __init__(self): + super().__init__() + CameraServer().launch() diff --git a/examples/robot/RomiReference/commands/arcadedrive.py b/examples/robot/RomiReference/commands/arcadedrive.py new file mode 100644 index 000000000..76f7b3945 --- /dev/null +++ b/examples/robot/RomiReference/commands/arcadedrive.py @@ -0,0 +1,34 @@ +# +# 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 commands2 +from subsystems.drivetrain import Drivetrain + + +class ArcadeDrive(commands2.Command): + def __init__( + self, + drive: Drivetrain, + forward: typing.Callable[[], float], + rotation: typing.Callable[[], float], + ) -> None: + """Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier + lambdas. This command does not terminate. + + :param drivetrain: The drivetrain subsystem on which this command will run + :param forward: Callable supplier of forward/backward speed + :param rotation: Callable supplier of rotational speed + """ + + self.drive = drive + self.forward = forward + self.rotation = rotation + + self.addRequirements(self.drive) + + def execute(self) -> None: + self.drive.arcadeDrive(self.forward(), self.rotation()) diff --git a/examples/robot/RomiReference/commands/autonomousdistance.py b/examples/robot/RomiReference/commands/autonomousdistance.py new file mode 100644 index 000000000..ad84366e2 --- /dev/null +++ b/examples/robot/RomiReference/commands/autonomousdistance.py @@ -0,0 +1,28 @@ +# +# 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 + +from commands.drivedistance import DriveDistance +from commands.turndegrees import TurnDegrees +from subsystems.drivetrain import Drivetrain + + +class AutonomousDistance(commands2.SequentialCommandGroup): + def __init__(self, drive: Drivetrain) -> None: + """Creates a new Autonomous Drive based on distance. This will drive out for a specified distance, + turn around and drive back. + + :param drivetrain: The drivetrain subsystem on which this command will run + """ + super().__init__() + + self.addCommands( + DriveDistance(-0.5, 10, drive), + TurnDegrees(-0.5, 180, drive), + DriveDistance(-0.5, 10, drive), + TurnDegrees(0.5, 180, drive), + ) diff --git a/examples/robot/RomiReference/commands/autonomoustime.py b/examples/robot/RomiReference/commands/autonomoustime.py new file mode 100644 index 000000000..1a48cf841 --- /dev/null +++ b/examples/robot/RomiReference/commands/autonomoustime.py @@ -0,0 +1,30 @@ +# +# 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 + +from commands.drivetime import DriveTime +from commands.turntime import TurnTime +from subsystems.drivetrain import Drivetrain + + +class AutonomousTime(commands2.SequentialCommandGroup): + def __init__(self, drive: Drivetrain) -> None: + """Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn + around for time (equivalent to time to turn around) and drive forward again. This should mimic + driving out, turning around and driving back. + + :param drivetrain: The drive subsystem on which this command will run + + """ + super().__init__() + + self.addCommands( + DriveTime(-0.6, 2.0, drive), + TurnTime(-0.5, 1.3, drive), + DriveTime(-0.6, 2.0, drive), + TurnTime(0.5, 1.3, drive), + ) diff --git a/examples/robot/RomiReference/commands/drivedistance.py b/examples/robot/RomiReference/commands/drivedistance.py new file mode 100644 index 000000000..41b0700d9 --- /dev/null +++ b/examples/robot/RomiReference/commands/drivedistance.py @@ -0,0 +1,43 @@ +# +# 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 + +from subsystems.drivetrain import Drivetrain + + +class DriveDistance(commands2.Command): + def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None: + """Creates a new DriveDistance. This command will drive your your robot for a desired distance at + a desired speed. + + :param speed: The speed at which the robot will drive + :param inches: The number of inches the robot will drive + :param drive: The drivetrain subsystem on which this command will run + """ + + self.distance = inches + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.drive.arcadeDrive(0, 0) + self.drive.resetEncoders() + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end.""" + # Compare distance travelled from start to desired distance + return abs(self.drive.getAverageDistanceInch()) >= self.distance diff --git a/examples/robot/RomiReference/commands/drivetime.py b/examples/robot/RomiReference/commands/drivetime.py new file mode 100644 index 000000000..bac0a840c --- /dev/null +++ b/examples/robot/RomiReference/commands/drivetime.py @@ -0,0 +1,46 @@ +# +# 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 wpilib + +from subsystems.drivetrain import Drivetrain + + +class DriveTime(commands2.Command): + """Creates a new DriveTime. This command will drive your robot for a desired speed and time.""" + + def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: + """Creates a new DriveTime. This command will drive your robot for a desired speed and time. + + :param speed: The speed which the robot will drive. Negative is in reverse. + :param time: How much time to drive in seconds + :param drive: The drivetrain subsystem on which this command will run + """ + + self.speed = speed + self.duration = time + self.drive = drive + self.addRequirements(drive) + + self.startTime = 0.0 + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.startTime = wpilib.Timer.getFPGATimestamp() + self.drive.arcadeDrive(0, 0) + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end""" + return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration diff --git a/examples/robot/RomiReference/commands/turndegrees.py b/examples/robot/RomiReference/commands/turndegrees.py new file mode 100644 index 000000000..8584bd12c --- /dev/null +++ b/examples/robot/RomiReference/commands/turndegrees.py @@ -0,0 +1,57 @@ +# +# 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 math +import commands2 + +from subsystems.drivetrain import Drivetrain + + +class TurnDegrees(commands2.Command): + def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None: + """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in + degrees) and rotational speed. + + :param speed: The speed which the robot will drive. Negative is in reverse. + :param degrees: Degrees to turn. Leverages encoders to compare distance. + :param drive: The drive subsystem on which this command will run + """ + + self.degrees = degrees + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + # Set motors to stop, read encoder values for starting point + self.drive.arcadeDrive(0, 0) + self.drive.resetEncoders() + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(0, self.speed) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end.""" + + # Need to convert distance travelled to degrees. The Standard + # Romi Chassis found here, https://www.pololu.com/category/203/romi-chassis-kits, + # has a wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm + # or 5.551 inches. We then take into consideration the width of the tires. + inchPerDegree = math.pi * 5.551 / 360.0 + + # Compare distance travelled from start to distance based on degree turn + return self._getAverageTurningDistance() >= inchPerDegree * self.degrees + + def _getAverageTurningDistance(self) -> float: + leftDistance = abs(self.drive.getLeftDistanceInch()) + rightDistance = abs(self.drive.getRightDistanceInch()) + return (leftDistance + rightDistance) / 2.0 diff --git a/examples/robot/RomiReference/commands/turntime.py b/examples/robot/RomiReference/commands/turntime.py new file mode 100644 index 000000000..f806c8b7e --- /dev/null +++ b/examples/robot/RomiReference/commands/turntime.py @@ -0,0 +1,48 @@ +# +# 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 wpilib + +from subsystems.drivetrain import Drivetrain + + +class TurnTime(commands2.Command): + """Creates a new TurnTime command. This command will turn your robot for a + desired rotational speed and time. + """ + + def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: + """Creates a new TurnTime. + + :param speed: The speed which the robot will turn. Negative is in reverse. + :param time: How much time to turn in seconds + :param drive: The drive subsystem on which this command will run + """ + + self.rotationalSpeed = speed + self.duration = time + self.drive = drive + self.addRequirements(drive) + + self.startTime = 0.0 + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.startTime = wpilib.Timer.getFPGATimestamp() + self.drive.arcadeDrive(0, 0) + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(0, self.rotationalSpeed) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end""" + return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration diff --git a/examples/robot/RomiReference/robot.py b/examples/robot/RomiReference/robot.py new file mode 100644 index 000000000..dcc48e1b9 --- /dev/null +++ b/examples/robot/RomiReference/robot.py @@ -0,0 +1,97 @@ +#!/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. +# + +# +# Example that shows how to connect to a ROMI from RobotPy +# +# Requirements +# ------------ +# +# You must have the robotpy-halsim-ws package installed. This is best done via: +# +# # Windows +# py -3 -m pip install robotpy[commands2,sim] +# +# # Linux/macOS +# pip3 install robotpy[commands2,sim] +# +# Run the program +# --------------- +# +# Use the dedicated ROMI command: +# +# # Windows +# py -3 -m robotpy run-romi +# +# # Linux/macOS +# python -m robotpy run-romi +# +# If your ROMI isn't at the default address, use --host/--port: +# +# python -m robotpy run-romi --host 10.0.0.2 --port 3300 +# +# By default the WPILib simulation GUI will be displayed. To disable the display +# you can add the --nogui option. +# + +import typing + +import wpilib +import commands2 + +from robotcontainer import RobotContainer + + +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 + """ + + autonomousCommand: typing.Optional[commands2.Command] = None + + 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__() + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = 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() + + if self.autonomousCommand: + self.autonomousCommand.schedule() + + 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: + self.autonomousCommand.cancel() + + def teleopPeriodic(self) -> None: + """This function is called periodically during operator control""" + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode + commands2.CommandScheduler.getInstance().cancelAll() diff --git a/examples/robot/RomiReference/robotcontainer.py b/examples/robot/RomiReference/robotcontainer.py new file mode 100644 index 000000000..de08384c4 --- /dev/null +++ b/examples/robot/RomiReference/robotcontainer.py @@ -0,0 +1,90 @@ +# +# 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 commands2 +import commands2.button +import wpilib +import romi + +from commands.arcadedrive import ArcadeDrive +from commands.autonomousdistance import AutonomousDistance +from commands.autonomoustime import AutonomousTime + +from subsystems.drivetrain import Drivetrain + + +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 = romi.OnBoardIO( + romi.OnBoardIO.ChannelMode.INPUT, romi.OnBoardIO.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.Trigger(self.onboardIO.getButtonAPressed) + onboardButtonA.onTrue(commands2.PrintCommand("Button A Pressed")).onFalse( + 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.Command]: + 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), + ) diff --git a/examples/robot/RomiReference/subsystems/drivetrain.py b/examples/robot/RomiReference/subsystems/drivetrain.py new file mode 100644 index 000000000..32c371cd5 --- /dev/null +++ b/examples/robot/RomiReference/subsystems/drivetrain.py @@ -0,0 +1,103 @@ +# +# 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 math + +import commands2 +import wpilib +import romi + + +class Drivetrain(commands2.Subsystem): + kCountsPerRevolution = 1440.0 + kWheelDiameterInch = 2.75591 + + def __init__(self) -> None: + + # The Romi has the left and right motors set to + # PWM channels 0 and 1 respectively + self.leftMotor = wpilib.Spark(0) + self.rightMotor = wpilib.Spark(1) + + # 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.rightMotor.setInverted(True) + + # The Romi has onboard encoders that are hardcoded + # to use DIO pins 4/5 and 6/7 for the left and right + self.leftEncoder = wpilib.Encoder(4, 5) + self.rightEncoder = wpilib.Encoder(6, 7) + + # Set up the differential drive controller + self.drive = wpilib.DifferentialDrive(self.leftMotor, self.rightMotor) + + # Set up the RomiGyro + self.gyro = romi.RomiGyro() + + # Use inches as unit for encoder distances + self.leftEncoder.setDistancePerPulse( + (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution + ) + self.rightEncoder.setDistancePerPulse( + (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution + ) + self.resetEncoders() + + def arcadeDrive(self, fwd: float, rot: float) -> None: + """ + Drives the robot using arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + self.drive.arcadeDrive(fwd, rot) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + self.leftEncoder.reset() + self.rightEncoder.reset() + + def getLeftEncoderCount(self) -> int: + return self.leftEncoder.get() + + def getRightEncoderCount(self) -> int: + return self.rightEncoder.get() + + def getLeftDistanceInch(self) -> float: + return self.leftEncoder.getDistance() + + def getRightDistanceInch(self) -> float: + return self.rightEncoder.getDistance() + + def getAverageDistanceInch(self) -> float: + """Gets the average distance of the TWO encoders.""" + return (self.getLeftDistanceInch() + self.getRightDistanceInch()) / 2.0 + + def getGyroAngleX(self) -> float: + """Current angle of the Romi around the X-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleX() + + def getGyroAngleY(self) -> float: + """Current angle of the Romi around the Y-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleY() + + def getGyroAngleZ(self) -> float: + """Current angle of the Romi around the Z-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleZ() + + def resetGyro(self) -> None: + """Reset the gyro""" + self.gyro.reset() diff --git a/examples/robot/SelectCommand/constants.py b/examples/robot/SelectCommand/constants.py new file mode 100644 index 000000000..add621e0a --- /dev/null +++ b/examples/robot/SelectCommand/constants.py @@ -0,0 +1,10 @@ +# +# 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. +# + + +class OIConstants: + # Example: the port of the driver's controller + kDriverControllerPort = 0 diff --git a/examples/robot/SelectCommand/robot.py b/examples/robot/SelectCommand/robot.py new file mode 100644 index 000000000..b65fda8cf --- /dev/null +++ b/examples/robot/SelectCommand/robot.py @@ -0,0 +1,67 @@ +#!/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 wpilib +import commands2 +import typing + +from robotcontainer import RobotContainer + + +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 + """ + + autonomousCommand: typing.Optional[commands2.Command] = None + + 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__() + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = RobotContainer() + + def disabledInit(self) -> None: + """This function is called once each time the robot enters Disabled mode.""" + pass + + def disabledPeriodic(self) -> None: + """This function is called periodically when disabled""" + pass + + def autonomousInit(self) -> None: + """This autonomous runs the autonomous command selected by your RobotContainer class.""" + self.autonomousCommand = self.container.getAutonomousCommand() + + if self.autonomousCommand: + self.autonomousCommand.schedule() + + def autonomousPeriodic(self) -> None: + """This function is called periodically during autonomous""" + pass + + 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: + self.autonomousCommand.cancel() + + def teleopPeriodic(self) -> None: + """This function is called periodically during operator control""" + pass + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode + commands2.CommandScheduler.getInstance().cancelAll() diff --git a/examples/robot/SelectCommand/robotcontainer.py b/examples/robot/SelectCommand/robotcontainer.py new file mode 100644 index 000000000..5b7dda46b --- /dev/null +++ b/examples/robot/SelectCommand/robotcontainer.py @@ -0,0 +1,67 @@ +# +# 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 enum +import commands2 + + +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. + """ + + # The enum used as keys for selecting the command to run. + class CommandSelector(enum.Enum): + ONE = enum.auto() + TWO = enum.auto() + THREE = enum.auto() + + # An example selector method for the selectcommand. + def select(self) -> CommandSelector: + """Returns the selector that will select which command to run. + Can base this choice on logical conditions evaluated at runtime. + """ + return self.CommandSelector.ONE + + def __init__(self) -> None: + # An example selectcommand. Will select from the three commands based on the value returned + # by the selector method at runtime. Note that selectcommand takes a generic type, so the + # selector does not have to be an enum; it could be any desired type (string, integer, + # boolean, double...) + self.example_select_command = commands2.SelectCommand( + # Maps selector values to commands + { + self.CommandSelector.ONE: commands2.PrintCommand( + "Command one was selected!" + ), + self.CommandSelector.TWO: commands2.PrintCommand( + "Command two was selected!" + ), + self.CommandSelector.THREE: commands2.PrintCommand( + "Command three was selected!" + ), + }, + self.select, + ) + + # Configure the button bindings + self.configureButtonBindings() + + def configureButtonBindings(self) -> None: + """Use this method to define your button->command mappings. Buttons can be created by + instantiating a {GenericHID} or one of its subclasses + ({edu.wpi.first.wpilibj.Joystick} or {XboxController}), and then calling passing it to a + {edu.wpi.first.wpilibj2.command.button.JoystickButton}. + """ + + def getAutonomousCommand(self) -> commands2.Command: + """Use this to pass the autonomous command to the main {Robot} class. + + :returns: the command to run in autonomous + """ + return self.example_select_command diff --git a/examples/robot/SimpleDifferentialDriveSimulation/drivetrain.py b/examples/robot/SimpleDifferentialDriveSimulation/drivetrain.py new file mode 100644 index 000000000..d41657069 --- /dev/null +++ b/examples/robot/SimpleDifferentialDriveSimulation/drivetrain.py @@ -0,0 +1,158 @@ +# +# 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 math + +import wpilib +import wpilib.simulation +import wpimath + + +class Drivetrain: + """Represents a differential drive style drivetrain.""" + + # 3 meters per second. + kMaxSpeed = 3.0 + # 1/2 rotation per second. + kMaxAngularSpeed = math.pi + + kTrackwidth = 0.381 * 2 + kWheelRadius = 0.0508 + kEncoderResolution = -4096 + + def __init__(self) -> None: + self.leftLeader = wpilib.PWMSparkMax(1) + self.leftFollower = wpilib.PWMSparkMax(2) + self.rightLeader = wpilib.PWMSparkMax(3) + self.rightFollower = wpilib.PWMSparkMax(4) + + self.leftEncoder = wpilib.Encoder(0, 1) + self.rightEncoder = wpilib.Encoder(2, 3) + + self.leftPIDController = wpimath.PIDController(8.5, 0, 0) + self.rightPIDController = wpimath.PIDController(8.5, 0, 0) + + self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.kFlat) + + self.kinematics = wpimath.DifferentialDriveKinematics(self.kTrackwidth) + self.odometry = wpimath.DifferentialDriveOdometry( + self.imu.getRotation2d(), + self.leftEncoder.getDistance(), + self.rightEncoder.getDistance(), + ) + + # Gains are for example purposes only - must be determined for your own + # robot! + self.feedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3) + + # Simulation classes help us simulate our robot + self.leftEncoderSim = wpilib.simulation.EncoderSim(self.leftEncoder) + self.rightEncoderSim = wpilib.simulation.EncoderSim(self.rightEncoder) + self.fieldSim = wpilib.Field2d() + self.drivetrainSystem = wpimath.LinearSystemId.identifyDrivetrainSystem( + 1.98, 0.2, 1.5, 0.3 + ) + self.drivetrainSimulator = wpilib.simulation.DifferentialDrivetrainSim( + self.drivetrainSystem, + self.kTrackwidth, + wpimath.DCMotor.CIM(2), + 8, + self.kWheelRadius, + ) + + # Subsystem constructor. + self.leftLeader.addFollower(self.leftFollower) + self.rightLeader.addFollower(self.rightFollower) + + # 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) + + # Set the distance per pulse for the drive encoders. We can simply use the + # distance traveled for one rotation of the wheel divided by the encoder + # resolution. + self.leftEncoder.setDistancePerPulse( + 2 * math.pi * self.kWheelRadius / self.kEncoderResolution + ) + self.rightEncoder.setDistancePerPulse( + 2 * math.pi * self.kWheelRadius / self.kEncoderResolution + ) + + self.leftEncoder.reset() + self.rightEncoder.reset() + + self.rightLeader.setInverted(True) + wpilib.SmartDashboard.putData("Field", self.fieldSim) + + def setSpeeds(self, speeds: wpimath.DifferentialDriveWheelSpeeds) -> None: + """Sets speeds to the drivetrain motors.""" + leftFeedforward = self.feedforward.calculate(speeds.left) + rightFeedforward = self.feedforward.calculate(speeds.right) + leftOutput = self.leftPIDController.calculate( + self.leftEncoder.getRate(), speeds.left + ) + rightOutput = self.rightPIDController.calculate( + self.rightEncoder.getRate(), speeds.right + ) + + self.leftLeader.setVoltage(leftOutput + leftFeedforward) + self.rightLeader.setVoltage(rightOutput + rightFeedforward) + + def drive(self, xSpeed: float, rot: float) -> None: + """Controls the robot using arcade drive. + + :param xSpeed: the speed for the x axis + :param rot: the rotation + """ + self.setSpeeds( + self.kinematics.toWheelSpeeds(wpimath.ChassisSpeeds(xSpeed, 0, rot)) + ) + + def updateOdometry(self) -> None: + """Update robot odometry.""" + self.odometry.update( + self.imu.getRotation2d(), + self.leftEncoder.getDistance(), + self.rightEncoder.getDistance(), + ) + + def resetOdometry(self, pose: wpimath.Pose2d) -> None: + """Resets robot odometry.""" + self.drivetrainSimulator.setPose(pose) + self.odometry.resetPosition( + self.imu.getRotation2d(), + self.leftEncoder.getDistance(), + self.rightEncoder.getDistance(), + pose, + ) + + def getPose(self) -> wpimath.Pose2d: + """Check the current robot pose.""" + return self.odometry.getPose() + + def simulationPeriodic(self) -> None: + """Update our simulation. This should be run every robot loop in simulation.""" + # To update our simulation, we set motor voltage inputs, update the + # simulation, and write the simulated positions and velocities to our + # simulated encoder and gyro. We negate the right side so that positive + # voltages make the right side move forward. + self.drivetrainSimulator.setInputs( + self.leftLeader.get() * wpilib.RobotController.getInputVoltage(), + self.rightLeader.get() * wpilib.RobotController.getInputVoltage(), + ) + self.drivetrainSimulator.update(0.02) + + self.leftEncoderSim.setDistance(self.drivetrainSimulator.getLeftPosition()) + self.leftEncoderSim.setRate(self.drivetrainSimulator.getLeftVelocity()) + self.rightEncoderSim.setDistance(self.drivetrainSimulator.getRightPosition()) + self.rightEncoderSim.setRate(self.drivetrainSimulator.getRightVelocity()) + # self.gyroSim.setAngle(-self.drivetrainSimulator.getHeading().getDegrees()) + + def periodic(self) -> None: + """Update odometry - this should be run every robot loop.""" + self.updateOdometry() + self.fieldSim.setRobotPose(self.odometry.getPose()) diff --git a/examples/robot/SimpleDifferentialDriveSimulation/robot.py b/examples/robot/SimpleDifferentialDriveSimulation/robot.py new file mode 100644 index 000000000..c2331e0ea --- /dev/null +++ b/examples/robot/SimpleDifferentialDriveSimulation/robot.py @@ -0,0 +1,68 @@ +#!/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 wpilib +import wpimath + +from drivetrain import Drivetrain + + +class MyRobot(wpilib.TimedRobot): + def __init__(self) -> None: + super().__init__() + self.controller = wpilib.XboxController(0) + + # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 + # to 1. + self.speedLimiter = wpimath.SlewRateLimiter(3) + self.rotLimiter = wpimath.SlewRateLimiter(3) + + self.drive = Drivetrain() + self.feedback = wpimath.LTVUnicycleController(0.020) + self.timer = wpilib.Timer() + + # Called once at the beginning of the robot program. + self.trajectory = wpimath.TrajectoryGenerator.generateTrajectory( + wpimath.Pose2d(2, 2, wpimath.Rotation2d()), + [], + wpimath.Pose2d(6, 4, wpimath.Rotation2d()), + wpimath.TrajectoryConfig(2, 2), + ) + + def robotPeriodic(self) -> None: + self.drive.periodic() + + def autonomousInit(self) -> None: + self.timer.restart() + self.drive.resetOdometry(self.trajectory.initialPose()) + + def autonomousPeriodic(self) -> None: + elapsed = self.timer.get() + reference = self.trajectory.sample(elapsed) + speeds = self.feedback.calculate(self.drive.getPose(), reference) + self.drive.drive(speeds.vx, speeds.omega) + + def teleopPeriodic(self) -> None: + # Get the x speed. We are inverting this because Xbox controllers return + # negative values when we push forward. + xSpeed = ( + -self.speedLimiter.calculate(self.controller.getLeftY()) + * Drivetrain.kMaxSpeed + ) + + # Get the rate of angular rotation. We are inverting this because we want a + # positive value when we pull to the left (remember, CCW is positive in + # mathematics). Xbox controllers return positive values when you pull to + # the right by default. + rot = ( + -self.rotLimiter.calculate(self.controller.getRightX()) + * Drivetrain.kMaxAngularSpeed + ) + self.drive.drive(xSpeed, rot) + + def simulationPeriodic(self) -> None: + self.drive.simulationPeriodic() diff --git a/examples/robot/Solenoid/robot.py b/examples/robot/Solenoid/robot.py new file mode 100644 index 000000000..45312b59d --- /dev/null +++ b/examples/robot/Solenoid/robot.py @@ -0,0 +1,122 @@ +#!/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 wpilib + +kSolenoidButton = 1 +kDoubleSolenoidForwardButton = 2 +kDoubleSolenoidReverseButton = 3 +kCompressorButton = 4 + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program showing the use of the solenoid classes during operator control. Three + buttons from a joystick will be used to control two solenoids: One button to control the position + of a single solenoid and the other two buttons to control a double solenoid. Single solenoids can + either be on or off, such that the air diverted through them goes through either one channel or + the other. Double solenoids have three states: Off, Forward, and Reverse. Forward and Reverse + divert the air through the two channels and correspond to the on and off of a single solenoid, + but a double solenoid can also be "off", where the solenoid will remain in its default power off + state. Additionally, double solenoids take up two channels on your PCM whereas single solenoids + only take a single channel. + """ + + def __init__(self) -> None: + super().__init__() + self.joystick = wpilib.Joystick(0) + + # Solenoid corresponds to a single solenoid. + # In this case, it's connected to channel 0 of a PH with the default CAN ID. + self.solenoid = wpilib.Solenoid( + busId=0, moduleType=wpilib.PneumaticsModuleType.REVPH, channel=0 + ) + + # DoubleSolenoid corresponds to a double solenoid. + # In this case, it's connected to channels 1 and 2 of a PH with the default CAN ID. + self.doubleSolenoid = wpilib.DoubleSolenoid( + busId=0, + moduleType=wpilib.PneumaticsModuleType.REVPH, + forwardChannel=1, + reverseChannel=2, + ) + + # Compressor connected to a PH with a default CAN ID (1) + self.compressor = wpilib.Compressor( + busId=0, moduleType=wpilib.PneumaticsModuleType.REVPH + ) + + # Publish elements to dashboard. + wpilib.SmartDashboard.putData("Single Solenoid", self.solenoid) + wpilib.SmartDashboard.putData("Double Solenoid", self.doubleSolenoid) + wpilib.SmartDashboard.putData("Compressor", self.compressor) + + def teleopPeriodic(self) -> None: + # Publish some raw data + # Get the pressure (in PSI) from the analog sensor connected to the PH. + # This function is supported only on the PH! + # On a PCM, this function will return 0. + wpilib.SmartDashboard.putNumber( + "PH Pressure [PSI]", self.compressor.getPressure() + ) + # Get compressor current draw. + wpilib.SmartDashboard.putNumber( + "Compressor Current", self.compressor.getCurrent() + ) + # Get whether the compressor is active. + wpilib.SmartDashboard.putBoolean( + "Compressor Active", self.compressor.isEnabled() + ) + # Get the digital pressure switch connected to the PCM/PH. + # The switch is open when the pressure is over ~120 PSI. + wpilib.SmartDashboard.putBoolean( + "Pressure Switch", self.compressor.getPressureSwitchValue() + ) + + # The output of GetRawButton is true/false depending on whether + # the button is pressed; Set takes a boolean for whether + # to retract the solenoid (false) or extend it (true). + self.solenoid.set(self.joystick.getRawButton(kSolenoidButton)) + + # GetRawButtonPressed will only return true once per press. + # If a button is pressed, set the solenoid to the respective channel. + + if self.joystick.getRawButtonPressed(kDoubleSolenoidForwardButton): + self.doubleSolenoid.set(wpilib.DoubleSolenoid.Value.kForward) + elif self.joystick.getRawButtonPressed(kDoubleSolenoidReverseButton): + self.doubleSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse) + + # On button press, toggle the compressor. + if self.joystick.getRawButtonPressed(kCompressorButton): + # Check whether the compressor is currently enabled. + isCompressorEnabled = self.compressor.isEnabled() + if isCompressorEnabled: + # Disable closed-loop mode on the compressor + self.compressor.disable() + else: + # Change the if statements to select the closed-loop you want to use: + + if False: + # Enable closed-loop mode based on the digital pressure switch connected to the PCM/PH. + # The switch is open when the pressure is over ~120 PSI. + self.compressor.enableDigital() + + if True: + # Enable closed-loop mode based on the analog pressure sensor connected to the PH. + # The compressor will run while the pressure reported by the sensor is in the + # specified range ([70 PSI, 120 PSI] in this example). + # Analog mode exists only on the PH! On the PCM, this enables digital control. + self.compressor.enableAnalog(70, 120) + + if False: + # Enable closed-loop mode based on both the digital pressure switch AND the analog + # pressure sensor connected to the PH. + # The compressor will run while the pressure reported by the analog sensor is in the + # specified range ([70 PSI, 120 PSI] in this example) AND the digital switch reports + # that the system is not full. + # Hybrid mode exists only on the PH! On the PCM, this enables digital control. + self.compressor.enableHybrid(70, 120) diff --git a/examples/robot/StateSpaceArm/robot.py b/examples/robot/StateSpaceArm/robot.py new file mode 100644 index 000000000..9625e73db --- /dev/null +++ b/examples/robot/StateSpaceArm/robot.py @@ -0,0 +1,144 @@ +#!/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 math +import wpilib +import wpimath.units +import wpimath + +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 +kRaisedPosition = wpimath.units.degreesToRadians(90.0) +kLoweredPosition = wpimath.units.degreesToRadians(0.0) + +# Moment of inertia of the arm, in kg * m^2. Can be estimated with CAD. If finding this constant +# is difficult, LinearSystem.identifyPositionSystem may be better. +kArmMOI = 1.2 + +# Reduction between motors and encoder, as output over input. If the arm spins slower than +# the motors, this number should be greater than one. +kArmGearing = 10.0 + + +class MyRobot(wpilib.TimedRobot): + """This is a sample program to demonstrate how to use a state-space controller to control an arm.""" + + def __init__(self) -> None: + super().__init__() + + self.profile = wpimath.TrapezoidProfile( + wpimath.TrapezoidProfile.Constraints( + wpimath.units.degreesToRadians(45), + wpimath.units.degreesToRadians(90), # Max arm speed and acceleration. + ) + ) + + self.lastProfiledReference = wpimath.TrapezoidProfile.State() + + # The plant holds a state-space model of our arm. This system has the following properties: + # + # States: [position, velocity], in radians and radians per second. + # Inputs (what we can "put in"): [voltage], in volts. + # Outputs (what we can measure): [position], in radians. + self.armPlant = wpimath.LinearSystemId.singleJointedArmSystem( + wpimath.DCMotor.NEO(2), + kArmMOI, + kArmGearing, + ).slice(0) + + # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.KalmanFilter_2_1_1( + self.armPlant, + # How accurate we think our model is, in radians and radians/sec. + ( + 0.015, + 0.17, + ), + # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading. + (0.01,), + 0.020, + ) + + # A LQR uses feedback to create voltage commands. + self.controller = wpimath.LinearQuadraticRegulator_2_1( + self.armPlant, + # qelms. Velocity error tolerance, in radians and radians per second. + # Decrease this to more heavily penalize state excursion, or make the + # controller behave more aggressively. + ( + wpimath.units.degreesToRadians(1.0), + wpimath.units.degreesToRadians(10.0), + ), + # relms. Control effort (voltage) tolerance. Decrease this to more + # heavily penalize control effort, or make the controller less + # aggressive. 12 is a good starting point because that is the + # (approximate) maximum voltage of a battery. + (12.0,), + # Nominal time between loops. 20ms for TimedRobot, but can be lower if + # using notifiers. + 0.020, + ) + + # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.LinearSystemLoop_2_1_1( + self.armPlant, self.controller, self.observer, 12.0, 0.020 + ) + + # An encoder set up to measure flywheel velocity in radians per second. + self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel) + + self.motor = wpilib.PWMSparkMax(kMotorPort) + + # A joystick to read the trigger from. + self.joystick = wpilib.Joystick(kJoystickPort) + + # We go 2 pi radians in 1 rotation, or 4096 counts. + self.encoder.setDistancePerPulse(math.tau / 4096) + + def teleopInit(self) -> None: + # Reset our loop to make sure it's in a known state. + self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()]) + + # Reset our last reference to the current state. + self.lastProfiledReference = wpimath.TrapezoidProfile.State( + self.encoder.getDistance(), self.encoder.getRate() + ) + + def teleopPeriodic(self) -> None: + # Sets the target position of our arm. This is similar to setting the setpoint of a + # PID controller. + + if self.joystick.getTrigger(): + # the trigger is pressed, so we go to the high goal. + goal = wpimath.TrapezoidProfile.State(kRaisedPosition, 0.0) + + else: + # Otherwise, we go to the low goal + goal = wpimath.TrapezoidProfile.State(kLoweredPosition, 0.0) + + # Step our TrapezoidalProfile forward 20ms and set it as our next reference + self.lastProfiledReference = self.profile.calculate( + 0.020, self.lastProfiledReference, goal + ) + self.loop.setNextR( + [self.lastProfiledReference.position, self.lastProfiledReference.velocity] + ) + + # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct([self.encoder.getDistance()]) + + # Update our LQR to generate new voltage commands and use the voltages to predict the next + # state with out Kalman filter. + self.loop.predict(0.020) + + # Send the new calculated voltage to the motors. + # voltage = duty cycle * battery voltage, so + # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) + self.motor.setVoltage(nextVoltage) diff --git a/examples/robot/StateSpaceElevator/robot.py b/examples/robot/StateSpaceElevator/robot.py new file mode 100644 index 000000000..2c63e122d --- /dev/null +++ b/examples/robot/StateSpaceElevator/robot.py @@ -0,0 +1,151 @@ +#!/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 math +import wpilib +import wpimath +import wpimath.units + +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 +kHighGoalPosition = wpimath.units.feetToMeters(3) +kLowGoalPosition = wpimath.units.feetToMeters(0) + +kCarriageMass = 4.5 +# kilograms + +# A 1.5in diameter drum has a radius of 0.75in, or 0.019in. +kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0 + +# Reduction between motors and encoder, as output over input. If the elevator spins slower than +# the motors, this number should be greater than one. +kElevatorGearing = 6.0 + + +class MyRobot(wpilib.TimedRobot): + """This is a sample program to demonstrate how to use a state-space controller to control an + elevator. + """ + + def __init__(self) -> None: + super().__init__() + + self.profile = wpimath.TrapezoidProfile( + wpimath.TrapezoidProfile.Constraints( + wpimath.units.feetToMeters(3.0), + wpimath.units.feetToMeters(6.0), # Max elevator speed and acceleration. + ) + ) + + self.lastProfiledReference = wpimath.TrapezoidProfile.State() + + # The plant holds a state-space model of our elevator. This system has the following properties: + + # States: [position, velocity], in meters and meters per second. + # Inputs (what we can "put in"): [voltage], in volts. + # Outputs (what we can measure): [position], in meters. + + # This elevator is driven by two NEO motors. + self.elevatorPlant = wpimath.LinearSystemId.elevatorSystem( + wpimath.DCMotor.NEO(2), + kCarriageMass, + kDrumRadius, + kElevatorGearing, + ).slice(0) + + # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.KalmanFilter_2_1_1( + self.elevatorPlant, + ( + wpimath.units.inchesToMeters(2), + wpimath.units.inchesToMeters(40), + ), # How accurate we think our model is, in meters and meters/second. + ( + 0.001, + ), # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading. + 0.020, + ) + + # A LQR uses feedback to create voltage commands. + self.controller = wpimath.LinearQuadraticRegulator_2_1( + self.elevatorPlant, + # qelms. State error tolerance, in meters and meters per second. + # Decrease this to more heavily penalize state excursion, or make the + # controller behave more aggressively. + ( + wpimath.units.inchesToMeters(1.0), + wpimath.units.inchesToMeters(10.0), + ), + # relms. Control effort (voltage) tolerance. Decrease this to more + # heavily penalize control effort, or make the controller less + # aggressive. 12 is a good starting point because that is the + # (approximate) maximum voltage of a battery. + (12.0,), + # Nominal time between loops. 20ms for TimedRobot, but can be lower if + # using notifiers. + 0.020, + ) + + # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.LinearSystemLoop_2_1_1( + self.elevatorPlant, self.controller, self.observer, 12.0, 0.020 + ) + + # An encoder set up to measure elevator height in meters. + self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel) + + self.motor = wpilib.PWMSparkMax(kMotorPort) + + # A joystick to read the trigger from. + self.joystick = wpilib.Joystick(kJoystickPort) + + # Circumference = pi * d, so distance per click = pi * d / counts + self.encoder.setDistancePerPulse(math.tau * kDrumRadius / 4096) + + def teleopInit(self) -> None: + # Reset our loop to make sure it's in a known state. + self.loop.reset([self.encoder.getDistance(), self.encoder.getRate()]) + + # Reset our last reference to the current state. + self.lastProfiledReference = wpimath.TrapezoidProfile.State( + self.encoder.getDistance(), self.encoder.getRate() + ) + + def teleopPeriodic(self) -> None: + # Sets the target position of our arm. This is similar to setting the setpoint of a + # PID controller. + + if self.joystick.getTrigger(): + # the trigger is pressed, so we go to the high goal. + goal = wpimath.TrapezoidProfile.State(kHighGoalPosition, 0.0) + + else: + # Otherwise, we go to the low goal + goal = wpimath.TrapezoidProfile.State(kLowGoalPosition, 0.0) + + # Step our TrapezoidalProfile forward 20ms and set it as our next reference + self.lastProfiledReference = self.profile.calculate( + 0.020, self.lastProfiledReference, goal + ) + self.loop.setNextR( + [self.lastProfiledReference.position, self.lastProfiledReference.velocity] + ) + + # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct([self.encoder.getDistance()]) + + # Update our LQR to generate new voltage commands and use the voltages to predict the next + # state with out Kalman filter. + self.loop.predict(0.020) + + # Send the new calculated voltage to the motors. + # voltage = duty cycle * battery voltage, so + # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) + self.motor.setVoltage(nextVoltage) diff --git a/examples/robot/StateSpaceFlywheel/robot.py b/examples/robot/StateSpaceFlywheel/robot.py new file mode 100644 index 000000000..c233fef89 --- /dev/null +++ b/examples/robot/StateSpaceFlywheel/robot.py @@ -0,0 +1,113 @@ +#!/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 math + +import wpilib +import wpimath.units + +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 + +kSpinUpRadPerSec = 500.0 +kFlywheelMomentOfInertia = 0.00032 # kg/m^2 + +# Reduction between motors and encoder, as output over input. If the flywheel spins slower than +# the motors, this number should be greater than one. +kFlywheelGearing = 1.0 + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program to demonstrate how to use a state-space controller to control a + flywheel. + """ + + def __init__(self) -> None: + super().__init__() + + self.kSpinUpRadPerSec = wpimath.units.rotationsPerMinuteToRadiansPerSecond(500) + + # The plant holds a state-space model of our flywheel. This system has the following properties: + # + # States: [velocity], in radians per second. + # Inputs (what we can "put in"): [voltage], in volts. + # Outputs (what we can measure): [velocity], in radians per second. + self.flywheelPlant = wpimath.LinearSystemId.flywheelSystem( + wpimath.DCMotor.NEO(2), + kFlywheelMomentOfInertia, + kFlywheelGearing, + ) + + # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.KalmanFilter_1_1_1( + self.flywheelPlant, + (3,), # How accurate we think our model is + (0.01,), # How accurate we think our encoder data is + 0.020, + ) + + # A LQR uses feedback to create voltage commands. + self.controller = wpimath.LinearQuadraticRegulator_1_1( + self.flywheelPlant, + # qelms. Velocity error tolerance, in radians per second. Decrease + # this to more heavily penalize state excursion, or make the controller behave more + # aggressively. + (8,), + # relms. Control effort (voltage) tolerance. Decrease this to more + # heavily penalize control effort, or make the controller less aggressive. 12 is a good + # starting point because that is the (approximate) maximum voltage of a battery. + (12,), + # Nominal time between loops. 0.020 for TimedRobot, but can be lower if using notifiers. + 0.020, + ) + + # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.LinearSystemLoop_1_1_1( + self.flywheelPlant, self.controller, self.observer, 12.0, 0.020 + ) + + # An encoder set up to measure flywheel velocity in radians per second. + self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel) + + self.motor = wpilib.PWMSparkMax(kMotorPort) + + # A joystick to read the trigger from. + self.joystick = wpilib.Joystick(kJoystickPort) + + # We go 2 pi radians per 4096 clicks. + self.encoder.setDistancePerPulse(math.tau / 4096) + + def teleopInit(self) -> None: + self.loop.reset([self.encoder.getRate()]) + + def teleopPeriodic(self) -> None: + + # Sets the target speed of our flywheel. This is similar to setting the setpoint of a + # PID controller. + if self.joystick.getTriggerPressed(): + # We just pressed the trigger, so let's set our next reference + self.loop.setNextR([kSpinUpRadPerSec]) + + elif self.joystick.getTriggerReleased(): + # We just released the trigger, so let's spin down + self.loop.setNextR([0.0]) + + # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct([self.encoder.getRate()]) + + # Update our LQR to generate new voltage commands and use the voltages to predict the next + # state with out Kalman filter. + self.loop.predict(0.020) + + # Send the new calculated voltage to the motors. + # voltage = duty cycle * battery voltage, so + # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) + self.motor.setVoltage(nextVoltage) diff --git a/examples/robot/StateSpaceFlywheelSysId/robot.py b/examples/robot/StateSpaceFlywheelSysId/robot.py new file mode 100644 index 000000000..1589a9771 --- /dev/null +++ b/examples/robot/StateSpaceFlywheelSysId/robot.py @@ -0,0 +1,103 @@ +#!/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 math +import wpilib +import wpimath +import wpimath.units + +kMotorPort = 0 +kEncoderAChannel = 0 +kEncoderBChannel = 1 +kJoystickPort = 0 +kSpinupRadPerSec = wpimath.units.rotationsPerMinuteToRadiansPerSecond(500.0) + +# Volts per (radian per second) +kFlywheelKv = 0.023 + +# Volts per (radian per second squared) +kFlywheelKa = 0.001 + + +class MyRobot(wpilib.TimedRobot): + """ + This is a sample program to demonstrate how to use a state-space controller to control a + flywheel. + """ + + def __init__(self) -> None: + super().__init__() + + # The plant holds a state-space model of our flywheel. This system has the following properties: + # + # States: [velocity], in radians per second. + # Inputs (what we can "put in"): [voltage], in volts. + # Outputs (what we can measure): [velocity], in radians per second. + # + # The Kv and Ka constants are found using the FRC Characterization toolsuite. + self.flywheelPlant = wpimath.LinearSystemId.identifyVelocitySystemRadians( + kFlywheelKv, kFlywheelKa + ) + + # The observer fuses our encoder data and voltage inputs to reject noise. + self.observer = wpimath.KalmanFilter_1_1_1( + self.flywheelPlant, + (3,), # How accurate we think our model is + (0.01,), # How accurate we think our encoder data is + 0.020, + ) + + # A LQR uses feedback to create voltage commands. + self.controller = wpimath.LinearQuadraticRegulator_1_1( + self.flywheelPlant, + (8,), # Velocity error tolerance + (12,), # Control effort (voltage) tolerance + 0.020, + ) + + # The state-space loop combines a controller, observer, feedforward and plant for easy control. + self.loop = wpimath.LinearSystemLoop_1_1_1( + self.flywheelPlant, self.controller, self.observer, 12.0, 0.020 + ) + + # An encoder set up to measure flywheel velocity in radians per second. + self.encoder = wpilib.Encoder(kEncoderAChannel, kEncoderBChannel) + + self.motor = wpilib.PWMSparkMax(kMotorPort) + + # A joystick to read the trigger from. + self.joystick = wpilib.Joystick(kJoystickPort) + + # We go 2 pi radians per 4096 clicks. + self.encoder.setDistancePerPulse(math.tau / 4096) + + def teleopInit(self) -> None: + self.loop.reset([self.encoder.getRate()]) + + def teleopPeriodic(self) -> None: + # Sets the target speed of our flywheel. This is similar to setting the setpoint of a + # PID controller. + if self.joystick.getTriggerPressed(): + # We just pressed the trigger, so let's set our next reference + self.loop.setNextR([kSpinupRadPerSec]) + + elif self.joystick.getTriggerReleased(): + # We just released the trigger, so let's spin down + self.loop.setNextR([0]) + + # Correct our Kalman filter's state vector estimate with encoder data. + self.loop.correct([self.encoder.getRate()]) + + # Update our LQR to generate new voltage commands and use the voltages to predict the next + # state with out Kalman filter. + self.loop.predict(0.020) + + # Send the new calculated voltage to the motors. + # voltage = duty cycle * battery voltage, so + # duty cycle = voltage / battery voltage + nextVoltage = self.loop.U(0) + self.motor.setVoltage(nextVoltage) diff --git a/examples/robot/SwerveBot/drivetrain.py b/examples/robot/SwerveBot/drivetrain.py new file mode 100644 index 000000000..59ce064ad --- /dev/null +++ b/examples/robot/SwerveBot/drivetrain.py @@ -0,0 +1,95 @@ +# +# 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 math +import wpilib +import swervemodule +import wpimath + +kMaxSpeed = 3.0 # 3 meters per second +kMaxAngularSpeed = math.pi # 1/2 rotation per second + + +class Drivetrain: + """ + Represents a swerve drive style drivetrain. + """ + + def __init__(self) -> None: + self.frontLeftLocation = wpimath.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.Translation2d(-0.381, -0.381) + + self.frontLeft = swervemodule.SwerveModule(1, 2, 0, 1, 2, 3) + self.frontRight = swervemodule.SwerveModule(3, 4, 4, 5, 6, 7) + self.backLeft = swervemodule.SwerveModule(5, 6, 8, 9, 10, 11) + self.backRight = swervemodule.SwerveModule(7, 8, 12, 13, 14, 15) + + self.imu = wpilib.OnboardIMU(wpilib.OnboardIMU.MountOrientation.kFlat) + + self.kinematics = wpimath.SwerveDrive4Kinematics( + self.frontLeftLocation, + self.frontRightLocation, + self.backLeftLocation, + self.backRightLocation, + ) + + self.odometry = wpimath.SwerveDrive4Odometry( + self.kinematics, + self.imu.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + self.imu.resetYaw() + + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + periodSeconds: float, + ) -> None: + """ + Method to drive the robot using joystick info. + :param xSpeed: Speed of the robot in the x direction (forward). + :param ySpeed: Speed of the robot in the y direction (sideways). + :param rot: Angular rate of the robot. + :param fieldRelative: Whether the provided x and y speeds are relative to the field. + :param periodSeconds: Time + """ + robot_speeds = wpimath.ChassisSpeeds(xSpeed, ySpeed, rot) + if fieldRelative: + robot_speeds = robot_speeds.toRobotRelative(self.imu.getRotation2d()) + + swerveModuleStates = self.kinematics.toSwerveModuleStates( + wpimath.ChassisSpeeds.discretize(robot_speeds, periodSeconds) + ) + wpimath.SwerveDrive4Kinematics.desaturateWheelSpeeds( + swerveModuleStates, kMaxSpeed + ) + self.frontLeft.setDesiredState(swerveModuleStates[0]) + self.frontRight.setDesiredState(swerveModuleStates[1]) + self.backLeft.setDesiredState(swerveModuleStates[2]) + self.backRight.setDesiredState(swerveModuleStates[3]) + + def updateOdometry(self) -> None: + """Updates the field relative position of the robot.""" + self.odometry.update( + self.imu.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) diff --git a/examples/robot/SwerveBot/robot.py b/examples/robot/SwerveBot/robot.py new file mode 100644 index 000000000..acfc173f8 --- /dev/null +++ b/examples/robot/SwerveBot/robot.py @@ -0,0 +1,63 @@ +#!/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 wpilib +import wpimath +import drivetrain + + +class MyRobot(wpilib.TimedRobot): + def __init__(self) -> None: + """Robot initialization function""" + super().__init__() + self.controller = wpilib.XboxController(0) + self.swerve = drivetrain.Drivetrain() + + # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + self.xspeedLimiter = wpimath.SlewRateLimiter(3) + self.yspeedLimiter = wpimath.SlewRateLimiter(3) + self.rotLimiter = wpimath.SlewRateLimiter(3) + + def autonomousPeriodic(self) -> None: + self.driveWithJoystick(False) + self.swerve.updateOdometry() + + def teleopPeriodic(self) -> None: + self.driveWithJoystick(True) + + def driveWithJoystick(self, fieldRelative: bool) -> None: + # Get the x speed. We are inverting this because Xbox controllers return + # negative values when we push forward. + xSpeed = ( + -self.xspeedLimiter.calculate( + wpimath.applyDeadband(self.controller.getLeftY(), 0.02) + ) + * drivetrain.kMaxSpeed + ) + + # Get the y speed or sideways/strafe speed. We are inverting this because + # we want a positive value when we pull to the left. Xbox controllers + # return positive values when you pull to the right by default. + ySpeed = ( + -self.yspeedLimiter.calculate( + wpimath.applyDeadband(self.controller.getLeftX(), 0.02) + ) + * drivetrain.kMaxSpeed + ) + + # Get the rate of angular rotation. We are inverting this because we want a + # positive value when we pull to the left (remember, CCW is positive in + # mathematics). Xbox controllers return positive values when you pull to + # the right by default. + rot = ( + -self.rotLimiter.calculate( + wpimath.applyDeadband(self.controller.getRightX(), 0.02) + ) + * drivetrain.kMaxAngularSpeed + ) + + self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) diff --git a/examples/robot/SwerveBot/swervemodule.py b/examples/robot/SwerveBot/swervemodule.py new file mode 100644 index 000000000..f02b478ca --- /dev/null +++ b/examples/robot/SwerveBot/swervemodule.py @@ -0,0 +1,132 @@ +# +# 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 math + +import wpilib +import wpimath + +kWheelRadius = 0.0508 +kEncoderResolution = 4096 +kModuleMaxAngularVelocity = math.pi +kModuleMaxAngularAcceleration = math.tau + + +class SwerveModule: + def __init__( + self, + driveMotorChannel: int, + turningMotorChannel: int, + driveEncoderChannelA: int, + driveEncoderChannelB: int, + turningEncoderChannelA: int, + turningEncoderChannelB: int, + ) -> None: + """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder. + + :param driveMotorChannel: PWM output for the drive motor. + :param turningMotorChannel: PWM output for the turning motor. + :param driveEncoderChannelA: DIO input for the drive encoder channel A + :param driveEncoderChannelB: DIO input for the drive encoder channel B + :param turningEncoderChannelA: DIO input for the turning encoder channel A + :param turningEncoderChannelB: DIO input for the turning encoder channel B + """ + self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) + self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) + + self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB) + self.turningEncoder = wpilib.Encoder( + turningEncoderChannelA, turningEncoderChannelB + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.drivePIDController = wpimath.PIDController(1, 0, 0) + + # Gains are for example purposes only - must be determined for your own robot! + self.turningPIDController = wpimath.ProfiledPIDController( + 1, + 0, + 0, + wpimath.TrapezoidProfile.Constraints( + kModuleMaxAngularVelocity, + kModuleMaxAngularAcceleration, + ), + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.driveFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 3) + self.turnFeedforward = wpimath.SimpleMotorFeedforwardMeters(1, 0.5) + + # Set the distance per pulse for the drive encoder. We can simply use the + # distance traveled for one rotation of the wheel divided by the encoder + # resolution. + self.driveEncoder.setDistancePerPulse( + math.tau * kWheelRadius / kEncoderResolution + ) + + # Set the distance (in this case, angle) in radians per pulse for the turning encoder. + # This is the the angle through an entire rotation (2 * pi) divided by the + # encoder resolution. + self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution) + + # Limit the PID Controller's input range between -pi and pi and set the input + # to be continuous. + self.turningPIDController.enableContinuousInput(-math.pi, math.pi) + + def getState(self) -> wpimath.SwerveModuleState: + """Returns the current state of the module. + + :returns: The current state of the module. + """ + return wpimath.SwerveModuleState( + self.driveEncoder.getRate(), + wpimath.Rotation2d(self.turningEncoder.getDistance()), + ) + + def getPosition(self) -> wpimath.SwerveModulePosition: + """Returns the current position of the module. + + :returns: The current position of the module. + """ + return wpimath.SwerveModulePosition( + self.driveEncoder.getDistance(), + wpimath.Rotation2d(self.turningEncoder.getDistance()), + ) + + def setDesiredState(self, desiredState: wpimath.SwerveModuleState) -> None: + """Sets the desired state for the module. + + :param desiredState: Desired state with speed and angle. + """ + + encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance()) + + # Optimize the reference state to avoid spinning further than 90 degrees + desiredState.optimize(encoderRotation) + + # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired + # direction of travel that can occur when modules change directions. This results in smoother + # driving. + desiredState.cosineScale(encoderRotation) + + # Calculate the drive output from the drive PID controller. + driveOutput = self.drivePIDController.calculate( + self.driveEncoder.getRate(), desiredState.speed + ) + + driveFeedforward = self.driveFeedforward.calculate(desiredState.speed) + + # Calculate the turning motor output from the turning PID controller. + turnOutput = self.turningPIDController.calculate( + self.turningEncoder.getDistance(), desiredState.angle.radians() + ) + + turnFeedforward = self.turnFeedforward.calculate( + self.turningPIDController.getSetpoint().velocity + ) + + self.driveMotor.setVoltage(driveOutput + driveFeedforward) + self.turningMotor.setVoltage(turnOutput + turnFeedforward) diff --git a/examples/robot/SysId/constants.py b/examples/robot/SysId/constants.py new file mode 100644 index 000000000..96652fcc1 --- /dev/null +++ b/examples/robot/SysId/constants.py @@ -0,0 +1,73 @@ +# +# 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 math + +import wpimath.units + + +class DriveConstants: + kLeftMotor1Port = 0 + kLeftMotor2Port = 1 + kRightMotor1Port = 2 + kRightMotor2Port = 3 + + kLeftEncoderPorts = (0, 1) + kRightEncoderPorts = (2, 3) + kLeftEncoderReversed = False + kRightEncoderReversed = True + + kEncoderCPR = 1024 + kWheelDiameter = wpimath.units.inchesToMeters(6) + # Assumes the encoders are directly mounted on the wheel shafts + kEncoderDistancePerPulse = (kWheelDiameter * math.pi) / kEncoderCPR + + +class ShooterConstants: + kEncoderPorts = (4, 5) + kEncoderReversed = False + kEncoderCPR = 1024 + # Distance units will be rotations + kEncoderDistancePerPulse = 1.0 / kEncoderCPR + + kShooterMotorPort = 4 + kFeederMotorPort = 5 + + kShooterFreeRPS = 5300.0 + kShooterTargetRPS = 4000.0 + kShooterToleranceRPS = 50.0 + + # These are not real PID gains, and will have to be tuned for your specific robot. + kP = 1.0 + + # On a real robot the feedforward constants should be empirically determined; these are + # reasonable guesses. + kS = 0.05 # V + # Should have value 12V at free speed + kV = 12.0 / kShooterFreeRPS # V/(rot/s) + kA = 0.0 # V/(rot/s^2) + + kFeederSpeed = 0.5 + + +class IntakeConstants: + kMotorPort = 6 + kSolenoidPorts = (2, 3) + + +class StorageConstants: + kMotorPort = 7 + kBallSensorPort = 6 + + +class AutoConstants: + kTimeout = 3 + kDriveDistance = 2.0 # m + kDriveSpeed = 0.5 + + +class OIConstants: + kDriverControllerPort = 0 diff --git a/examples/robot/SysId/robot.py b/examples/robot/SysId/robot.py new file mode 100644 index 000000000..9ec120491 --- /dev/null +++ b/examples/robot/SysId/robot.py @@ -0,0 +1,66 @@ +#!/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. +# + +from commands2 import CommandScheduler, TimedCommandRobot + +from sysidroutinebot import SysIdRoutineBot + + +class MyRobot(TimedCommandRobot): + """ + The methods in this class are called automatically corresponding to each mode, + as described in the TimedRobot documentation. + """ + + 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.robot = SysIdRoutineBot() + + # Configure default commands and condition bindings on robot startup + self.robot.configureBindings() + + self.autonomous_command = None + + def disabledInit(self) -> None: + """This function is called once each time the robot enters Disabled mode.""" + pass + + def disabledPeriodic(self) -> None: + pass + + def autonomousInit(self) -> None: + self.autonomous_command = self.robot.getAutonomousCommand() + + if self.autonomous_command is not None: + CommandScheduler.getInstance().schedule(self.autonomous_command) + + def autonomousPeriodic(self) -> None: + """This function is called periodically during autonomous.""" + pass + + 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.autonomous_command is not None: + self.autonomous_command.cancel() + + def teleopPeriodic(self) -> None: + """This function is called periodically during operator control.""" + pass + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll() + + def testPeriodic(self) -> None: + """This function is called periodically during test mode.""" + pass diff --git a/examples/robot/SysId/subsystems/drive.py b/examples/robot/SysId/subsystems/drive.py new file mode 100644 index 000000000..c157119d7 --- /dev/null +++ b/examples/robot/SysId/subsystems/drive.py @@ -0,0 +1,125 @@ +# +# 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. +# + +from typing import Callable + +from commands2 import Command, Subsystem +from commands2.sysid import SysIdRoutine +from wpilib import DifferentialDrive, Encoder, PWMSparkMax, RobotController +from wpilib.sysid import SysIdRoutineLog + +from constants import DriveConstants + + +class Drive(Subsystem): + """Represents the drive subsystem.""" + + def __init__(self) -> None: + """Creates a new Drive subsystem.""" + super().__init__() + + # The motors on the left side of the drive. + self.left_motor = PWMSparkMax(DriveConstants.kLeftMotor1Port) + + # The motors on the right side of the drive. + self.right_motor = PWMSparkMax(DriveConstants.kRightMotor1Port) + + # The robot's drive + self.drive = DifferentialDrive(self.left_motor, self.right_motor) + + # The left-side drive encoder + self.left_encoder = Encoder( + DriveConstants.kLeftEncoderPorts[0], + DriveConstants.kLeftEncoderPorts[1], + DriveConstants.kLeftEncoderReversed, + ) + + # The right-side drive encoder + self.right_encoder = Encoder( + DriveConstants.kRightEncoderPorts[0], + DriveConstants.kRightEncoderPorts[1], + DriveConstants.kRightEncoderReversed, + ) + + # Create a new SysId routine for characterizing the drive. + self.sys_id_routine = SysIdRoutine( + # Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. + SysIdRoutine.Config(), + SysIdRoutine.Mechanism( + # Tell SysId how to plumb the driving voltage to the motors. + self._driveVoltage, + # Tell SysId how to record a frame of data for each motor on the mechanism being + # characterized. + self._log, + # Tell SysId to make generated commands require this subsystem, suffix test state in + # WPILog with this subsystem's name ("drive") + self, + ), + ) + + # Add the second motors on each side of the drivetrain + self.left_motor.addFollower(PWMSparkMax(DriveConstants.kLeftMotor2Port)) + self.right_motor.addFollower(PWMSparkMax(DriveConstants.kRightMotor2Port)) + + # 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.right_motor.setInverted(True) + + # Sets the distance per pulse for the encoders + self.left_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse) + self.right_encoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse) + + def _driveVoltage(self, voltage: float) -> None: + self.left_motor.setVoltage(voltage) + self.right_motor.setVoltage(voltage) + + def _log(self, sys_id_routine: SysIdRoutineLog) -> None: + # Record a frame for the left motors. Since these share an encoder, we consider + # the entire group to be one motor. + sys_id_routine.motor("drive-left").voltage( + self.left_motor.get() * RobotController.getBatteryVoltage() + ).position(self.left_encoder.getDistance()).velocity( + self.left_encoder.getRate() + ) + # Record a frame for the right motors. Since these share an encoder, we consider + # the entire group to be one motor. + sys_id_routine.motor("drive-right").voltage( + self.right_motor.get() * RobotController.getBatteryVoltage() + ).position(self.right_encoder.getDistance()).velocity( + self.right_encoder.getRate() + ) + + def arcadeDriveCommand( + self, fwd: Callable[[], float], rot: Callable[[], float] + ) -> Command: + """Returns a command that drives the robot with arcade controls. + + :param fwd: the commanded forward movement + :param rot: the commanded rotation + """ + + # A split-stick arcade command, with forward/backward controlled by the left + # hand, and turning controlled by the right. + return self.run(lambda: self.drive.arcadeDrive(fwd(), rot())).withName( + "arcadeDrive" + ) + + def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command: + """Returns a command that will execute a quasistatic test in the given direction. + + :param direction: The direction (forward or reverse) to run the test in + """ + + return self.sys_id_routine.quasistatic(direction) + + def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command: + """Returns a command that will execute a dynamic test in the given direction. + + :param direction: The direction (forward or reverse) to run the test in + """ + + return self.sys_id_routine.dynamic(direction) diff --git a/examples/robot/SysId/subsystems/shooter.py b/examples/robot/SysId/subsystems/shooter.py new file mode 100644 index 000000000..11573c530 --- /dev/null +++ b/examples/robot/SysId/subsystems/shooter.py @@ -0,0 +1,118 @@ +# +# 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. +# + +from typing import Callable + +from commands2 import Command, Subsystem +from commands2.sysid import SysIdRoutine +from wpilib import Encoder, PWMSparkMax, RobotController +from wpilib.sysid import SysIdRoutineLog + +import wpimath +import wpimath.units + +from constants import ShooterConstants + + +class Shooter(Subsystem): + """Represents the shooter subsystem.""" + + def __init__(self) -> None: + """Creates a new Shooter subsystem.""" + super().__init__() + + # The motor on the shooter wheel . + self.shooter_motor = PWMSparkMax(ShooterConstants.kShooterMotorPort) + + # The motor on the feeder wheels. + self.feeder_motor = PWMSparkMax(ShooterConstants.kFeederMotorPort) + + # The shooter wheel encoder + self.shooter_encoder = Encoder( + ShooterConstants.kEncoderPorts[0], + ShooterConstants.kEncoderPorts[1], + ShooterConstants.kEncoderReversed, + ) + + # Create a new SysId routine for characterizing the shooter. + self.sys_id_routine = SysIdRoutine( + # Empty config defaults to 1 volt/second ramp rate and 7 volt step voltage. + SysIdRoutine.Config(), + SysIdRoutine.Mechanism( + # Tell SysId how to plumb the driving voltage to the motor(s). + self.shooter_motor.setVoltage, + # Tell SysId how to record a frame of data for each motor on the mechanism being + # characterized. + self._log, + # Tell SysId to make generated commands require this subsystem, suffix test state in + # WPILog with this subsystem's name ("shooter") + self, + ), + ) + + # PID controller to run the shooter wheel in closed-loop, set the constants equal to those + # calculated by SysId + self.shooter_feedback = wpimath.PIDController(ShooterConstants.kP, 0, 0) + # Feedforward controller to run the shooter wheel in closed-loop, set the constants equal to + # those calculated by SysId + self.shooter_feedforward = wpimath.SimpleMotorFeedforwardRadians( + ShooterConstants.kS, + ShooterConstants.kV / wpimath.units.rotationsToRadians(1), + ShooterConstants.kA / wpimath.units.rotationsToRadians(1), + ) + + # Sets the distance per pulse for the encoders + self.shooter_encoder.setDistancePerPulse( + ShooterConstants.kEncoderDistancePerPulse + ) + + def _log(self, sys_id_routine: SysIdRoutineLog) -> None: + # Record a frame for the shooter motor. + sys_id_routine.motor("shooter-wheel").voltage( + self.shooter_motor.get() * RobotController.getBatteryVoltage() + ).angularPosition(self.shooter_encoder.getDistance()).angularVelocity( + self.shooter_encoder.getRate() + ) + + def runShooter(self, shooterSpeed: Callable[[], float]) -> Command: + """Returns a command that runs the shooter at a specifc velocity. + + :param shooterSpeed: The commanded shooter wheel speed in rotations per second + """ + + # Run shooter wheel at the desired speed using a PID controller and feedforward. + def _run_shooter() -> None: + target_speed = shooterSpeed() + target_speed_radians = wpimath.units.rotationsToRadians(target_speed) + self.shooter_motor.setVoltage( + self.shooter_feedback.calculate( + self.shooter_encoder.getRate(), target_speed + ) + + self.shooter_feedforward.calculate(target_speed_radians) + ) + self.feeder_motor.set(ShooterConstants.kFeederSpeed) + + def _stop_motors(interrupted: bool) -> None: + self.shooter_motor.stopMotor() + self.feeder_motor.stopMotor() + + return self.run(_run_shooter).finallyDo(_stop_motors).withName("runShooter") + + def sysIdQuasistatic(self, direction: SysIdRoutine.Direction) -> Command: + """Returns a command that will execute a quasistatic test in the given direction. + + :param direction: The direction (forward or reverse) to run the test in + """ + + return self.sys_id_routine.quasistatic(direction) + + def sysIdDynamic(self, direction: SysIdRoutine.Direction) -> Command: + """Returns a command that will execute a dynamic test in the given direction. + + :param direction: The direction (forward or reverse) to run the test in + """ + + return self.sys_id_routine.dynamic(direction) diff --git a/examples/robot/SysId/sysidroutinebot.py b/examples/robot/SysId/sysidroutinebot.py new file mode 100644 index 000000000..0a2a267c2 --- /dev/null +++ b/examples/robot/SysId/sysidroutinebot.py @@ -0,0 +1,91 @@ +# +# 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. +# + +from commands2 import Command +from commands2.button import CommandXboxController +from commands2.sysid import SysIdRoutine + +from subsystems.drive import Drive +from subsystems.shooter import Shooter + +from constants import OIConstants + + +class SysIdRoutineBot: + """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.drive = Drive() + self.shooter = Shooter() + + # The driver's controller + self.controller = CommandXboxController(OIConstants.kDriverControllerPort) + + def configureBindings(self) -> None: + """Use this method to define bindings between conditions and commands. These are useful for + automating robot behaviors based on button and sensor input. + + Should be called in the robot class constructor. + + Event binding methods are available on the :class:`.Trigger` class. + """ + + # Control the drive with split-stick arcade controls + self.drive.setDefaultCommand( + self.drive.arcadeDriveCommand( + lambda: -self.controller.getLeftY(), + lambda: -self.controller.getRightX(), + ) + ) + + # Bind full set of SysId routine tests to buttons; a complete routine should run each of these + # once. + # Using bumpers as a modifier and combining it with the buttons so that we can have both sets + # of bindings at once + self.controller.a().and_(self.controller.rightBumper()).whileTrue( + self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward) + ) + self.controller.b().and_(self.controller.rightBumper()).whileTrue( + self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) + ) + self.controller.x().and_(self.controller.rightBumper()).whileTrue( + self.drive.sysIdDynamic(SysIdRoutine.Direction.kForward) + ) + self.controller.y().and_(self.controller.rightBumper()).whileTrue( + self.drive.sysIdDynamic(SysIdRoutine.Direction.kReverse) + ) + + # Control the shooter wheel with the left trigger + self.shooter.setDefaultCommand( + self.shooter.runShooter(self.controller.getLeftTriggerAxis) + ) + + self.controller.a().and_(self.controller.leftBumper()).whileTrue( + self.shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward) + ) + self.controller.b().and_(self.controller.leftBumper()).whileTrue( + self.shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) + ) + self.controller.x().and_(self.controller.leftBumper()).whileTrue( + self.shooter.sysIdDynamic(SysIdRoutine.Direction.kForward) + ) + self.controller.y().and_(self.controller.leftBumper()).whileTrue( + self.shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse) + ) + + def getAutonomousCommand(self) -> Command: + """Use this to define the command that runs during autonomous. + + Scheduled during :meth:`.Robot.autonomousInit`. + """ + + # Do nothing + return self.drive.run(lambda: None) diff --git a/examples/robot/TankDrive/robot.py b/examples/robot/TankDrive/robot.py new file mode 100755 index 000000000..73e097b79 --- /dev/null +++ b/examples/robot/TankDrive/robot.py @@ -0,0 +1,33 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing the use of the DifferentialDrive class. + Runs the motors with tank steering. + """ + + def __init__(self): + """Robot initialization function""" + super().__init__() + + leftMotor = wpilib.PWMSparkMax(0) + rightMotor = wpilib.PWMSparkMax(1) + self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor) + self.leftStick = wpilib.Joystick(0) + self.rightStick = wpilib.Joystick(1) + + # 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. + rightMotor.setInverted(True) + + def teleopPeriodic(self): + self.robotDrive.tankDrive(-self.leftStick.getY(), -self.rightStick.getY()) diff --git a/examples/robot/TankDriveXboxController/robot.py b/examples/robot/TankDriveXboxController/robot.py new file mode 100755 index 000000000..a3a400835 --- /dev/null +++ b/examples/robot/TankDriveXboxController/robot.py @@ -0,0 +1,38 @@ +#!/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 wpilib + + +class MyRobot(wpilib.TimedRobot): + """ + This is a demo program showing the use of the DifferentialDrive class. + Runs the motors with tank steering and an Xbox controller. + """ + + def __init__(self): + """Robot initialization function""" + super().__init__() + + leftMotor = wpilib.PWMSparkMax(0) + rightMotor = wpilib.PWMSparkMax(1) + self.robotDrive = wpilib.DifferentialDrive(leftMotor, rightMotor) + self.driverController = wpilib.XboxController(0) + + # 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. + rightMotor.setInverted(True) + + def teleopPeriodic(self): + # Drive with tank drive. + # That means that the Y axis of the left stick moves the left side + # of the robot forward and backward, and the Y axis of the right stick + # moves the right side of the robot forward and backward. + self.robotDrive.tankDrive( + -self.driverController.getLeftY(), -self.driverController.getRightY() + ) diff --git a/examples/robot/XrpReference/commands/arcadedrive.py b/examples/robot/XrpReference/commands/arcadedrive.py new file mode 100644 index 000000000..e370b745d --- /dev/null +++ b/examples/robot/XrpReference/commands/arcadedrive.py @@ -0,0 +1,34 @@ +# +# 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 commands2 +from subsystems.drivetrain import Drivetrain + + +class ArcadeDrive(commands2.Command): + def __init__( + self, + drive: Drivetrain, + xaxisSpeedSupplier: typing.Callable[[], float], + zaxisRotateSupplier: typing.Callable[[], float], + ) -> None: + """Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier + lambdas. This command does not terminate. + + :param drivetrain: The drivetrain subsystem on which this command will run + :param xaxisSpeedSupplier: Callable supplier of forward/backward speed + :param zaxisRotateSupplier: Callable supplier of rotational speed + """ + + self.drive = drive + self.xaxisSpeedSupplier = xaxisSpeedSupplier + self.zaxisRotateSupplier = zaxisRotateSupplier + + self.addRequirements(self.drive) + + def execute(self) -> None: + self.drive.arcadeDrive(self.xaxisSpeedSupplier(), self.zaxisRotateSupplier()) diff --git a/examples/robot/XrpReference/commands/autonomousdistance.py b/examples/robot/XrpReference/commands/autonomousdistance.py new file mode 100644 index 000000000..ad84366e2 --- /dev/null +++ b/examples/robot/XrpReference/commands/autonomousdistance.py @@ -0,0 +1,28 @@ +# +# 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 + +from commands.drivedistance import DriveDistance +from commands.turndegrees import TurnDegrees +from subsystems.drivetrain import Drivetrain + + +class AutonomousDistance(commands2.SequentialCommandGroup): + def __init__(self, drive: Drivetrain) -> None: + """Creates a new Autonomous Drive based on distance. This will drive out for a specified distance, + turn around and drive back. + + :param drivetrain: The drivetrain subsystem on which this command will run + """ + super().__init__() + + self.addCommands( + DriveDistance(-0.5, 10, drive), + TurnDegrees(-0.5, 180, drive), + DriveDistance(-0.5, 10, drive), + TurnDegrees(0.5, 180, drive), + ) diff --git a/examples/robot/XrpReference/commands/autonomoustime.py b/examples/robot/XrpReference/commands/autonomoustime.py new file mode 100644 index 000000000..1a48cf841 --- /dev/null +++ b/examples/robot/XrpReference/commands/autonomoustime.py @@ -0,0 +1,30 @@ +# +# 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 + +from commands.drivetime import DriveTime +from commands.turntime import TurnTime +from subsystems.drivetrain import Drivetrain + + +class AutonomousTime(commands2.SequentialCommandGroup): + def __init__(self, drive: Drivetrain) -> None: + """Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn + around for time (equivalent to time to turn around) and drive forward again. This should mimic + driving out, turning around and driving back. + + :param drivetrain: The drive subsystem on which this command will run + + """ + super().__init__() + + self.addCommands( + DriveTime(-0.6, 2.0, drive), + TurnTime(-0.5, 1.3, drive), + DriveTime(-0.6, 2.0, drive), + TurnTime(0.5, 1.3, drive), + ) diff --git a/examples/robot/XrpReference/commands/drivedistance.py b/examples/robot/XrpReference/commands/drivedistance.py new file mode 100644 index 000000000..41b0700d9 --- /dev/null +++ b/examples/robot/XrpReference/commands/drivedistance.py @@ -0,0 +1,43 @@ +# +# 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 + +from subsystems.drivetrain import Drivetrain + + +class DriveDistance(commands2.Command): + def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None: + """Creates a new DriveDistance. This command will drive your your robot for a desired distance at + a desired speed. + + :param speed: The speed at which the robot will drive + :param inches: The number of inches the robot will drive + :param drive: The drivetrain subsystem on which this command will run + """ + + self.distance = inches + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.drive.arcadeDrive(0, 0) + self.drive.resetEncoders() + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end.""" + # Compare distance travelled from start to desired distance + return abs(self.drive.getAverageDistanceInch()) >= self.distance diff --git a/examples/robot/XrpReference/commands/drivetime.py b/examples/robot/XrpReference/commands/drivetime.py new file mode 100644 index 000000000..bac0a840c --- /dev/null +++ b/examples/robot/XrpReference/commands/drivetime.py @@ -0,0 +1,46 @@ +# +# 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 wpilib + +from subsystems.drivetrain import Drivetrain + + +class DriveTime(commands2.Command): + """Creates a new DriveTime. This command will drive your robot for a desired speed and time.""" + + def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: + """Creates a new DriveTime. This command will drive your robot for a desired speed and time. + + :param speed: The speed which the robot will drive. Negative is in reverse. + :param time: How much time to drive in seconds + :param drive: The drivetrain subsystem on which this command will run + """ + + self.speed = speed + self.duration = time + self.drive = drive + self.addRequirements(drive) + + self.startTime = 0.0 + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.startTime = wpilib.Timer.getFPGATimestamp() + self.drive.arcadeDrive(0, 0) + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(self.speed, 0) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end""" + return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration diff --git a/examples/robot/XrpReference/commands/turndegrees.py b/examples/robot/XrpReference/commands/turndegrees.py new file mode 100644 index 000000000..2c3c7dafc --- /dev/null +++ b/examples/robot/XrpReference/commands/turndegrees.py @@ -0,0 +1,57 @@ +# +# 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 math +import commands2 + +from subsystems.drivetrain import Drivetrain + + +class TurnDegrees(commands2.Command): + def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None: + """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in + degrees) and rotational speed. + + :param speed: The speed which the robot will drive. Negative is in reverse. + :param degrees: Degrees to turn. Leverages encoders to compare distance. + :param drive: The drive subsystem on which this command will run + """ + + self.degrees = degrees + self.speed = speed + self.drive = drive + self.addRequirements(drive) + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + # Set motors to stop, read encoder values for starting point + self.drive.arcadeDrive(0, 0) + self.drive.resetEncoders() + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(0, self.speed) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end.""" + + # Need to convert distance travelled to degrees. The Standard + # XRP Chassis found here, https://www.sparkfun.com/products/22230, + # has a wheel placement diameter (163 mm) - width of the wheel (8 mm) = 155 mm + # or 6.102 inches. We then take into consideration the width of the tires. + inchPerDegree = math.pi * 6.102 / 360 + + # Compare distance travelled from start to distance based on degree turn + return self._getAverageTurningDistance() >= inchPerDegree * self.degrees + + def _getAverageTurningDistance(self) -> float: + leftDistance = abs(self.drive.getLeftDistanceInch()) + rightDistance = abs(self.drive.getRightDistanceInch()) + return (leftDistance + rightDistance) / 2.0 diff --git a/examples/robot/XrpReference/commands/turntime.py b/examples/robot/XrpReference/commands/turntime.py new file mode 100644 index 000000000..f806c8b7e --- /dev/null +++ b/examples/robot/XrpReference/commands/turntime.py @@ -0,0 +1,48 @@ +# +# 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 wpilib + +from subsystems.drivetrain import Drivetrain + + +class TurnTime(commands2.Command): + """Creates a new TurnTime command. This command will turn your robot for a + desired rotational speed and time. + """ + + def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: + """Creates a new TurnTime. + + :param speed: The speed which the robot will turn. Negative is in reverse. + :param time: How much time to turn in seconds + :param drive: The drive subsystem on which this command will run + """ + + self.rotationalSpeed = speed + self.duration = time + self.drive = drive + self.addRequirements(drive) + + self.startTime = 0.0 + + def initialize(self) -> None: + """Called when the command is initially scheduled.""" + self.startTime = wpilib.Timer.getFPGATimestamp() + self.drive.arcadeDrive(0, 0) + + def execute(self) -> None: + """Called every time the scheduler runs while the command is scheduled.""" + self.drive.arcadeDrive(0, self.rotationalSpeed) + + def end(self, interrupted: bool) -> None: + """Called once the command ends or is interrupted.""" + self.drive.arcadeDrive(0, 0) + + def isFinished(self) -> bool: + """Returns true when the command should end""" + return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration diff --git a/examples/robot/XrpReference/robot.py b/examples/robot/XrpReference/robot.py new file mode 100644 index 000000000..6827d86b4 --- /dev/null +++ b/examples/robot/XrpReference/robot.py @@ -0,0 +1,97 @@ +#!/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. +# + +# +# Example that shows how to connect to a XRP from RobotPy +# +# Requirements +# ------------ +# +# You must have the robotpy-xrp package installed. This is best done via: +# +# # Windows +# py -3 -m pip install robotpy[commands2,sim,xrp] +# +# # Linux/macOS +# pip3 install robotpy[commands2,sim,xrp] +# +# Run the program +# --------------- +# +# Use the dedicated XRP command: +# +# # Windows +# py -3 -m robotpy run-xrp +# +# # Linux/macOS +# python -m robotpy run-xrp +# +# If your XRP isn't at the default address, use --host/--port: +# +# python -m robotpy run-xrp --host 192.168.42.1 --port 3540 +# +# By default the WPILib simulation GUI will be displayed. To disable the display +# you can add the --nogui option. +# + +import typing + +import wpilib +import commands2 + +from robotcontainer import RobotContainer + + +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 + """ + + autonomousCommand: typing.Optional[commands2.Command] = None + + 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__() + + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = 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() + + if self.autonomousCommand: + self.autonomousCommand.schedule() + + 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: + self.autonomousCommand.cancel() + + def teleopPeriodic(self) -> None: + """This function is called periodically during operator control""" + + def testInit(self) -> None: + # Cancels all running commands at the start of test mode + commands2.CommandScheduler.getInstance().cancelAll() diff --git a/examples/robot/XrpReference/robotcontainer.py b/examples/robot/XrpReference/robotcontainer.py new file mode 100644 index 000000000..19fde0472 --- /dev/null +++ b/examples/robot/XrpReference/robotcontainer.py @@ -0,0 +1,95 @@ +# +# 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 commands2 +import commands2.button +import wpilib +import xrp + +from commands.arcadedrive import ArcadeDrive +from commands.autonomousdistance import AutonomousDistance +from commands.autonomoustime import AutonomousTime + +from subsystems.arm import Arm +from subsystems.drivetrain import Drivetrain + + +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 = xrp.XRPOnBoardIO() + self.arm = Arm() + + # Assumes a gamepad plugged into channnel 0 + self.controller = wpilib.Joystick(0) + + # Create SmartDashboard chooser for autonomous routines + self.chooser = wpilib.SendableChooser() + + 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 scheduled over it + self.drivetrain.setDefaultCommand(self.getArcadeDriveCommand()) + + # Example of how to use the onboard IO + onboardButtonA = commands2.button.Trigger(self.onboardIO.getUserButtonPressed) + onboardButtonA.onTrue(commands2.PrintCommand("USER Button Pressed")).onFalse( + commands2.PrintCommand("USER Button Released") + ) + + joystickAButton = commands2.button.JoystickButton(self.controller, 1) + joystickAButton.onTrue( + commands2.InstantCommand(lambda: self.arm.setAngle(45.0), self.arm) + ) + joystickAButton.onFalse( + commands2.InstantCommand(lambda: self.arm.setAngle(0.0), self.arm) + ) + + joystickBButton = commands2.button.JoystickButton(self.controller, 2) + joystickBButton.onTrue( + commands2.InstantCommand(lambda: self.arm.setAngle(90.0), self.arm) + ) + joystickBButton.onFalse( + commands2.InstantCommand(lambda: self.arm.setAngle(0.0), self.arm) + ) + + # 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.Command]: + 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), + ) diff --git a/examples/robot/XrpReference/subsystems/arm.py b/examples/robot/XrpReference/subsystems/arm.py new file mode 100644 index 000000000..3306da11f --- /dev/null +++ b/examples/robot/XrpReference/subsystems/arm.py @@ -0,0 +1,28 @@ +# +# 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 xrp + + +class Arm(commands2.Subsystem): + + def __init__(self): + """Creates a new Arm.""" + + # Device number 4 maps to the physical Servo 1 port on the XRP + self.armServo = xrp.XRPServo(4) + + def periodic(self): + """This method will be called once per scheduler run""" + + def setAngle(self, angleDeg: float): + """ + Set the current angle of the arm (0 - 180 degrees). + + :param angleDeg: Desired arm angle in degrees + """ + self.armServo.setAngle(angleDeg) diff --git a/examples/robot/XrpReference/subsystems/drivetrain.py b/examples/robot/XrpReference/subsystems/drivetrain.py new file mode 100644 index 000000000..04d80108d --- /dev/null +++ b/examples/robot/XrpReference/subsystems/drivetrain.py @@ -0,0 +1,113 @@ +# +# 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 math + +import commands2 +import wpilib +import wpiutil +import xrp + + +class Drivetrain(commands2.Subsystem): + kGearRatio = (30.0 / 14.0) * (28.0 / 16.0) * (36.0 / 9.0) * (26.0 / 8.0) # 48.75:1 + kCountsPerMotorShaftRev = 12.0 + kCountsPerRevolution = kCountsPerMotorShaftRev * kGearRatio # 585.0 + kWheelDiameterInch = 2.3622 # 60 mm + + def __init__(self) -> None: + + # The XRP has the left and right motors set to + # PWM channels 0 and 1 respectively + self.leftMotor = xrp.XRPMotor(0) + self.rightMotor = xrp.XRPMotor(1) + + # The XRP has onboard encoders that are hardcoded + # to use DIO pins 4/5 and 6/7 for the left and right + self.leftEncoder = wpilib.Encoder(4, 5) + self.rightEncoder = wpilib.Encoder(6, 7) + + # Set up the differential drive controller + self.drive = wpilib.DifferentialDrive(self.leftMotor.set, self.rightMotor.set) + + # TODO: these don't work + # wpiutil.SendableRegistry.addChild(self.drive, self.leftMotor) + # wpiutil.SendableRegistry.addChild(self.drive, self.rightMotor) + + # Set up the XRPGyro + self.gyro = xrp.XRPGyro() + + # 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.rightMotor.setInverted(True) + + # Use inches as unit for encoder distances + self.leftEncoder.setDistancePerPulse( + (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution + ) + self.rightEncoder.setDistancePerPulse( + (math.pi * self.kWheelDiameterInch) / self.kCountsPerRevolution + ) + self.resetEncoders() + + def arcadeDrive(self, xaxisSpeed: float, zaxisRotate: float) -> None: + """ + Drives the robot using arcade controls. + + :param xaxisSpeed: the commanded forward movement + :param zaxisRotate: the commanded rotation + """ + self.drive.arcadeDrive(xaxisSpeed, zaxisRotate) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + self.leftEncoder.reset() + self.rightEncoder.reset() + + def getLeftEncoderCount(self) -> int: + return self.leftEncoder.get() + + def getRightEncoderCount(self) -> int: + return self.rightEncoder.get() + + def getLeftDistanceInch(self) -> float: + return self.leftEncoder.getDistance() + + def getRightDistanceInch(self) -> float: + return self.rightEncoder.getDistance() + + def getAverageDistanceInch(self) -> float: + """Gets the average distance of the TWO encoders.""" + return (self.getLeftDistanceInch() + self.getRightDistanceInch()) / 2.0 + + def getGyroAngleX(self) -> float: + """Current angle of the Romi around the X-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleX() + + def getGyroAngleY(self) -> float: + """Current angle of the Romi around the Y-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleY() + + def getGyroAngleZ(self) -> float: + """Current angle of the Romi around the Z-axis. + + :returns: The current angle of the Romi in degrees + """ + return self.gyro.getAngleZ() + + def resetGyro(self) -> None: + """Reset the gyro""" + self.gyro.reset() + + def periodic(self) -> None: + """This method will be called once per scheduler run""" diff --git a/examples/robot/check_header.py b/examples/robot/check_header.py new file mode 100755 index 000000000..66e39e34b --- /dev/null +++ b/examples/robot/check_header.py @@ -0,0 +1,79 @@ +#!/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. +# + +from pathlib import Path + + +def check_file_content(file_path): + with open(file_path, "r") as file: + lines = file.readlines() + + if file.name.endswith("robot.py"): + expected_lines = [ + "#!/usr/bin/env python3\n", + "#\n", + "# Copyright (c) FIRST and other WPILib contributors.\n", + "# Open Source Software; you can modify and/or share it under the terms of\n", + "# the WPILib BSD license file in the root directory of this project.\n", + "#\n", + "\n", + ] + else: + expected_lines = [ + "#\n", + "# Copyright (c) FIRST and other WPILib contributors.\n", + "# Open Source Software; you can modify and/or share it under the terms of\n", + "# the WPILib BSD license file in the root directory of this project.\n", + "#\n", + "\n", + ] + + if lines[: len(expected_lines)] != expected_lines: + print( + "\n".join( + [ + f"{file_path}", + "ERROR: File must start with the following lines", + "------------------------------", + "".join(expected_lines[:-1]), + "------------------------------", + "Found:", + "".join(lines[: len(expected_lines)]), + "------------------------------", + ] + ) + ) + + return False + return True + + +def main(): + current_directory = Path(__file__).parent + python_files = [ + x + for x in current_directory.glob("./**/*.py") + if not x.parent == current_directory and x.stat().st_size != 0 + ] + + non_compliant_files = [ + file for file in python_files if not check_file_content(file) + ] + + non_compliant_files.sort() + + if non_compliant_files: + print("Non-compliant files:") + for file in non_compliant_files: + print(f"- {file}") + exit(1) # Exit with an error code + else: + print("All files are compliant.") + + +if __name__ == "__main__": + main() diff --git a/examples/robot/examples.toml b/examples/robot/examples.toml new file mode 100644 index 000000000..20ceae317 --- /dev/null +++ b/examples/robot/examples.toml @@ -0,0 +1,51 @@ +[tests] +base = [ + "AddressableLED", + "AprilTagsVision", + "ArcadeDrive", + "ArcadeDriveXboxController", + "ArmSimulation", + "CANPDP", + "DifferentialDriveBot", + "DigitalCommunication", + "DriveDistanceOffboard", + "DutyCycleEncoder", + "DutyCycleInput", + "ElevatorProfiledPID", + "ElevatorSimulation", + "ElevatorTrapezoidProfile", + "Encoder", + "EventLoop", + "FlywheelBangBangController", + "GettingStarted", + "Gyro", + "GyroMecanum", + "HatchbotInlined", + "HatchbotTraditional", + "HidRumble", + "HttpCamera", + "I2CCommunication", + "IntermediateVision", + "MecanumBot", + "MecanumDrive", + "Mechanism2d", + "MotorControl", + "PotentiometerPID", + "QuickVision", + "RomiReference", + "SelectCommand", + "SimpleDifferentialDriveSimulation", + "Solenoid", + "StateSpaceArm", + "StateSpaceElevator", + "StateSpaceFlywheel", + "StateSpaceFlywheelSysId", + "SwerveBot", + "SysId", + "TankDrive", + "TankDriveXboxController", + "XrpReference", +] + +ignored = [ +] diff --git a/subprojects/robotpy-wpilib/tests/test_opmode_robot.py b/subprojects/robotpy-wpilib/tests/test_opmode_robot.py index abc04214d..350b8906e 100644 --- a/subprojects/robotpy-wpilib/tests/test_opmode_robot.py +++ b/subprojects/robotpy-wpilib/tests/test_opmode_robot.py @@ -135,6 +135,7 @@ def __init__(self): assert options[0].name == "OneArgOpMode" +@pytest.mark.xfail(reason="wpilib bug") def test_none_periodic(): class MyMockRobot(MockRobot): def __init__(self):