├── FrisbeeBot ├── subsystems │ ├── __init__.py │ └── shootersubsystem.py ├── constants.py └── robot.py ├── HatchbotInlined ├── subsystems │ ├── __init__.py │ ├── hatchsubsystem.py │ └── drivesubsystem.py ├── constants.py └── robot.py ├── MagicbotSimple ├── autonomous │ ├── __init__.py │ └── two_steps.py ├── components │ ├── __init__.py │ ├── component1.py │ └── component2.py ├── tests │ └── pyfrc_test.py └── robot.py ├── RamseteCommand ├── subsystems │ └── __init__.py ├── constants.py └── robot.py ├── HatchbotTraditional ├── subsystems │ ├── __init__.py │ ├── hatchsubsystem.py │ └── drivesubsystem.py ├── commands │ ├── releasehatch.py │ ├── grabhatch.py │ ├── halvedrivespeed.py │ ├── defaultdrive.py │ ├── drivedistance.py │ └── complexauto.py ├── constants.py └── robot.py ├── StatefulAutonomous ├── autonomous │ ├── __init__.py │ ├── drive_forward.py │ ├── drive_backwards.py │ └── feature_example.py ├── tests │ ├── pyfrc_test.py │ └── autonomous_test.py └── robot.py ├── PhysicsDriveCharacterization ├── .gitignore └── README.md ├── GameData ├── gamedata.md └── robot.py ├── .gitignore ├── .pre-commit-config.yaml ├── Timed ├── tests │ ├── pyfrc_test.py │ └── basic_test.py └── src │ └── robot.py ├── PhysicsCamSim └── tests │ └── pyfrc_test.py ├── SchedulerEventLogging ├── constants.py └── robot.py ├── HttpCamera ├── robot.py └── vision.py ├── IntermediateVision ├── robot.py └── vision.py ├── QuickVision └── robot.py ├── XrpReference ├── subsystems │ └── arm.py └── commands │ ├── autonomous_distance.py │ ├── autonomous_time.py │ ├── arcadedrive.py │ ├── drivedistance.py │ ├── turntime.py │ ├── drivetime.py │ └── turndegrees.py ├── DutyCycleInput └── robot.py ├── .github └── workflows │ └── test.yml ├── AprilTagsVision └── robot.py ├── ArmSimulation ├── robot.py ├── constants.py └── subsytems │ └── arm.py ├── HidRumble └── robot.py ├── RomiReference └── commands │ ├── autonomous_distance.py │ ├── autonomous_time.py │ ├── arcadedrive.py │ ├── drivedistance.py │ ├── turntime.py │ ├── drivetime.py │ └── turndegrees.py ├── SysId ├── constants.py ├── robot.py └── sysidroutinebot.py ├── DriveDistanceOffboard ├── constants.py ├── commands │ └── drivedistanceprofiled.py ├── examplesmartmotorcontroller.py └── robot.py ├── TankDrive └── robot.py ├── ArcadeDrive └── robot.py ├── ElevatorProfiledPID └── robot.py ├── GyroDriveCommands ├── constants.py ├── commands │ ├── turntoangle.py │ └── turntoangleprofiled.py └── robot.py ├── ArmBotOffboard ├── constants.py ├── subsystems │ └── armsubsystem.py ├── robot.py ├── robotcontainer.py └── examplesmartmotorcontroller.py ├── ArcadeDriveXboxController └── robot.py ├── README.md ├── TankDriveXboxController └── robot.py ├── DifferentialDriveBot └── robot.py ├── MotorControl └── robot.py ├── AddressableLED └── robot.py ├── Physics4Wheel └── src │ └── robot.py ├── MecanumDrive └── robot.py ├── ArmBot ├── constants.py └── subsystems │ └── armsubsystem.py ├── DigitalCommunication └── robot.py ├── GyroMecanum └── robot.py ├── Physics └── src │ └── robot.py ├── PhysicsSPI └── src │ └── robot.py ├── ElevatorSimulation └── robot.py ├── Relay └── robot.py ├── ElevatorTrapezoidProfile ├── robot.py └── examplesmartmotorcontroller.py ├── Gyro └── robot.py ├── Ultrasonic └── robot.py ├── I2CCommunication └── robot.py ├── FlywheelBangBangController └── physics.py ├── Mechanism2d └── robot.py ├── MecanumBot └── robot.py ├── GettingStarted └── robot.py ├── UltrasonicPID └── robot.py ├── CANPDP └── robot.py ├── PhysicsMecanum └── src │ ├── robot.py │ └── physics.py ├── PotentiometerPID └── robot.py ├── SelectCommand ├── robot.py └── robotcontainer.py ├── run_tests.sh ├── SwerveBot └── robot.py ├── MecanumDriveXbox └── robot.py ├── Encoder └── robot.py └── check_header.py /FrisbeeBot/subsystems/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /HatchbotInlined/subsystems/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /MagicbotSimple/autonomous/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /MagicbotSimple/components/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /RamseteCommand/subsystems/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /HatchbotTraditional/subsystems/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /StatefulAutonomous/autonomous/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PhysicsDriveCharacterization/.gitignore: -------------------------------------------------------------------------------- 1 | *-data.json 2 | *.csv 3 | -------------------------------------------------------------------------------- /PhysicsDriveCharacterization/README.md: -------------------------------------------------------------------------------- 1 | Robot Drivetrain Characterization 2 | ================================= 3 | 4 | This has been moved to its own separate repository at https://github.com/robotpy/robot-characterization 5 | -------------------------------------------------------------------------------- /GameData/gamedata.md: -------------------------------------------------------------------------------- 1 | # Game Data 2 | 3 | ## 2020 - Infinite Recharge 4 | 5 | Game data used to send alliances which color to use for position control with the control panel 6 | 7 | - `R`: Red 8 | - `B`: Blue 9 | - `G`: Green 10 | - `Y`: Yellow 11 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.py[ocd] 2 | __pycache__ 3 | .coverage 4 | .cache 5 | 6 | .vscode/ 7 | .deploy_cfg 8 | deploy.json 9 | 10 | .project 11 | .pydevproject 12 | 13 | pyproject.toml 14 | wpilib_preferences.json 15 | 16 | imgui.ini 17 | simgui*.json 18 | 19 | opkg_cache 20 | pip_cache 21 | 22 | networktables.* -------------------------------------------------------------------------------- /MagicbotSimple/components/component1.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | 8 | class Component1: 9 | def execute(self): 10 | pass 11 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | repos: 3 | - repo: https://github.com/psf/black-pre-commit-mirror 4 | rev: 24.10.0 5 | hooks: 6 | - id: black 7 | - repo: local 8 | hooks: 9 | - id: check-headers 10 | name: check headers 11 | language: script 12 | entry: check_header.py 13 | pass_filenames: false 14 | require_serial: true 15 | types: [python] 16 | -------------------------------------------------------------------------------- /Timed/tests/pyfrc_test.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | This test module imports tests that come with pyfrc, and can be used 9 | to test basic functionality of just about any robot. 10 | """ 11 | 12 | from pyfrc.tests import * 13 | -------------------------------------------------------------------------------- /PhysicsCamSim/tests/pyfrc_test.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | This test module imports tests that come with pyfrc, and can be used 9 | to test basic functionality of just about any robot. 10 | """ 11 | 12 | from pyfrc.tests import * 13 | -------------------------------------------------------------------------------- /StatefulAutonomous/tests/pyfrc_test.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | This test module imports tests that come with pyfrc, and can be used 9 | to test basic functionality of just about any robot. 10 | """ 11 | 12 | from pyfrc.tests import * 13 | -------------------------------------------------------------------------------- /MagicbotSimple/tests/pyfrc_test.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | This test module imports tests that come with pyfrc, and can be used 9 | to test basic functionality of just about any robot. 10 | """ 11 | 12 | from pyfrc.tests import * 13 | from magicbot.magicbot_tests import * 14 | -------------------------------------------------------------------------------- /HatchbotTraditional/commands/releasehatch.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from subsystems.hatchsubsystem import HatchSubsystem 10 | 11 | 12 | class ReleaseHatch(commands2.InstantCommand): 13 | def __init__(self, hatch: HatchSubsystem) -> None: 14 | super().__init__(hatch.releaseHatch, hatch) 15 | -------------------------------------------------------------------------------- /StatefulAutonomous/tests/autonomous_test.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | This test module imports tests that come with robotpy-wpilib-utilities, 9 | and can be used to test basic functionality of robots that use the 10 | autonomous framework. 11 | """ 12 | 13 | from robotpy_ext.autonomous.selector_tests import * 14 | -------------------------------------------------------------------------------- /SchedulerEventLogging/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | A place for the constant values in the code that may be used in more than one place. 9 | This offers a convenient resources to teams who need to make both quick and universal 10 | changes. 11 | """ 12 | 13 | 14 | class OIConstants: 15 | # Example: the port of the driver's controller 16 | kDriverControllerPort = 0 17 | -------------------------------------------------------------------------------- /HatchbotTraditional/commands/grabhatch.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | from subsystems.hatchsubsystem import HatchSubsystem 9 | 10 | 11 | class GrabHatch(commands2.Command): 12 | def __init__(self, hatch: HatchSubsystem) -> None: 13 | super().__init__() 14 | self.hatch = hatch 15 | self.addRequirements(hatch) 16 | 17 | def initialize(self) -> None: 18 | self.hatch.grabHatch() 19 | 20 | def isFinished(self) -> bool: 21 | return True 22 | -------------------------------------------------------------------------------- /HatchbotTraditional/commands/halvedrivespeed.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from subsystems.drivesubsystem import DriveSubsystem 10 | 11 | 12 | class HalveDriveSpeed(commands2.Command): 13 | def __init__(self, drive: DriveSubsystem) -> None: 14 | super().__init__() 15 | self.drive = drive 16 | 17 | def initialize(self) -> None: 18 | self.drive.setMaxOutput(0.5) 19 | 20 | def end(self, interrupted: bool) -> None: 21 | self.drive.setMaxOutput(1.0) 22 | -------------------------------------------------------------------------------- /HttpCamera/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | 9 | import wpilib 10 | import wpilib.cameraserver 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | def robotInit(self): 15 | # Your image processing code will be launched via a stub that will set up logging and initialize NetworkTables 16 | # to talk to your robot code. 17 | # https://robotpy.readthedocs.io/en/stable/vision/roborio.html#important-notes 18 | 19 | wpilib.CameraServer.launch("vision.py:main") 20 | -------------------------------------------------------------------------------- /IntermediateVision/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """ 13 | This is a demo program showing the use of OpenCV to do vision processing. The image is acquired 14 | from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has 15 | many methods for different types of processing. 16 | """ 17 | 18 | def robotInit(self): 19 | wpilib.CameraServer.launch("vision.py:main") 20 | -------------------------------------------------------------------------------- /QuickVision/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | from wpilib.cameraserver import CameraServer 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | Uses the CameraServer class to automatically capture video from a USB webcam and send it to the 15 | FRC dashboard without doing any vision processing. This is the easiest way to get camera images 16 | to the dashboard. Just add this to the robotInit() method in your program. 17 | """ 18 | 19 | def robotInit(self): 20 | CameraServer().launch() 21 | -------------------------------------------------------------------------------- /XrpReference/subsystems/arm.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import xrp 9 | 10 | 11 | class Arm(commands2.Subsystem): 12 | 13 | def __init__(self): 14 | """Creates a new Arm.""" 15 | 16 | # Device number 4 maps to the physical Servo 1 port on the XRP 17 | self.armServo = xrp.XRPServo(4) 18 | 19 | def periodic(self): 20 | """This method will be called once per scheduler run""" 21 | 22 | def setAngle(self, angleDeg: float): 23 | """ 24 | Set the current angle of the arm (0 - 180 degrees). 25 | 26 | :param angleDeg: Desired arm angle in degrees 27 | """ 28 | self.armServo.setAngle(angleDeg) 29 | -------------------------------------------------------------------------------- /MagicbotSimple/autonomous/two_steps.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | from magicbot import AutonomousStateMachine, tunable, timed_state 8 | 9 | from components.component2 import Component2 10 | 11 | 12 | class TwoSteps(AutonomousStateMachine): 13 | MODE_NAME = "Two Steps" 14 | DEFAULT = True 15 | 16 | component2: Component2 17 | 18 | drive_speed = tunable(-1) 19 | 20 | @timed_state(duration=2, next_state="do_something", first=True) 21 | def dont_do_something(self): 22 | """This happens first""" 23 | pass 24 | 25 | @timed_state(duration=5) 26 | def do_something(self): 27 | """This happens second""" 28 | self.component2.do_something() 29 | -------------------------------------------------------------------------------- /HatchbotTraditional/commands/defaultdrive.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import typing 8 | import commands2 9 | from subsystems.drivesubsystem import DriveSubsystem 10 | 11 | 12 | class DefaultDrive(commands2.Command): 13 | def __init__( 14 | self, 15 | drive: DriveSubsystem, 16 | forward: typing.Callable[[], float], 17 | rotation: typing.Callable[[], float], 18 | ) -> None: 19 | super().__init__() 20 | 21 | self.drive = drive 22 | self.forward = forward 23 | self.rotation = rotation 24 | 25 | self.addRequirements(self.drive) 26 | 27 | def execute(self) -> None: 28 | self.drive.arcadeDrive(self.forward(), self.rotation()) 29 | -------------------------------------------------------------------------------- /DutyCycleInput/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | def robotInit(self): 13 | """Robot initialization function""" 14 | 15 | self.dutyCycle = wpilib.DutyCycle(wpilib.DigitalInput(0)) 16 | 17 | def robotPeriodic(self): 18 | # Duty Cycle Frequency in Hz 19 | frequency = self.dutyCycle.getFrequency() 20 | 21 | # Output of duty cycle, ranging from 0 to 1 22 | # 1 is fully on, 0 is fully off 23 | output = self.dutyCycle.getOutput() 24 | 25 | wpilib.SmartDashboard.putNumber("Frequency", frequency) 26 | wpilib.SmartDashboard.putNumber("Duty Cycle", output) 27 | -------------------------------------------------------------------------------- /HatchbotTraditional/subsystems/hatchsubsystem.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | import commands2 9 | 10 | import constants 11 | 12 | 13 | class HatchSubsystem(commands2.Subsystem): 14 | def __init__(self) -> None: 15 | super().__init__() 16 | 17 | self.hatchSolenoid = wpilib.DoubleSolenoid( 18 | constants.kHatchSolenoidModule, 19 | constants.kHatchSolenoidModuleType, 20 | *constants.kHatchSolenoidPorts 21 | ) 22 | 23 | def grabHatch(self) -> None: 24 | """Grabs the hatch""" 25 | self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward) 26 | 27 | def releaseHatch(self) -> None: 28 | """Releases the hatch""" 29 | self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse) 30 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | --- 2 | name: dist 3 | 4 | on: 5 | pull_request: 6 | push: 7 | branches: 8 | - main 9 | - '2027' 10 | tags: 11 | - '*' 12 | workflow_dispatch: 13 | 14 | jobs: 15 | check: 16 | runs-on: ubuntu-latest 17 | steps: 18 | - uses: actions/checkout@v5 19 | - uses: pre-commit/action@v3.0.1 20 | 21 | test: 22 | runs-on: ${{ matrix.os }} 23 | strategy: 24 | matrix: 25 | os: ["ubuntu-22.04", "macos-14", "windows-2022"] 26 | python_version: 27 | - '3.9' 28 | - '3.10' 29 | - '3.11' 30 | - '3.12' 31 | - '3.13' 32 | 33 | steps: 34 | - uses: actions/checkout@v5 35 | - uses: actions/setup-python@v5 36 | with: 37 | python-version: ${{ matrix.python_version }} 38 | - name: Install deps 39 | run: | 40 | pip install 'robotpy[commands2,romi]<2026.0.0,>=2025.0.0b3' numpy pytest 41 | - name: Run tests 42 | run: bash run_tests.sh 43 | shell: bash 44 | -------------------------------------------------------------------------------- /AprilTagsVision/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | 9 | import wpilib 10 | import wpilib.cameraserver 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | """ 15 | This is a demo program showing the detection of AprilTags. The image is acquired from the USB 16 | camera, then any detected AprilTags are marked up on the image and sent to the dashboard. 17 | Be aware that the performance on this is much worse than a coprocessor solution!""" 18 | 19 | def robotInit(self): 20 | # Your image processing code will be launched via a stub that will set up logging and initialize NetworkTables 21 | # to talk to your robot code. 22 | # https://robotpy.readthedocs.io/en/stable/vision/roborio.html#important-notes 23 | 24 | wpilib.CameraServer.launch("vision.py:main") 25 | -------------------------------------------------------------------------------- /ArmSimulation/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | from subsytems.arm import Arm 11 | from constants import Constants 12 | 13 | 14 | class MyRobot(wpilib.TimedRobot): 15 | def robotInit(self): 16 | self.arm = Arm() 17 | self.joystick = wpilib.Joystick(Constants.kJoystickPort) 18 | 19 | def teleopInit(self): 20 | self.arm.loadPreferences() 21 | 22 | def teleopPeriodic(self): 23 | if self.joystick.getTrigger(): 24 | # Here, we run PID control like normal. 25 | self.arm.reachSetpoint() 26 | else: 27 | # Otherwise, we disable the motor. 28 | self.arm.stop() 29 | 30 | def disabledInit(self): 31 | # This just makes sure that our simulation code knows that the motor's off. 32 | self.arm.stop() 33 | -------------------------------------------------------------------------------- /ArmSimulation/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import math 8 | 9 | from wpimath import units 10 | 11 | 12 | class Constants: 13 | kMotorPort = 0 14 | kEncoderAChannel = 0 15 | kEncoderBChannel = 1 16 | kJoystickPort = 0 17 | 18 | kArmPositionKey = "ArmPosition" 19 | kArmPKey = "ArmP" 20 | 21 | # The P gain for the PID controller that drives this arm. 22 | kDefaultArmKp = 50.0 23 | kDefaultArmSetpointDegrees = 75.0 24 | 25 | # distance per pulse = (angle per revolution) / (pulses per revolution) 26 | # = (2 * PI rads) / (4096 pulses) 27 | kArmEncoderDistPerPulse = 2.0 * math.pi / 4096 28 | 29 | kArmReduction = 200 30 | kArmMass = 8.0 # Kilograms 31 | kArmLength = units.inchesToMeters(30) 32 | kMinAngleRads = units.degreesToRadians(-75) 33 | kMaxAngleRads = units.degreesToRadians(255) 34 | -------------------------------------------------------------------------------- /HidRumble/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """ 13 | This is a demo program showing the use of GenericHID's rumble feature. 14 | """ 15 | 16 | def robotInit(self): 17 | """Robot initialization function""" 18 | 19 | self.hid = wpilib.XboxController(0) 20 | 21 | def autonomousInit(self): 22 | # Turn on rumble at the start of auto 23 | self.hid.setRumble(wpilib.XboxController.RumbleType.kLeftRumble, 1.0) 24 | self.hid.setRumble(wpilib.XboxController.RumbleType.kRightRumble, 1.0) 25 | 26 | def disabledInit(self): 27 | # Stop the rumble when entering disabled 28 | self.hid.setRumble(wpilib.XboxController.RumbleType.kLeftRumble, 0.0) 29 | self.hid.setRumble(wpilib.XboxController.RumbleType.kRightRumble, 0.0) 30 | -------------------------------------------------------------------------------- /StatefulAutonomous/autonomous/drive_forward.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | from robotpy_ext.autonomous import StatefulAutonomous, state, timed_state 8 | 9 | 10 | class DriveForward(StatefulAutonomous): 11 | MODE_NAME = "Drive Forward" 12 | 13 | def initialize(self): 14 | # This allows you to tune the variable via the SmartDashboard over 15 | # networktables 16 | self.register_sd_var("drive_speed", 1) 17 | 18 | @timed_state(duration=0.5, next_state="drive_forward", first=True) 19 | def drive_wait(self): 20 | self.drive.tankDrive(0, 0) 21 | 22 | @timed_state(duration=5, next_state="stop") 23 | def drive_forward(self): 24 | self.drive.tankDrive(self.drive_speed, -1 * (self.drive_speed)) 25 | 26 | @state() # Remove or modify this to add additional states to this class. 27 | def stop(self): 28 | self.drive.tankDrive(0, 0) 29 | -------------------------------------------------------------------------------- /RomiReference/commands/autonomous_distance.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from commands.drivedistance import DriveDistance 10 | from commands.turndegrees import TurnDegrees 11 | from subsystems.drivetrain import Drivetrain 12 | 13 | 14 | class AutonomousDistance(commands2.SequentialCommandGroup): 15 | def __init__(self, drive: Drivetrain) -> None: 16 | """Creates a new Autonomous Drive based on distance. This will drive out for a specified distance, 17 | turn around and drive back. 18 | 19 | :param drivetrain: The drivetrain subsystem on which this command will run 20 | """ 21 | super().__init__() 22 | 23 | self.addCommands( 24 | DriveDistance(-0.5, 10, drive), 25 | TurnDegrees(-0.5, 180, drive), 26 | DriveDistance(-0.5, 10, drive), 27 | TurnDegrees(0.5, 180, drive), 28 | ) 29 | -------------------------------------------------------------------------------- /XrpReference/commands/autonomous_distance.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from commands.drivedistance import DriveDistance 10 | from commands.turndegrees import TurnDegrees 11 | from subsystems.drivetrain import Drivetrain 12 | 13 | 14 | class AutonomousDistance(commands2.SequentialCommandGroup): 15 | def __init__(self, drive: Drivetrain) -> None: 16 | """Creates a new Autonomous Drive based on distance. This will drive out for a specified distance, 17 | turn around and drive back. 18 | 19 | :param drivetrain: The drivetrain subsystem on which this command will run 20 | """ 21 | super().__init__() 22 | 23 | self.addCommands( 24 | DriveDistance(-0.5, 10, drive), 25 | TurnDegrees(-0.5, 180, drive), 26 | DriveDistance(-0.5, 10, drive), 27 | TurnDegrees(0.5, 180, drive), 28 | ) 29 | -------------------------------------------------------------------------------- /HatchbotTraditional/commands/drivedistance.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from subsystems.drivesubsystem import DriveSubsystem 10 | 11 | 12 | class DriveDistance(commands2.Command): 13 | def __init__(self, inches: float, speed: float, drive: DriveSubsystem) -> None: 14 | super().__init__() 15 | self.distance = inches 16 | self.speed = speed 17 | self.drive = drive 18 | self.addRequirements(drive) 19 | 20 | def initialize(self) -> None: 21 | self.drive.resetEncoders() 22 | self.drive.arcadeDrive(self.speed, 0) 23 | 24 | def execute(self) -> None: 25 | self.drive.arcadeDrive(self.speed, 0) 26 | 27 | def end(self, interrupted: bool) -> None: 28 | self.drive.arcadeDrive(0, 0) 29 | 30 | def isFinished(self) -> bool: 31 | return abs(self.drive.getAverageEncoderDistance()) >= self.distance 32 | -------------------------------------------------------------------------------- /StatefulAutonomous/autonomous/drive_backwards.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | from robotpy_ext.autonomous import StatefulAutonomous, state, timed_state 8 | 9 | 10 | class DriveBackwards(StatefulAutonomous): 11 | MODE_NAME = "Drive Backwards" 12 | 13 | def initialize(self): 14 | # This allows you to tune the variable via the SmartDashboard over 15 | # networktables 16 | self.register_sd_var("drive_speed", -1) 17 | 18 | @timed_state(duration=0.5, next_state="drive_backwards", first=True) 19 | def drive_wait(self): 20 | self.drive.tankDrive(0, 0) 21 | 22 | @timed_state(duration=5, next_state="stop") 23 | def drive_backwards(self): 24 | self.drive.tankDrive(self.drive_speed, -1 * (self.drive_speed)) 25 | 26 | @state() # Remove or modify this to add additional states to this class. 27 | def stop(self): 28 | self.drive.tankDrive(0, 0) 29 | -------------------------------------------------------------------------------- /SysId/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import math 8 | 9 | 10 | class DriveConstants: 11 | # The PWM IDs for the drivetrain motor controllers. 12 | kLeftMotor1Port = 0 13 | kLeftMotor2Port = 1 14 | kRightMotor1Port = 2 15 | kRightMotor2Port = 3 16 | 17 | # Encoders and their respective motor controllers. 18 | kLeftEncoderPorts = (0, 1) 19 | kRightEncoderPorts = (2, 3) 20 | kLeftEncoderReversed = False 21 | kRightEncoderReversed = True 22 | 23 | # Encoder counts per revolution/rotation. 24 | kEncoderCPR = 1024.0 25 | kWheelDiameterInches = 6.0 26 | 27 | # Assumes the encoders are directly mounted on the wheel shafts 28 | kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR 29 | 30 | 31 | # autonomous 32 | class AutonomousConstants: 33 | kTimeoutSeconds = 3.0 34 | kDriveDistanceMetres = 2.0 35 | kDriveSpeed = 0.5 36 | 37 | 38 | class OIConstants: 39 | kDriverControllerPort = 0 40 | -------------------------------------------------------------------------------- /HatchbotInlined/subsystems/hatchsubsystem.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | import commands2 9 | import commands2.cmd 10 | 11 | import constants 12 | 13 | 14 | class HatchSubsystem(commands2.Subsystem): 15 | def __init__(self) -> None: 16 | super().__init__() 17 | 18 | self.hatchSolenoid = wpilib.DoubleSolenoid( 19 | constants.kHatchSolenoidModule, 20 | constants.kHatchSolenoidModuleType, 21 | *constants.kHatchSolenoidPorts 22 | ) 23 | 24 | def grabHatch(self) -> commands2.Command: 25 | """Grabs the hatch""" 26 | return commands2.cmd.runOnce( 27 | lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kForward), self 28 | ) 29 | 30 | def releaseHatch(self) -> commands2.Command: 31 | """Releases the hatch""" 32 | return commands2.cmd.runOnce( 33 | lambda: self.hatchSolenoid.set(wpilib.DoubleSolenoid.Value.kReverse), self 34 | ) 35 | -------------------------------------------------------------------------------- /RomiReference/commands/autonomous_time.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from commands.drivetime import DriveTime 10 | from commands.turntime import TurnTime 11 | from subsystems.drivetrain import Drivetrain 12 | 13 | 14 | class AutonomousTime(commands2.SequentialCommandGroup): 15 | def __init__(self, drive: Drivetrain) -> None: 16 | """Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn 17 | around for time (equivalent to time to turn around) and drive forward again. This should mimic 18 | driving out, turning around and driving back. 19 | 20 | :param drivetrain: The drive subsystem on which this command will run 21 | 22 | """ 23 | super().__init__() 24 | 25 | self.addCommands( 26 | DriveTime(-0.6, 2.0, drive), 27 | TurnTime(-0.5, 1.3, drive), 28 | DriveTime(-0.6, 2.0, drive), 29 | TurnTime(0.5, 1.3, drive), 30 | ) 31 | -------------------------------------------------------------------------------- /XrpReference/commands/autonomous_time.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from commands.drivetime import DriveTime 10 | from commands.turntime import TurnTime 11 | from subsystems.drivetrain import Drivetrain 12 | 13 | 14 | class AutonomousTime(commands2.SequentialCommandGroup): 15 | def __init__(self, drive: Drivetrain) -> None: 16 | """Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn 17 | around for time (equivalent to time to turn around) and drive forward again. This should mimic 18 | driving out, turning around and driving back. 19 | 20 | :param drivetrain: The drive subsystem on which this command will run 21 | 22 | """ 23 | super().__init__() 24 | 25 | self.addCommands( 26 | DriveTime(-0.6, 2.0, drive), 27 | TurnTime(-0.5, 1.3, drive), 28 | DriveTime(-0.6, 2.0, drive), 29 | TurnTime(0.5, 1.3, drive), 30 | ) 31 | -------------------------------------------------------------------------------- /DriveDistanceOffboard/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | A place for the constant values in the code that may be used in more than one place. 9 | This offers a convenient resources to teams who need to make both quick and universal 10 | changes. 11 | """ 12 | 13 | import math 14 | 15 | 16 | class DriveConstants: 17 | kLeftMotor1Port = 0 18 | kLeftMotor2Port = 1 19 | kRightMotor1Port = 2 20 | kRightMotor2Port = 3 21 | 22 | """ 23 | These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! 24 | These characterization values MUST be determined either experimentally or theoretically 25 | for *your* robot's drive. 26 | The SysId tool provides a convenient method for obtaining these values for your robot. 27 | """ 28 | ksVolts = 1 29 | kvVoltSecondsPerMeter = 0.8 30 | kaVoltSecondsSquaredPerMeter = 0.15 31 | 32 | kp = 1 33 | 34 | kMaxSpeedMetersPerSecond = 3 35 | kMaxAccelerationMetersPerSecondSquared = 3 36 | 37 | 38 | class OIConstants: 39 | kDriverControllerPort = 0 40 | -------------------------------------------------------------------------------- /TankDrive/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a demo program showing the use of the DifferentialDrive class. 15 | Runs the motors with tank steering. 16 | """ 17 | 18 | def robotInit(self): 19 | """Robot initialization function""" 20 | 21 | leftMotor = wpilib.PWMSparkMax(0) 22 | rightMotor = wpilib.PWMSparkMax(1) 23 | self.robotDrive = wpilib.drive.DifferentialDrive(leftMotor, rightMotor) 24 | self.leftStick = wpilib.Joystick(0) 25 | self.rightStick = wpilib.Joystick(1) 26 | 27 | # We need to invert one side of the drivetrain so that positive voltages 28 | # result in both sides moving forward. Depending on how your robot's 29 | # gearbox is constructed, you might have to invert the left side instead. 30 | rightMotor.setInverted(True) 31 | 32 | def teleopPeriodic(self): 33 | self.robotDrive.tankDrive(-self.leftStick.getY(), -self.rightStick.getY()) 34 | -------------------------------------------------------------------------------- /HatchbotTraditional/commands/complexauto.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | import constants 10 | 11 | from .drivedistance import DriveDistance 12 | from .releasehatch import ReleaseHatch 13 | 14 | from subsystems.drivesubsystem import DriveSubsystem 15 | from subsystems.hatchsubsystem import HatchSubsystem 16 | 17 | 18 | class ComplexAuto(commands2.SequentialCommandGroup): 19 | """ 20 | A complex auto command that drives forward, releases a hatch, and then drives backward. 21 | """ 22 | 23 | def __init__(self, drive: DriveSubsystem, hatch: HatchSubsystem): 24 | super().__init__( 25 | # Drive forward the specified distance 26 | DriveDistance( 27 | constants.kAutoDriveDistanceInches, constants.kAutoDriveSpeed, drive 28 | ), 29 | # Release the hatch 30 | ReleaseHatch(hatch), 31 | # Drive backward the specified distance 32 | DriveDistance( 33 | constants.kAutoBackupDistanceInches, -constants.kAutoDriveSpeed, drive 34 | ), 35 | ) 36 | -------------------------------------------------------------------------------- /RomiReference/commands/arcadedrive.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import typing 8 | import commands2 9 | from subsystems.drivetrain import Drivetrain 10 | 11 | 12 | class ArcadeDrive(commands2.Command): 13 | def __init__( 14 | self, 15 | drive: Drivetrain, 16 | forward: typing.Callable[[], float], 17 | rotation: typing.Callable[[], float], 18 | ) -> None: 19 | """Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier 20 | lambdas. This command does not terminate. 21 | 22 | :param drivetrain: The drivetrain subsystem on which this command will run 23 | :param forward: Callable supplier of forward/backward speed 24 | :param rotation: Callable supplier of rotational speed 25 | """ 26 | super().__init__() 27 | 28 | self.drive = drive 29 | self.forward = forward 30 | self.rotation = rotation 31 | 32 | self.addRequirements(self.drive) 33 | 34 | def execute(self) -> None: 35 | self.drive.arcadeDrive(self.forward(), self.rotation()) 36 | -------------------------------------------------------------------------------- /ArcadeDrive/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a demo program showing the use of the DifferentialDrive class. 15 | Runs the motors with arcade steering. 16 | """ 17 | 18 | def robotInit(self): 19 | """Robot initialization function""" 20 | 21 | leftMotor = wpilib.PWMSparkMax(0) 22 | rightMotor = wpilib.PWMSparkMax(1) 23 | self.robotDrive = wpilib.drive.DifferentialDrive(leftMotor, rightMotor) 24 | self.stick = wpilib.Joystick(0) 25 | 26 | # We need to invert one side of the drivetrain so that positive voltages 27 | # result in both sides moving forward. Depending on how your robot's 28 | # gearbox is constructed, you might have to invert the left side instead. 29 | rightMotor.setInverted(True) 30 | 31 | def teleopPeriodic(self): 32 | # Drive with arcade drive. 33 | # That means that the Y axis drives forward 34 | # and backward, and the X turns left and right. 35 | self.robotDrive.arcadeDrive(self.stick.getY(), self.stick.getX()) 36 | -------------------------------------------------------------------------------- /MagicbotSimple/components/component2.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | from .component1 import Component1 9 | 10 | from magicbot import feedback, will_reset_to 11 | 12 | 13 | class Component2: 14 | component1: Component1 15 | some_motor: wpilib.Talon 16 | 17 | # This is changed to the value in robot.py 18 | SOME_CONSTANT: int 19 | 20 | # This gets reset after each invocation of execute() 21 | did_something = will_reset_to(False) 22 | 23 | def on_enable(self): 24 | """Called when the robot enters teleop or autonomous mode""" 25 | self.logger.info( 26 | "Robot is enabled: I have SOME_CONSTANT=%s", self.SOME_CONSTANT 27 | ) 28 | 29 | def do_something(self): 30 | self.did_something = True 31 | 32 | # Use @feedback to send state external to the robot code to NetworkTables. 33 | # This will be called after execute(). 34 | @feedback 35 | def get_motor_voltage(self) -> float: 36 | return self.some_motor.getVoltage() 37 | 38 | def execute(self): 39 | if self.did_something: 40 | self.some_motor.set(1) 41 | else: 42 | self.some_motor.set(0) 43 | -------------------------------------------------------------------------------- /XrpReference/commands/arcadedrive.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import typing 8 | import commands2 9 | from subsystems.drivetrain import Drivetrain 10 | 11 | 12 | class ArcadeDrive(commands2.Command): 13 | def __init__( 14 | self, 15 | drive: Drivetrain, 16 | xaxisSpeedSupplier: typing.Callable[[], float], 17 | zaxisRotateSupplier: typing.Callable[[], float], 18 | ) -> None: 19 | """Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier 20 | lambdas. This command does not terminate. 21 | 22 | :param drivetrain: The drivetrain subsystem on which this command will run 23 | :param xaxisSpeedSupplier: Callable supplier of forward/backward speed 24 | :param zaxisRotateSupplier: Callable supplier of rotational speed 25 | """ 26 | super().__init__() 27 | 28 | self.drive = drive 29 | self.xaxisSpeedSupplier = xaxisSpeedSupplier 30 | self.zaxisRotateSupplier = zaxisRotateSupplier 31 | 32 | self.addRequirements(self.drive) 33 | 34 | def execute(self) -> None: 35 | self.drive.arcadeDrive(self.xaxisSpeedSupplier(), self.zaxisRotateSupplier()) 36 | -------------------------------------------------------------------------------- /HatchbotInlined/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | # 8 | # The constants module is a convenience place for teams to hold robot-wide 9 | # numerical or boolean constants. Don't use this for any other purpose! 10 | # 11 | 12 | import math 13 | import wpilib 14 | 15 | # Motors 16 | kLeftMotor1Port = 0 17 | kLeftMotor2Port = 1 18 | kRightMotor1Port = 2 19 | kRightMotor2Port = 3 20 | 21 | # Encoders 22 | kLeftEncoderPorts = (0, 1) 23 | kRightEncoderPorts = (2, 3) 24 | kLeftEncoderReversed = False 25 | kRightEncoderReversed = True 26 | 27 | kEncoderCPR = 1024 28 | kWheelDiameterInches = 6 29 | # Assumes the encoders are directly mounted on the wheel shafts 30 | kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR 31 | 32 | # Hatch 33 | kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM 34 | kHatchSolenoidModule = 0 35 | kHatchSolenoidPorts = (0, 1) 36 | 37 | # Autonomous 38 | kAutoDriveDistanceInches = 60 39 | kAutoBackupDistanceInches = 20 40 | kAutoDriveSpeed = 0.5 41 | 42 | # Operator Interface 43 | kDriverControllerPort = 0 44 | 45 | # Physical parameters 46 | kDriveTrainMotorCount = 2 47 | kTrackWidth = 0.381 * 2 48 | kGearingRatio = 8 49 | kWheelRadius = 0.0508 50 | 51 | # kEncoderResolution = - 52 | -------------------------------------------------------------------------------- /HatchbotTraditional/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | # 8 | # The constants module is a convenience place for teams to hold robot-wide 9 | # numerical or boolean constants. Don't use this for any other purpose! 10 | # 11 | 12 | import math 13 | import wpilib 14 | 15 | # Motors 16 | kLeftMotor1Port = 0 17 | kLeftMotor2Port = 1 18 | kRightMotor1Port = 2 19 | kRightMotor2Port = 3 20 | 21 | # Encoders 22 | kLeftEncoderPorts = (0, 1) 23 | kRightEncoderPorts = (2, 3) 24 | kLeftEncoderReversed = False 25 | kRightEncoderReversed = True 26 | 27 | kEncoderCPR = 1024 28 | kWheelDiameterInches = 6 29 | # Assumes the encoders are directly mounted on the wheel shafts 30 | kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR 31 | 32 | # Hatch 33 | kHatchSolenoidModuleType = wpilib.PneumaticsModuleType.CTREPCM 34 | kHatchSolenoidModule = 0 35 | kHatchSolenoidPorts = (0, 1) 36 | 37 | # Autonomous 38 | kAutoDriveDistanceInches = 60 39 | kAutoBackupDistanceInches = 20 40 | kAutoDriveSpeed = 0.5 41 | 42 | # Operator Interface 43 | kDriverControllerPort = 0 44 | 45 | # Physical parameters 46 | kDriveTrainMotorCount = 2 47 | kTrackWidth = 0.381 * 2 48 | kGearingRatio = 8 49 | kWheelRadius = 0.0508 50 | 51 | # kEncoderResolution = - 52 | -------------------------------------------------------------------------------- /ElevatorProfiledPID/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpimath.controller 10 | import wpimath.trajectory 11 | import math 12 | 13 | 14 | class MyRobot(wpilib.TimedRobot): 15 | kDt = 0.02 16 | 17 | def robotInit(self) -> None: 18 | self.joystick = wpilib.Joystick(1) 19 | self.encoder = wpilib.Encoder(1, 2) 20 | self.motor = wpilib.PWMSparkMax(1) 21 | 22 | # Create a PID controller whose setpoint's change is subject to maximum 23 | # velocity and acceleration constraints. 24 | self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75) 25 | self.controller = wpimath.controller.ProfiledPIDController( 26 | 1.3, 0, 0.7, self.constraints, self.kDt 27 | ) 28 | 29 | self.encoder.setDistancePerPulse(1 / 360 * 2 * math.pi * 1.5) 30 | 31 | def teleopPeriodic(self) -> None: 32 | if self.joystick.getRawButtonPressed(2): 33 | self.controller.setGoal(5) 34 | elif self.joystick.getRawButtonPressed(3): 35 | self.controller.setGoal(0) 36 | 37 | # Run controller and update motor output 38 | self.motor.set(self.controller.calculate(self.encoder.getDistance())) 39 | -------------------------------------------------------------------------------- /GyroDriveCommands/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | A place for the constant values in the code that may be used in more than one place. 9 | This offers a convenient resources to teams who need to make both quick and universal 10 | changes. 11 | """ 12 | 13 | import math 14 | 15 | 16 | class DriveConstants: 17 | kLeftMotor1Port = 0 18 | kLeftMotor2Port = 1 19 | kRightMotor1Port = 2 20 | kRightMotor2Port = 3 21 | 22 | kLeftEncoderPorts = (0, 1) 23 | kRightEncoderPorts = (2, 3) 24 | kLeftEncoderReversed = False 25 | kRightEncoderReversed = True 26 | 27 | kEncoderCPR = 1024 28 | kWheelDiameterInches = 6 29 | 30 | # Assumes the encoders are directly mounted on the wheel shafts 31 | kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR 32 | 33 | kGyroReversed = False 34 | 35 | kStabilizationP = 1 36 | kStabilizationI = 0.5 37 | kStabilizationD = 0 38 | 39 | kTurnP = 1 40 | kTurnI = 0 41 | kTurnD = 0 42 | 43 | kMaxTurnRateDegPerS = 100 44 | kMaxTurnAccelerationDegPerSSquared = 300 45 | 46 | kTurnToleranceDeg = 5 47 | kTurnRateToleranceDegPerS = 10 # degrees per second 48 | 49 | 50 | class OIConstants: 51 | kDriverControllerPort = 0 52 | -------------------------------------------------------------------------------- /MagicbotSimple/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | from magicbot import MagicRobot 10 | 11 | from components.component1 import Component1 12 | from components.component2 import Component2 13 | 14 | 15 | class MyRobot(MagicRobot): 16 | # 17 | # Define components here 18 | # 19 | 20 | component1: Component1 21 | component2: Component2 22 | 23 | # You can even pass constants to components 24 | SOME_CONSTANT = 1 25 | 26 | def createObjects(self): 27 | """Initialize all wpilib motors & sensors""" 28 | 29 | # TODO: create button example here 30 | 31 | self.component1_motor = wpilib.Talon(1) 32 | self.some_motor = wpilib.Talon(2) 33 | 34 | self.joystick = wpilib.Joystick(0) 35 | 36 | # 37 | # No autonomous routine boilerplate required here, anything in the 38 | # autonomous folder will automatically get added to a list 39 | # 40 | 41 | def teleopPeriodic(self): 42 | """Place code here that does things as a result of operator 43 | actions""" 44 | 45 | try: 46 | if self.joystick.getTrigger(): 47 | self.component2.do_something() 48 | except: 49 | self.onException() 50 | -------------------------------------------------------------------------------- /ArmBotOffboard/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | The constants module is a convenience place for teams to hold robot-wide 9 | numerical or boolean constants. Don't use this for any other purpose! 10 | """ 11 | 12 | import math 13 | 14 | # Drive 15 | 16 | # The PWM IDs for the drivetrain motor controllers. 17 | kLeftMotor1Port = 0 18 | kLeftMotor2Port = 1 19 | kRightMotor1Port = 2 20 | kRightMotor2Port = 3 21 | 22 | # Encoders and their respective motor controllers. 23 | kLeftEncoderPorts = (0, 1) 24 | kRightEncoderPorts = (2, 3) 25 | kLeftEncoderReversed = False 26 | kRightEncoderReversed = True 27 | 28 | # Encoder counts per revolution/rotation. 29 | kEncoderCPR = 1024 30 | kWheelDiameterInches = 6 31 | 32 | # Assumes the encoders are directly mounted on the wheel shafts 33 | kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR 34 | 35 | # Arm 36 | 37 | # NOTE: Please do NOT use these values on your robot. 38 | kMotorPort = 4 39 | kP = 1 40 | kSVolts = 1 41 | kGVolts = 1 42 | kVVoltSecondPerRad = 0.5 43 | kAVoltSecondSquaredPerRad = 0.1 44 | kMaxVelocityRadPerSecond = 3 45 | kMaxAccelerationRadPerSecSquared = 10 46 | 47 | # The offset of the arm from the horizontal in its neutral position, 48 | # measured from the horizontal 49 | kArmOffsetRads = 0.5 50 | 51 | # OIConstants 52 | kDriverControllerPort = 0 53 | -------------------------------------------------------------------------------- /ArcadeDriveXboxController/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a demo program showing the use of the DifferentialDrive class. 15 | Runs the motors with split arcade steering and an Xbox controller. 16 | """ 17 | 18 | def robotInit(self): 19 | """Robot initialization function""" 20 | 21 | leftMotor = wpilib.PWMSparkMax(0) 22 | rightMotor = wpilib.PWMSparkMax(1) 23 | self.robotDrive = wpilib.drive.DifferentialDrive(leftMotor, rightMotor) 24 | self.driverController = wpilib.XboxController(0) 25 | 26 | # We need to invert one side of the drivetrain so that positive voltages 27 | # result in both sides moving forward. Depending on how your robot's 28 | # gearbox is constructed, you might have to invert the left side instead. 29 | rightMotor.setInverted(True) 30 | 31 | def teleopPeriodic(self): 32 | # Drive with split arcade style 33 | # That means that the Y axis of the left stick moves forward 34 | # and backward, and the X of the right stick turns left and right. 35 | self.robotDrive.arcadeDrive( 36 | -self.driverController.getLeftY(), -self.driverController.getRightX() 37 | ) 38 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | RobotPy Example Code 2 | ==================== 3 | 4 | **NOTE**: The `main` branch contains examples that work with the latest version 5 | of RobotPy. While we don't change all that much every year, if you're looking 6 | specifically for examples for older years, there are branches for different 7 | years. If you are looking for examples that work with the current beta, there 8 | will be a YEAR-beta branch containing those examples. 9 | 10 | This repository contains lots of example code that can be used to learn more 11 | about creating robots using RobotPy. These examples are for core 12 | RobotPy/WPILib libraries. Examples specific to individual vendors can be 13 | found in their individual RobotPy repository under their 'examples' folder: 14 | 15 | * [Cross The Road Electronics (CTRE)](https://github.com/robotpy/robotpy-ctre/tree/main/examples) 16 | * [NavX](https://github.com/robotpy/robotpy-navx/tree/main/examples) 17 | * [REV Robotics](https://github.com/robotpy/robotpy-rev/tree/main/examples) 18 | 19 | For RobotPy documentation, go to https://robotpy.readthedocs.io/ 20 | 21 | Contributions 22 | ------------- 23 | 24 | WPILib (and the various vendors) have lots of examples that haven't been 25 | translated from C++/Java into Python yet. If there's an example that you 26 | found helpful for understanding how to write robot code, we'd love for you 27 | to take a shot at translating it into python and contributing to this 28 | repository! 29 | 30 | Please check out our [porting guide](CONTRIBUTING.md) for a variety of 31 | guidelines for new examples. 32 | -------------------------------------------------------------------------------- /TankDriveXboxController/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a demo program showing the use of the DifferentialDrive class. 15 | Runs the motors with tank steering and an Xbox controller. 16 | """ 17 | 18 | def robotInit(self): 19 | """Robot initialization function""" 20 | 21 | leftMotor = wpilib.PWMSparkMax(0) 22 | rightMotor = wpilib.PWMSparkMax(1) 23 | self.robotDrive = wpilib.drive.DifferentialDrive(leftMotor, rightMotor) 24 | self.driverController = wpilib.XboxController(0) 25 | 26 | # We need to invert one side of the drivetrain so that positive voltages 27 | # result in both sides moving forward. Depending on how your robot's 28 | # gearbox is constructed, you might have to invert the left side instead. 29 | rightMotor.setInverted(True) 30 | 31 | def teleopPeriodic(self): 32 | # Drive with tank drive. 33 | # That means that the Y axis of the left stick moves the left side 34 | # of the robot forward and backward, and the Y axis of the right stick 35 | # moves the right side of the robot forward and backward. 36 | self.robotDrive.tankDrive( 37 | -self.driverController.getLeftY(), -self.driverController.getRightY() 38 | ) 39 | -------------------------------------------------------------------------------- /GameData/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import ntcore 10 | 11 | PNUE_MOD_TYPE = wpilib.PneumaticsModuleType.CTREPCM 12 | 13 | 14 | class MyRobot(wpilib.TimedRobot): 15 | """ 16 | Example file showing how to get game-data from your driver station / FMS 17 | """ 18 | 19 | def robotInit(self): 20 | # A way of demonstrating the difference between the game data strings 21 | self.blue = wpilib.Solenoid(PNUE_MOD_TYPE, 0) 22 | self.red = wpilib.Solenoid(PNUE_MOD_TYPE, 1) 23 | self.green = wpilib.Solenoid(PNUE_MOD_TYPE, 2) 24 | self.yellow = wpilib.Solenoid(PNUE_MOD_TYPE, 3) 25 | # Set game data to empty string by default 26 | self.gameData = "" 27 | # Get the SmartDashboard table from networktables 28 | self.sd = ntcore.NetworkTableInstance.getDefault().getTable("SmartDashboard") 29 | 30 | def teleopPeriodic(self): 31 | data = wpilib.DriverStation.getGameSpecificMessage() 32 | if data: 33 | # Set the robot gamedata property and set a network tables value 34 | self.gameData = data 35 | self.sd.putString("gameData", self.gameData) 36 | 37 | # Solenoid based indicator of the current color 38 | self.blue.set(self.gameData == "B") 39 | self.red.set(self.gameData == "R") 40 | self.green.set(self.gameData == "G") 41 | self.yellow.set(self.gameData == "Y") 42 | -------------------------------------------------------------------------------- /DifferentialDriveBot/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | import wpimath.filter 11 | 12 | from drivetrain import Drivetrain 13 | 14 | 15 | class MyRobot(wpilib.TimedRobot): 16 | def robotInit(self): 17 | self.controller = wpilib.XboxController(0) 18 | self.drive = Drivetrain() 19 | 20 | # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. 21 | self.speedLimiter = wpimath.filter.SlewRateLimiter(3) 22 | self.rotLimiter = wpimath.filter.SlewRateLimiter(3) 23 | 24 | def autonomousPeriodic(self): 25 | self.teleopPeriodic() 26 | self.drive.updateOdometry() 27 | 28 | def teleopPeriodic(self): 29 | # Get the x speed. We are inverting this because Xbox controllers return 30 | # negative values when we push forward. 31 | xSpeed = ( 32 | -self.speedLimiter.calculate(self.controller.getLeftY()) 33 | * Drivetrain.MAX_SPEED 34 | ) 35 | 36 | # Get the rate of angular rotation. We are inverting this because we want a 37 | # positive value when we pull to the left (remember, CCW is positive in 38 | # mathematics). Xbox controllers return positive values when you pull to 39 | # the right by default. 40 | rot = ( 41 | -self.rotLimiter.calculate(self.controller.getRightX()) 42 | * Drivetrain.MAX_ANGULAR_SPEED 43 | ) 44 | 45 | self.drive.drive(xSpeed, rot) 46 | -------------------------------------------------------------------------------- /StatefulAutonomous/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | from robotpy_ext.autonomous import AutonomousModeSelector 12 | 13 | 14 | class MyRobot(wpilib.TimedRobot): 15 | """ 16 | This shows using the AutonomousModeSelector to automatically choose 17 | autonomous modes. 18 | 19 | If you find this useful, you may want to consider using the Magicbot 20 | framework, as it already has this integrated into it. 21 | """ 22 | 23 | def robotInit(self): 24 | # Simple two wheel drive 25 | self.drive = wpilib.drive.DifferentialDrive(wpilib.Talon(0), wpilib.Talon(1)) 26 | 27 | # Items in this dictionary are available in your autonomous mode 28 | # as attributes on your autonomous object 29 | self.components = {"drive": self.drive} 30 | 31 | # * The first argument is the name of the package that your autonomous 32 | # modes are located in 33 | # * The second argument is passed to each StatefulAutonomous when they 34 | # start up 35 | self.automodes = AutonomousModeSelector("autonomous", self.components) 36 | 37 | def autonomousInit(self): 38 | self.drive.setSafetyEnabled(True) 39 | self.automodes.start() 40 | 41 | def autonomousPeriodic(self): 42 | self.automodes.periodic() 43 | 44 | def disabledInit(self): 45 | self.automodes.disable() 46 | 47 | def teleopInit(self): 48 | pass 49 | 50 | def teleopPeriodic(self): 51 | pass 52 | -------------------------------------------------------------------------------- /MotorControl/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | import math 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | """ 15 | This sample program shows how to control a motor using a joystick. In the operator control part 16 | of the program, the joystick is read and the value is written to the motor. 17 | 18 | Joystick analog values range from -1 to 1 and motor controller inputs also range from -1 to 1 19 | making it easy to work together. 20 | 21 | In addition, the encoder value of an encoder connected to ports 0 and 1 is consistently sent 22 | to the Dashboard. 23 | """ 24 | 25 | kMotorPort = 0 26 | kJoystickPort = 0 27 | kEncoderPortA = 0 28 | kEncoderPortB = 1 29 | 30 | def robotInit(self): 31 | """Robot initialization function""" 32 | 33 | self.motor = wpilib.PWMSparkMax(self.kMotorPort) 34 | self.joystick = wpilib.Joystick(self.kJoystickPort) 35 | self.encoder = wpilib.Encoder(self.kEncoderPortA, self.kEncoderPortB) 36 | # Use SetDistancePerPulse to set the multiplier for GetDistance 37 | # This is set up assuming a 6 inch wheel with a 360 CPR encoder. 38 | self.encoder.setDistancePerPulse((math.pi * 6) / 360.0) 39 | 40 | def robotPeriodic(self): 41 | """The RobotPeriodic function is called every control packet no matter the robot mode.""" 42 | wpilib.SmartDashboard.putNumber("Encoder", self.encoder.getDistance()) 43 | 44 | def teleopPeriodic(self): 45 | self.motor.set(self.joystick.getY()) 46 | -------------------------------------------------------------------------------- /Timed/src/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """Main robot class.""" 13 | 14 | def robotInit(self): 15 | """Robot-wide initialization code should go here.""" 16 | self.lstick = wpilib.Joystick(0) 17 | self.motor = wpilib.Talon(3) 18 | 19 | self.timer = wpilib.Timer() 20 | self.loops = 0 21 | 22 | def autonomousInit(self): 23 | """Called only at the beginning of autonomous mode.""" 24 | pass 25 | 26 | def autonomousPeriodic(self): 27 | """Called every 20ms in autonomous mode.""" 28 | pass 29 | 30 | def disabledInit(self): 31 | """Called only at the beginning of disabled mode.""" 32 | self.logger.info("%d loops / %f seconds", self.loops, self.timer.get()) 33 | 34 | def disabledPeriodic(self): 35 | """Called every 20ms in disabled mode.""" 36 | pass 37 | 38 | def teleopInit(self): 39 | """Called only at the beginning of teleoperated mode.""" 40 | self.loops = 0 41 | self.timer.reset() 42 | self.timer.start() 43 | 44 | def teleopPeriodic(self): 45 | """Called every 20ms in teleoperated mode""" 46 | # Move a motor with a Joystick 47 | self.motor.set(self.lstick.getY()) 48 | 49 | # Print out the number of loop iterations passed every second 50 | self.loops += 1 51 | if self.timer.advanceIfElapsed(1): 52 | self.logger.info("%d loops / second", self.loops) 53 | self.loops = 0 54 | -------------------------------------------------------------------------------- /XrpReference/commands/drivedistance.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from subsystems.drivetrain import Drivetrain 10 | 11 | 12 | class DriveDistance(commands2.Command): 13 | def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None: 14 | """Creates a new DriveDistance. This command will drive your your robot for a desired distance at 15 | a desired speed. 16 | 17 | :param speed: The speed at which the robot will drive 18 | :param inches: The number of inches the robot will drive 19 | :param drive: The drivetrain subsystem on which this command will run 20 | """ 21 | super().__init__() 22 | 23 | self.distance = inches 24 | self.speed = speed 25 | self.drive = drive 26 | self.addRequirements(drive) 27 | 28 | def initialize(self) -> None: 29 | """Called when the command is initially scheduled.""" 30 | self.drive.arcadeDrive(0, 0) 31 | self.drive.resetEncoders() 32 | 33 | def execute(self) -> None: 34 | """Called every time the scheduler runs while the command is scheduled.""" 35 | self.drive.arcadeDrive(self.speed, 0) 36 | 37 | def end(self, interrupted: bool) -> None: 38 | """Called once the command ends or is interrupted.""" 39 | self.drive.arcadeDrive(0, 0) 40 | 41 | def isFinished(self) -> bool: 42 | """Returns true when the command should end.""" 43 | # Compare distance travelled from start to desired distance 44 | return abs(self.drive.getAverageDistanceInch()) >= self.distance 45 | -------------------------------------------------------------------------------- /RomiReference/commands/drivedistance.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | from subsystems.drivetrain import Drivetrain 10 | 11 | 12 | class DriveDistance(commands2.Command): 13 | def __init__(self, speed: float, inches: float, drive: Drivetrain) -> None: 14 | """Creates a new DriveDistance. This command will drive your your robot for a desired distance at 15 | a desired speed. 16 | 17 | :param speed: The speed at which the robot will drive 18 | :param inches: The number of inches the robot will drive 19 | :param drive: The drivetrain subsystem on which this command will run 20 | """ 21 | super().__init__() 22 | 23 | self.distance = inches 24 | self.speed = speed 25 | self.drive = drive 26 | self.addRequirements(drive) 27 | 28 | def initialize(self) -> None: 29 | """Called when the command is initially scheduled.""" 30 | self.drive.arcadeDrive(0, 0) 31 | self.drive.resetEncoders() 32 | 33 | def execute(self) -> None: 34 | """Called every time the scheduler runs while the command is scheduled.""" 35 | self.drive.arcadeDrive(self.speed, 0) 36 | 37 | def end(self, interrupted: bool) -> None: 38 | """Called once the command ends or is interrupted.""" 39 | self.drive.arcadeDrive(0, 0) 40 | 41 | def isFinished(self) -> bool: 42 | """Returns true when the command should end.""" 43 | # Compare distance travelled from start to desired distance 44 | return abs(self.drive.getAverageDistanceInch()) >= self.distance 45 | -------------------------------------------------------------------------------- /DriveDistanceOffboard/commands/drivedistanceprofiled.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | 9 | import wpimath.trajectory 10 | 11 | import subsystems.drivesubsystem 12 | import constants 13 | 14 | 15 | class DriveDistanceProfiled(commands2.TrapezoidProfileCommand): 16 | """Drives a set distance using a motion profile.""" 17 | 18 | def __init__( 19 | self, meters: float, drive: subsystems.drivesubsystem.DriveSubsystem 20 | ) -> None: 21 | """Creates a new DriveDistanceProfiled command. 22 | 23 | :param meters: The distance to drive. 24 | :param drive: The drive subsystem to use. 25 | """ 26 | super().__init__( 27 | wpimath.trajectory.TrapezoidProfile( 28 | # Limit the max acceleration and velocity 29 | wpimath.trajectory.TrapezoidProfile.Constraints( 30 | constants.DriveConstants.kMaxSpeedMetersPerSecond, 31 | constants.DriveConstants.kMaxAccelerationMetersPerSecondSquared, 32 | ) 33 | ), 34 | # Pipe the profile state to the drive 35 | lambda setpointState: drive.setDriveStates(setpointState, setpointState), 36 | # End at desired position in meters; implicitly starts at 0 37 | lambda: wpimath.trajectory.TrapezoidProfile.State(meters, 0), 38 | # Current position 39 | lambda: wpimath.trajectory.TrapezoidProfile.State(0, 0), 40 | # Require the drive 41 | drive, 42 | ) 43 | # Reset drive encoders since we're starting at 0 44 | drive.resetEncoders() 45 | -------------------------------------------------------------------------------- /AddressableLED/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | kLEDBuffer = 60 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | def robotInit(self): 15 | # PWM Port 9 16 | # Must be a PWM header, not MXP or DIO 17 | self.led = wpilib.AddressableLED(9) 18 | 19 | # LED Data 20 | self.ledData = [wpilib.AddressableLED.LEDData() for _ in range(kLEDBuffer)] 21 | 22 | # Store what the last hue of the first pixel is 23 | self.rainbowFirstPixelHue = 0 24 | 25 | # Default to a length of 60, start empty output 26 | # Length is expensive to set, so only set it once, then just update data 27 | self.led.setLength(kLEDBuffer) 28 | 29 | # Set the data 30 | self.led.setData(self.ledData) 31 | self.led.start() 32 | 33 | def robotPeriodic(self): 34 | # Fill the buffer with a rainbow 35 | self.rainbow() 36 | 37 | # Set the LEDs 38 | self.led.setData(self.ledData) 39 | 40 | def rainbow(self): 41 | # For every pixel 42 | for i in range(kLEDBuffer): 43 | # Calculate the hue - hue is easier for rainbows because the color 44 | # shape is a circle so only one value needs to precess 45 | hue = (self.rainbowFirstPixelHue + (i * 180 / kLEDBuffer)) % 180 46 | 47 | # Set the value 48 | self.ledData[i].setHSV(int(hue), 255, 128) 49 | 50 | # Increase by to make the rainbow "move" 51 | self.rainbowFirstPixelHue += 3 52 | 53 | # Check bounds 54 | self.rainbowFirstPixelHue %= 180 55 | -------------------------------------------------------------------------------- /Physics4Wheel/src/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """Main robot class""" 14 | 15 | def robotInit(self): 16 | """Robot-wide initialization code should go here""" 17 | 18 | self.lstick = wpilib.Joystick(0) 19 | self.rstick = wpilib.Joystick(1) 20 | 21 | # Define front and rear motors 22 | self.lf_motor = wpilib.Jaguar(1) 23 | self.lr_motor = wpilib.Jaguar(2) 24 | self.rf_motor = wpilib.Jaguar(3) 25 | self.rr_motor = wpilib.Jaguar(4) 26 | 27 | # add the followers to the left and right motors 28 | self.lf_motor.addFollower(self.lr_motor) 29 | self.rf_motor.addFollower(self.rr_motor) 30 | 31 | # Set the right side motors to be inverted 32 | self.lf_motor.setInverted(True) 33 | 34 | self.drive = wpilib.drive.DifferentialDrive(self.lf_motor, self.lr_motor) 35 | 36 | # Position gets automatically updated as robot moves 37 | self.gyro = wpilib.AnalogGyro(1) 38 | 39 | def autonomousInit(self): 40 | """Called when autonomous mode is enabled""" 41 | 42 | self.timer = wpilib.Timer() 43 | self.timer.start() 44 | 45 | def autonomousPeriodic(self): 46 | if self.timer.get() < 2.0: 47 | self.drive.arcadeDrive(-1.0, -0.3) 48 | else: 49 | self.drive.arcadeDrive(0, 0) 50 | 51 | def teleopPeriodic(self): 52 | """Called when operation control mode is enabled""" 53 | self.drive.tankDrive(-self.lstick.getY(), -self.rstick.getY()) 54 | -------------------------------------------------------------------------------- /RomiReference/commands/turntime.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import wpilib 9 | 10 | from subsystems.drivetrain import Drivetrain 11 | 12 | 13 | class TurnTime(commands2.Command): 14 | """Creates a new TurnTime command. This command will turn your robot for a 15 | desired rotational speed and time. 16 | """ 17 | 18 | def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: 19 | """Creates a new TurnTime. 20 | 21 | :param speed: The speed which the robot will turn. Negative is in reverse. 22 | :param time: How much time to turn in seconds 23 | :param drive: The drive subsystem on which this command will run 24 | """ 25 | super().__init__() 26 | 27 | self.rotationalSpeed = speed 28 | self.duration = time 29 | self.drive = drive 30 | self.addRequirements(drive) 31 | 32 | self.startTime = 0.0 33 | 34 | def initialize(self) -> None: 35 | """Called when the command is initially scheduled.""" 36 | self.startTime = wpilib.Timer.getFPGATimestamp() 37 | self.drive.arcadeDrive(0, 0) 38 | 39 | def execute(self) -> None: 40 | """Called every time the scheduler runs while the command is scheduled.""" 41 | self.drive.arcadeDrive(0, self.rotationalSpeed) 42 | 43 | def end(self, interrupted: bool) -> None: 44 | """Called once the command ends or is interrupted.""" 45 | self.drive.arcadeDrive(0, 0) 46 | 47 | def isFinished(self) -> bool: 48 | """Returns true when the command should end""" 49 | return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration 50 | -------------------------------------------------------------------------------- /XrpReference/commands/turntime.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import wpilib 9 | 10 | from subsystems.drivetrain import Drivetrain 11 | 12 | 13 | class TurnTime(commands2.Command): 14 | """Creates a new TurnTime command. This command will turn your robot for a 15 | desired rotational speed and time. 16 | """ 17 | 18 | def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: 19 | """Creates a new TurnTime. 20 | 21 | :param speed: The speed which the robot will turn. Negative is in reverse. 22 | :param time: How much time to turn in seconds 23 | :param drive: The drive subsystem on which this command will run 24 | """ 25 | super().__init__() 26 | 27 | self.rotationalSpeed = speed 28 | self.duration = time 29 | self.drive = drive 30 | self.addRequirements(drive) 31 | 32 | self.startTime = 0.0 33 | 34 | def initialize(self) -> None: 35 | """Called when the command is initially scheduled.""" 36 | self.startTime = wpilib.Timer.getFPGATimestamp() 37 | self.drive.arcadeDrive(0, 0) 38 | 39 | def execute(self) -> None: 40 | """Called every time the scheduler runs while the command is scheduled.""" 41 | self.drive.arcadeDrive(0, self.rotationalSpeed) 42 | 43 | def end(self, interrupted: bool) -> None: 44 | """Called once the command ends or is interrupted.""" 45 | self.drive.arcadeDrive(0, 0) 46 | 47 | def isFinished(self) -> bool: 48 | """Returns true when the command should end""" 49 | return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration 50 | -------------------------------------------------------------------------------- /XrpReference/commands/drivetime.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import wpilib 9 | 10 | from subsystems.drivetrain import Drivetrain 11 | 12 | 13 | class DriveTime(commands2.Command): 14 | """Creates a new DriveTime. This command will drive your robot for a desired speed and time.""" 15 | 16 | def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: 17 | """Creates a new DriveTime. This command will drive your robot for a desired speed and time. 18 | 19 | :param speed: The speed which the robot will drive. Negative is in reverse. 20 | :param time: How much time to drive in seconds 21 | :param drive: The drivetrain subsystem on which this command will run 22 | """ 23 | super().__init__() 24 | 25 | self.speed = speed 26 | self.duration = time 27 | self.drive = drive 28 | self.addRequirements(drive) 29 | 30 | self.startTime = 0.0 31 | 32 | def initialize(self) -> None: 33 | """Called when the command is initially scheduled.""" 34 | self.startTime = wpilib.Timer.getFPGATimestamp() 35 | self.drive.arcadeDrive(0, 0) 36 | 37 | def execute(self) -> None: 38 | """Called every time the scheduler runs while the command is scheduled.""" 39 | self.drive.arcadeDrive(self.speed, 0) 40 | 41 | def end(self, interrupted: bool) -> None: 42 | """Called once the command ends or is interrupted.""" 43 | self.drive.arcadeDrive(0, 0) 44 | 45 | def isFinished(self) -> bool: 46 | """Returns true when the command should end""" 47 | return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration 48 | -------------------------------------------------------------------------------- /MecanumDrive/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a demo program showing how to use Mecanum control with the 15 | MecanumDrive class. 16 | """ 17 | 18 | # Channels on the roboRIO that the motor controllers are plugged in to 19 | kFrontLeftChannel = 2 20 | kRearLeftChannel = 3 21 | kFrontRightChannel = 1 22 | kRearRightChannel = 0 23 | 24 | # The channel on the driver station that the joystick is connected to 25 | kJoystickChannel = 0 26 | 27 | def robotInit(self): 28 | self.frontLeft = wpilib.PWMSparkMax(self.kFrontLeftChannel) 29 | self.rearLeft = wpilib.PWMSparkMax(self.kRearLeftChannel) 30 | self.frontRight = wpilib.PWMSparkMax(self.kFrontRightChannel) 31 | self.rearRight = wpilib.PWMSparkMax(self.kRearRightChannel) 32 | 33 | # invert the right side motors 34 | # you may need to change or remove this to match your robot 35 | self.frontRight.setInverted(True) 36 | self.rearRight.setInverted(True) 37 | 38 | self.robotDrive = wpilib.drive.MecanumDrive( 39 | self.frontLeft, self.rearLeft, self.frontRight, self.rearRight 40 | ) 41 | 42 | self.stick = wpilib.Joystick(self.kJoystickChannel) 43 | 44 | def teleopPeriodic(self): 45 | # Use the joystick X axis for lateral movement, Y axis for forward 46 | # movement, and Z axis for rotation. 47 | self.robotDrive.driveCartesian( 48 | -self.stick.getY(), 49 | -self.stick.getX(), 50 | -self.stick.getZ(), 51 | ) 52 | -------------------------------------------------------------------------------- /RomiReference/commands/drivetime.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import wpilib 9 | 10 | from subsystems.drivetrain import Drivetrain 11 | 12 | 13 | class DriveTime(commands2.Command): 14 | """Creates a new DriveTime. This command will drive your robot for a desired speed and time.""" 15 | 16 | def __init__(self, speed: float, time: float, drive: Drivetrain) -> None: 17 | """Creates a new DriveTime. This command will drive your robot for a desired speed and time. 18 | 19 | :param speed: The speed which the robot will drive. Negative is in reverse. 20 | :param time: How much time to drive in seconds 21 | :param drive: The drivetrain subsystem on which this command will run 22 | """ 23 | super().__init__() 24 | 25 | self.speed = speed 26 | self.duration = time 27 | self.drive = drive 28 | self.addRequirements(drive) 29 | 30 | self.startTime = 0.0 31 | 32 | def initialize(self) -> None: 33 | """Called when the command is initially scheduled.""" 34 | self.startTime = wpilib.Timer.getFPGATimestamp() 35 | self.drive.arcadeDrive(0, 0) 36 | 37 | def execute(self) -> None: 38 | """Called every time the scheduler runs while the command is scheduled.""" 39 | self.drive.arcadeDrive(self.speed, 0) 40 | 41 | def end(self, interrupted: bool) -> None: 42 | """Called once the command ends or is interrupted.""" 43 | self.drive.arcadeDrive(0, 0) 44 | 45 | def isFinished(self) -> bool: 46 | """Returns true when the command should end""" 47 | return wpilib.Timer.getFPGATimestamp() - self.startTime >= self.duration 48 | -------------------------------------------------------------------------------- /ArmBot/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | The constants module is a convenience place for teams to hold robot-wide 9 | numerical or boolean constants. Don't use this for any other purpose! 10 | """ 11 | 12 | import math 13 | 14 | 15 | class DriveConstants: 16 | # The PWM IDs for the drivetrain motor controllers. 17 | kLeftMotor1Port = 0 18 | kLeftMotor2Port = 1 19 | kRightMotor1Port = 2 20 | kRightMotor2Port = 3 21 | 22 | # Encoders and their respective motor controllers. 23 | kLeftEncoderPorts = (0, 1) 24 | kRightEncoderPorts = (2, 3) 25 | kLeftEncoderReversed = False 26 | kRightEncoderReversed = True 27 | 28 | # Encoder counts per revolution/rotation. 29 | kEncoderCPR = 1024 30 | kWheelDiameterInches = 6 31 | 32 | # Assumes the encoders are directly mounted on the wheel shafts 33 | kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR 34 | 35 | 36 | class ArmConstants: 37 | # NOTE: Please do NOT use these values on your robot. 38 | kMotorPort = 4 39 | kP = 1 40 | kSVolts = 1 41 | kGVolts = 1 42 | kVVoltSecondPerRad = 0.5 43 | kAVoltSecondSquaredPerRad = 0.1 44 | 45 | kMaxVelocityRadPerSecond = 3 46 | kMaxAccelerationRadPerSecSquared = 10 47 | 48 | kEncoderPorts = (4, 5) 49 | kEncoderPPR = 256 50 | kEncoderDistancePerPulse = 2.0 * math.pi / kEncoderPPR 51 | 52 | # The offset of the arm from the horizontal in its neutral position, 53 | # measured from the horizontal 54 | kArmOffsetRads = 0.5 55 | 56 | 57 | class AutoConstants: 58 | kAutoTimeoutSeconds = 12 59 | kAutoShootTimeSeconds = 7 60 | 61 | 62 | class OIConstants: 63 | kDriverControllerPort = 0 64 | -------------------------------------------------------------------------------- /DigitalCommunication/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """ 13 | This is a sample program demonstrating how to communicate to a light controller from the robot 14 | code using the roboRIO's DIO ports. 15 | """ 16 | 17 | # define ports for digitalcommunication with light controller 18 | kAlliancePort = 0 19 | kEnabledPort = 1 20 | kAutonomousPort = 2 21 | kAlertPort = 3 22 | 23 | def robotInit(self): 24 | """Robot initialization function""" 25 | 26 | self.allianceOutput = wpilib.DigitalOutput(self.kAlliancePort) 27 | self.enabledOutput = wpilib.DigitalOutput(self.kEnabledPort) 28 | self.autonomousOutput = wpilib.DigitalOutput(self.kAutonomousPort) 29 | self.alertOutput = wpilib.DigitalOutput(self.kAlertPort) 30 | 31 | def robotPeriodic(self): 32 | setAlliance = False 33 | alliance = wpilib.DriverStation.getAlliance() 34 | if alliance: 35 | setAlliance = alliance == wpilib.DriverStation.Alliance.kRed 36 | 37 | # pull alliance port high if on red alliance, pull low if on blue alliance 38 | self.allianceOutput.set(setAlliance) 39 | 40 | # pull enabled port high if enabled, low if disabled 41 | self.enabledOutput.set(wpilib.DriverStation.isEnabled()) 42 | 43 | # pull auto port high if in autonomous, low if in teleop (or disabled) 44 | self.autonomousOutput.set(wpilib.DriverStation.isAutonomous()) 45 | 46 | # pull alert port high if match time remaining is between 30 and 25 seconds 47 | matchTime = wpilib.DriverStation.getMatchTime() 48 | self.alertOutput.set(30 >= matchTime >= 25) 49 | -------------------------------------------------------------------------------- /FrisbeeBot/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | A place for the constant values in the code that may be used in more than one place. 9 | This offers a convenient resources to teams who need to make both quick and universal 10 | changes. 11 | """ 12 | 13 | import math 14 | 15 | 16 | class DriveConstants: 17 | kLeftMotor1Port = 0 18 | kLeftMotor2Port = 1 19 | kRightMotor1Port = 2 20 | kRightMotor2Port = 3 21 | 22 | kLeftEncoderPorts = (0, 1) 23 | kRightEncoderPorts = (2, 3) 24 | kLeftEncoderReversed = False 25 | kRightEncoderReversed = True 26 | 27 | kEncoderCPR = 1024 28 | kWheelDiameterInches = 6 29 | 30 | # Assumes the encoders are directly mounted on the wheel shafts 31 | kEncoderDistancePerPulse = (kWheelDiameterInches * math.pi) / kEncoderCPR 32 | 33 | 34 | class ShooterConstants: 35 | kEncoderPorts = (4, 5) 36 | kEncoderReversed = False 37 | kEncoderCPR = 1024 38 | kEncoderDistancePerPulse = 1 / kEncoderCPR 39 | 40 | kShooterMotorPort = 4 41 | kFeederMotorPort = 5 42 | 43 | kShooterFreeRPS = 5300 44 | kShooterTargetRPS = 4000 45 | kShooterToleranceRPS = 50 46 | 47 | # These are not real PID gains, and will have to be tuned for your specific robot. 48 | kP = 1 49 | kI = 0 50 | kD = 0 51 | 52 | # On a real robot the feedforward constants should be empirically determined; these are 53 | # reasonable guesses. 54 | kSVolts = 0.05 55 | 56 | # Should have value 12V at free speed... 57 | kVVoltSecondsPerRotation = 12.0 / kShooterFreeRPS 58 | 59 | kFeederSpeed = 0.5 60 | 61 | 62 | class AutoConstants: 63 | kAutoTimeoutSeconds = 12 64 | kAutoShootTimeSeconds = 7 65 | 66 | 67 | class OIConstants: 68 | kDriverControllerPort = 0 69 | -------------------------------------------------------------------------------- /GyroMecanum/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | 9 | import wpilib 10 | import wpilib.drive 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | """ 15 | This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation vectors 16 | in relation to the starting orientation of the robot (field-oriented controls). 17 | """ 18 | 19 | # gyro calibration constant, may need to be adjusted; 20 | # gyro value of 360 is set to correspond to one full revolution 21 | kVoltsPerDegreePerSecond = 0.0128 22 | 23 | kFrontLeftChannel = 0 24 | kRearLeftChannel = 1 25 | kFrontRightChannel = 2 26 | kRearRightChannel = 3 27 | kGyroPort = 0 28 | kJoystickPort = 0 29 | 30 | def robotInit(self): 31 | """Robot initialization function""" 32 | 33 | self.gyro = wpilib.AnalogGyro(self.kGyroPort) 34 | self.joystick = wpilib.Joystick(self.kJoystickPort) 35 | 36 | frontLeft = wpilib.PWMSparkMax(self.kFrontLeftChannel) 37 | rearLeft = wpilib.PWMSparkMax(self.kRearLeftChannel) 38 | frontRight = wpilib.PWMSparkMax(self.kFrontRightChannel) 39 | rearRight = wpilib.PWMSparkMax(self.kRearRightChannel) 40 | 41 | frontRight.setInverted(True) 42 | rearRight.setInverted(True) 43 | 44 | self.robotDrive = wpilib.drive.MecanumDrive( 45 | frontLeft, rearLeft, frontRight, rearRight 46 | ) 47 | 48 | self.gyro.setSensitivity(self.kVoltsPerDegreePerSecond) 49 | 50 | def teleopPeriodic(self): 51 | self.robotDrive.driveCartesian( 52 | -self.joystick.getY(), 53 | -self.joystick.getX(), 54 | -self.joystick.getZ(), 55 | self.gyro.getRotation2d(), 56 | ) 57 | -------------------------------------------------------------------------------- /Physics/src/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """Main robot class""" 14 | 15 | def robotInit(self): 16 | """Robot-wide initialization code should go here""" 17 | 18 | self.lstick = wpilib.Joystick(0) 19 | self.rstick = wpilib.Joystick(1) 20 | 21 | self.l_motor = wpilib.Jaguar(1) 22 | self.r_motor = wpilib.Jaguar(2) 23 | 24 | # Position gets automatically updated as robot moves 25 | self.gyro = wpilib.AnalogGyro(1) 26 | 27 | self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor) 28 | 29 | self.motor = wpilib.Jaguar(4) 30 | 31 | self.limit1 = wpilib.DigitalInput(1) 32 | self.limit2 = wpilib.DigitalInput(2) 33 | 34 | self.position = wpilib.AnalogInput(2) 35 | 36 | def autonomousInit(self): 37 | """Called when autonomous mode is enabled""" 38 | 39 | self.timer = wpilib.Timer() 40 | self.timer.start() 41 | 42 | def autonomousPeriodic(self): 43 | if self.timer.get() < 2.0: 44 | self.drive.arcadeDrive(-1.0, -0.3) 45 | else: 46 | self.drive.arcadeDrive(0, 0) 47 | 48 | def teleopPeriodic(self): 49 | """Called when operation control mode is enabled""" 50 | 51 | self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) 52 | 53 | # Move a motor with a Joystick 54 | y = self.rstick.getY() 55 | 56 | # stop movement backwards when 1 is on 57 | if self.limit1.get(): 58 | y = max(0, y) 59 | 60 | # stop movement forwards when 2 is on 61 | if self.limit2.get(): 62 | y = min(0, y) 63 | 64 | self.motor.set(y) 65 | -------------------------------------------------------------------------------- /PhysicsSPI/src/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """Main robot class""" 14 | 15 | def robotInit(self): 16 | """Robot-wide initialization code should go here""" 17 | 18 | self.lstick = wpilib.Joystick(0) 19 | self.rstick = wpilib.Joystick(1) 20 | 21 | self.l_motor = wpilib.Jaguar(1) 22 | self.r_motor = wpilib.Jaguar(2) 23 | 24 | # Position gets automatically updated as robot moves 25 | self.gyro = wpilib.ADXRS450_Gyro() 26 | 27 | self.drive = wpilib.drive.DifferentialDrive(self.l_motor, self.r_motor) 28 | 29 | self.motor = wpilib.Jaguar(4) 30 | 31 | self.limit1 = wpilib.DigitalInput(1) 32 | self.limit2 = wpilib.DigitalInput(2) 33 | 34 | self.position = wpilib.AnalogInput(2) 35 | 36 | def autonomousInit(self): 37 | """Called when autonomous mode is enabled""" 38 | 39 | self.timer = wpilib.Timer() 40 | self.timer.start() 41 | 42 | def autonomousPeriodic(self): 43 | if self.timer.get() < 2.0: 44 | self.drive.arcadeDrive(-1.0, -0.3) 45 | else: 46 | self.drive.arcadeDrive(0, 0) 47 | 48 | def teleopPeriodic(self): 49 | """Called when operation control mode is enabled""" 50 | 51 | self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX()) 52 | 53 | # Move a motor with a Joystick 54 | y = self.rstick.getY() 55 | 56 | # stop movement backwards when 1 is on 57 | if self.limit1.get(): 58 | y = max(0, y) 59 | 60 | # stop movement forwards when 2 is on 61 | if self.limit2.get(): 62 | y = min(0, y) 63 | 64 | self.motor.set(y) 65 | -------------------------------------------------------------------------------- /ElevatorSimulation/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import math 9 | import wpilib 10 | import wpimath.controller 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | kMotorPort = 0 15 | kEncoderAChannel = 0 16 | kEncoderBChannel = 1 17 | kJoystickPort = 0 18 | 19 | kElevatorKp = 5.0 20 | kElevatorGearing = 10.0 21 | kElevatorDrumRadius = 0.0508 # 2 inches in meters 22 | kCarriageMass = 4 23 | 24 | kMinElevatorHeight = 0.0508 # 2 inches 25 | kMaxElevatorHeight = 1.27 # 50 inches 26 | 27 | # distance per pulse = (distance per revolution) / (pulses per revolution) 28 | # = (Pi * D) / ppr 29 | kElevatorEncoderDistPerPulse = 2.0 * math.pi * kElevatorDrumRadius / 4096.0 30 | 31 | def robotInit(self) -> None: 32 | # standard classes for controlling our elevator 33 | self.controller = wpimath.controller.PIDController(self.kElevatorKp, 0, 0) 34 | self.encoder = wpilib.Encoder(self.kEncoderAChannel, self.kEncoderBChannel) 35 | self.motor = wpilib.PWMSparkMax(self.kMotorPort) 36 | self.joystick = wpilib.Joystick(self.kJoystickPort) 37 | 38 | self.encoder.setDistancePerPulse(self.kElevatorEncoderDistPerPulse) 39 | 40 | def teleopPeriodic(self) -> None: 41 | if self.joystick.getTrigger(): 42 | # Here, we run PID control like normal, with a constant setpoint of 30in (0.762 meters). 43 | pidOutput = self.controller.calculate(self.encoder.getDistance(), 0.762) 44 | self.motor.setVoltage(pidOutput) 45 | else: 46 | # Otherwise we disable the motor 47 | self.motor.set(0.0) 48 | 49 | def disabledInit(self) -> None: 50 | # This just makes sure that our simulation code knows that the motor is off 51 | self.motor.set(0) 52 | -------------------------------------------------------------------------------- /SysId/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | from commands2 import CommandScheduler, TimedCommandRobot 9 | 10 | from sysidroutinebot import SysIdRoutineBot 11 | 12 | 13 | class MyRobot(TimedCommandRobot): 14 | """The VM is configured to automatically run this class, and to call the functions corresponding to 15 | each mode, as described in the TimedRobot documentation. If you change the name of this class or 16 | the package after creating this project, you must also update the build.gradle file in the 17 | project. 18 | """ 19 | 20 | def robotInit(self) -> None: 21 | """This function is run when the robot is first started up and should be used for any 22 | initialization code. 23 | """ 24 | self.robot = SysIdRoutineBot() 25 | 26 | self.robot.configureBindings() 27 | 28 | self.autonomous_command = self.robot.getAutonomousCommand() 29 | 30 | def disabledInit(self) -> None: 31 | """This function is called once each time the robot enters Disabled mode.""" 32 | pass 33 | 34 | def disabledPeriodic(self) -> None: 35 | pass 36 | 37 | def autonomousInit(self) -> None: 38 | self.autonomous_command.schedule() 39 | 40 | def teleopInit(self) -> None: 41 | # This makes sure that the autonomous stops running when 42 | # teleop starts running. If you want the autonomous to 43 | # continue until interrupted by another command, remove 44 | # this line or comment it out. 45 | self.autonomous_command.cancel() 46 | 47 | def testInit(self) -> None: 48 | # Cancels all running commands at the start of test mode. 49 | CommandScheduler.getInstance().cancelAll() 50 | 51 | def testPeriodic(self) -> None: 52 | """This function is called periodically during test mode.""" 53 | pass 54 | -------------------------------------------------------------------------------- /Relay/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """ 13 | This is a sample program which uses joystick buttons to control a relay. A Relay (generally a 14 | spike) has two outputs, each of which can be at either 0V or 12V and so can be used for actions 15 | such as turning a motor off, full forwards, or full reverse, and is generally used on the 16 | compressor. This program uses two buttons on a joystick and each button corresponds to one 17 | output; pressing the button sets the output to 12V and releasing sets it to 0V. 18 | """ 19 | 20 | kRelayForwardButton = 1 21 | kRelayReverseButton = 2 22 | 23 | def robotInit(self): 24 | """Robot initialization function""" 25 | 26 | self.stick = wpilib.Joystick(0) 27 | self.relay = wpilib.Relay(0) 28 | 29 | def teleopPeriodic(self): 30 | # Retrieve the button values. GetRawButton will 31 | # return true if the button is pressed and false if not. 32 | 33 | forward = self.stick.getRawButton(self.kRelayForwardButton) 34 | reverse = self.stick.getRawButton(self.kRelayReverseButton) 35 | 36 | ## 37 | # Depending on the button values, we want to use one of 38 | # kOn, kOff, kForward, or kReverse. kOn sets both outputs to 12V, 39 | # kOff sets both to 0V, kForward sets forward to 12V 40 | # and reverse to 0V, and kReverse sets reverse to 12V and forward to 0V. 41 | ## 42 | 43 | if forward and reverse: 44 | self.relay.set(wpilib.Relay.Value.kOn) 45 | elif forward: 46 | self.relay.set(wpilib.Relay.Value.kForward) 47 | elif reverse: 48 | self.relay.set(wpilib.Relay.Value.kReverse) 49 | else: 50 | self.relay.set(wpilib.Relay.Value.kOff) 51 | -------------------------------------------------------------------------------- /ArmBotOffboard/subsystems/armsubsystem.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import commands2.cmd 9 | import wpimath.controller 10 | import wpimath.trajectory 11 | 12 | import constants 13 | import examplesmartmotorcontroller 14 | 15 | 16 | class ArmSubsystem(commands2.TrapezoidProfileSubsystem): 17 | """A robot arm subsystem that moves with a motion profile.""" 18 | 19 | # Create a new ArmSubsystem 20 | def __init__(self) -> None: 21 | super().__init__( 22 | wpimath.trajectory.TrapezoidProfile.Constraints( 23 | constants.kMaxVelocityRadPerSecond, 24 | constants.kMaxAccelerationRadPerSecSquared, 25 | ), 26 | constants.kArmOffsetRads, 27 | ) 28 | self.motor = examplesmartmotorcontroller.ExampleSmartMotorController( 29 | constants.kMotorPort 30 | ) 31 | self.feedforward = wpimath.controller.ArmFeedforward( 32 | constants.kSVolts, 33 | constants.kGVolts, 34 | constants.kVVoltSecondPerRad, 35 | constants.kAVoltSecondSquaredPerRad, 36 | ) 37 | self.motor.setPID(constants.kP, 0, 0) 38 | 39 | def useState(self, setpoint: wpimath.trajectory.TrapezoidProfile.State) -> None: 40 | # Calculate the feedforward from the setpoint 41 | feedforward = self.feedforward.calculate(setpoint.position, setpoint.velocity) 42 | 43 | # Add the feedforward to the PID output to get the motor output 44 | self.motor.setSetPoint( 45 | examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, 46 | setpoint.position, 47 | feedforward / 12.0, 48 | ) 49 | 50 | def setArmGoalCommand(self, kArmOffsetRads: float) -> commands2.Command: 51 | return commands2.cmd.runOnce(lambda: self.setGoal(kArmOffsetRads), self) 52 | -------------------------------------------------------------------------------- /Timed/tests/basic_test.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | # 8 | # This is a pytest based testing system. Anything you can do with 9 | # pytest, you can do with this too. There are a few magic parameters 10 | # provided as fixtures that will allow your tests to access the robot 11 | # code and stuff 12 | # 13 | # control - a pyfrc.test_support.controller.TestController object 14 | # robot - This is whatever is returned from the run function in robot.py 15 | # robot_file - The filename of robot.py 16 | # robot_path - The directory name that robot.py is located in 17 | # 18 | 19 | import wpilib.simulation 20 | 21 | import pytest 22 | 23 | 24 | def test_operator_control(control, robot): 25 | """ 26 | This test checks to see if a joystick input causes a corresponding 27 | motor output. Obviously this is a silly example, but you can build 28 | upon it to make much more comprehensive (and stateful) tests. 29 | 30 | Keep in mind that when you set a value, the robot code does not 31 | see the value immediately, but only when teleopPeriodic is called, 32 | which is typically every 20ms 33 | """ 34 | 35 | # run_robot will cause the robot to be initialized and robotInit to be called 36 | with control.run_robot(): 37 | motorsim = wpilib.simulation.PWMSim(robot.motor.getChannel()) 38 | joysim = wpilib.simulation.JoystickSim(robot.lstick) 39 | 40 | # Set the joystick value to something 41 | joysim.setY(0.5) 42 | 43 | # Enable the robot, check to see if it was set 44 | control.step_timing(seconds=0.1, autonomous=False, enabled=True) 45 | assert 0.5 == pytest.approx(motorsim.getSpeed(), rel=0.01) 46 | 47 | # change it, see if it's still set 48 | joysim.setY(0.2) 49 | control.step_timing(seconds=0.1, autonomous=False, enabled=True) 50 | assert 0.2 == pytest.approx(motorsim.getSpeed(), rel=0.01) 51 | -------------------------------------------------------------------------------- /ElevatorTrapezoidProfile/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpimath.controller 10 | from wpimath.trajectory import TrapezoidProfile 11 | 12 | import examplesmartmotorcontroller 13 | 14 | 15 | class MyRobot(wpilib.TimedRobot): 16 | kDt = 0.02 17 | 18 | def robotInit(self): 19 | self.joystick = wpilib.Joystick(1) 20 | self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1) 21 | # Note: These gains are fake, and will have to be tuned for your robot. 22 | self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 1.5) 23 | 24 | # Create a motion profile with the given maximum velocity and maximum 25 | # acceleration constraints for the next setpoint. 26 | self.profile = TrapezoidProfile(TrapezoidProfile.Constraints(1.75, 0.75)) 27 | 28 | self.goal = TrapezoidProfile.State() 29 | self.setpoint = TrapezoidProfile.State() 30 | 31 | # Note: These gains are fake, and will have to be tuned for your robot. 32 | self.motor.setPID(1.3, 0.0, 0.7) 33 | 34 | def teleopPeriodic(self): 35 | if self.joystick.getRawButtonPressed(2): 36 | self.goal = TrapezoidProfile.State(5, 0) 37 | elif self.joystick.getRawButtonPressed(3): 38 | self.goal = TrapezoidProfile.State(0, 0) 39 | 40 | # Retrieve the profiled setpoint for the next timestep. This setpoint moves 41 | # toward the goal while obeying the constraints. 42 | self.setpoint = self.profile.calculate(self.kDt, self.setpoint, self.goal) 43 | 44 | # Send setpoint to offboard controller PID 45 | self.motor.setSetPoint( 46 | examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition, 47 | self.setpoint.position, 48 | self.feedforward.calculate(self.setpoint.velocity) / 12, 49 | ) 50 | -------------------------------------------------------------------------------- /GyroDriveCommands/commands/turntoangle.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | import commands2 9 | import commands2.cmd 10 | import wpimath.controller 11 | 12 | from subsystems.drivesubsystem import DriveSubsystem 13 | 14 | import constants 15 | 16 | 17 | class TurnToAngle(commands2.PIDCommand): 18 | """A command that will turn the robot to the specified angle.""" 19 | 20 | def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: 21 | """ 22 | Turns to robot to the specified angle. 23 | 24 | :param: targetAngleDegrees The angle to turn to 25 | :param: drive The drive subsystem to 26 | """ 27 | super().__init__( 28 | wpimath.controller.PIDController( 29 | constants.DriveConstants.kTurnP, 30 | constants.DriveConstants.kTurnI, 31 | constants.DriveConstants.kTurnD, 32 | ), 33 | # Close loop on heading 34 | drive.getHeading, 35 | # Set reference to target 36 | targetAngleDegrees, 37 | # Pipe output to turn robot 38 | lambda output: drive.arcadeDrive(0, output), 39 | # Require the drive 40 | drive, 41 | ) 42 | 43 | # Set the controller to be continuous (because it is an angle controller) 44 | self.getController().enableContinuousInput(-180, 180) 45 | 46 | # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the 47 | # setpoint before it is considered as having reached the reference 48 | self.getController().setTolerance( 49 | constants.DriveConstants.kTurnToleranceDeg, 50 | constants.DriveConstants.kTurnRateToleranceDegPerS, 51 | ) 52 | 53 | def isFinished(self) -> bool: 54 | # End when the controller is at the reference. 55 | return self.getController().atSetpoint() 56 | -------------------------------------------------------------------------------- /IntermediateVision/vision.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | # 8 | # This is a demo program showing the use of OpenCV to do vision processing. The image is acquired 9 | # from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has 10 | # many methods for different types of processing. 11 | # 12 | # NOTE: This code runs in its own process, so we cannot access the robot here, 13 | # nor can we create/use/see wpilib objects 14 | # 15 | # To try this code out locally (if you have robotpy-cscore installed), you 16 | # can execute `python3 -m cscore vision.py:main` 17 | # 18 | 19 | import cv2 20 | import numpy as np 21 | 22 | from cscore import CameraServer as CS 23 | 24 | 25 | def main(): 26 | CS.enableLogging() 27 | 28 | # Get the UsbCamera from CameraServer 29 | camera = CS.startAutomaticCapture() 30 | # Set the resolution 31 | camera.setResolution(640, 480) 32 | 33 | # Get a CvSink. This will capture images from the camera 34 | cvSink = CS.getVideo() 35 | # Setup a CvSource. This will send images back to the Dashboard 36 | outputStream = CS.putVideo("Rectangle", 640, 480) 37 | 38 | # Allocating new images is very expensive, always try to preallocate 39 | mat = np.zeros(shape=(480, 640, 3), dtype=np.uint8) 40 | 41 | while True: 42 | # Tell the CvSink to grab a frame from the camera and put it 43 | # in the source image. If there is an error notify the output. 44 | time, mat = cvSink.grabFrame(mat) 45 | if time == 0: 46 | # Send the output the error. 47 | outputStream.notifyError(cvSink.getError()) 48 | # skip the rest of the current iteration 49 | continue 50 | 51 | # Put a rectangle on the image 52 | cv2.rectangle(mat, (100, 100), (400, 400), (255, 255, 255), 5) 53 | 54 | # Give the output stream a new image to display 55 | outputStream.putFrame(mat) 56 | -------------------------------------------------------------------------------- /Gyro/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a sample program to demonstrate how to use a gyro sensor to make a robot drive straight. 15 | This program uses a joystick to drive forwards and backwards while the gyro is used for direction 16 | keeping. 17 | """ 18 | 19 | kAngleSetpoint = 0.0 20 | kP = 0.005 # propotional turning constant 21 | 22 | # gyro calibration constant, may need to be adjusted; 23 | # gyro value of 360 is set to correspond to one full revolution 24 | kVoltsPerDegreePerSecond = 0.0128 25 | 26 | kLeftMotorPort = 0 27 | kRightMotorPort = 1 28 | kGyroPort = 0 29 | kJoystickPort = 0 30 | 31 | def robotInit(self): 32 | """Robot initialization function""" 33 | 34 | self.leftDrive = wpilib.PWMSparkMax(self.kLeftMotorPort) 35 | self.rightDrive = wpilib.PWMSparkMax(self.kRightMotorPort) 36 | self.myRobot = wpilib.drive.DifferentialDrive(self.leftDrive, self.rightDrive) 37 | self.gyro = wpilib.AnalogGyro(self.kGyroPort) 38 | self.joystick = wpilib.Joystick(self.kJoystickPort) 39 | 40 | self.gyro.setSensitivity(self.kVoltsPerDegreePerSecond) 41 | 42 | # We need to invert one side of the drivetrain so that positive voltages 43 | # result in both sides moving forward. Depending on how your robot's 44 | # gearbox is constructed, you might have to invert the left side instead. 45 | self.rightDrive.setInverted(True) 46 | 47 | def teleopPeriodic(self): 48 | # The motor speed is set from the joystick while the DifferentialDrive turning value is assigned 49 | # from the error between the setpoint and the gyro angle. 50 | turningValue = (self.kAngleSetpoint - self.gyro.getAngle()) * self.kP 51 | self.myRobot.arcadeDrive(-self.joystick.getY(), -turningValue) 52 | -------------------------------------------------------------------------------- /FrisbeeBot/subsystems/shootersubsystem.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | import wpimath.controller 9 | import commands2 10 | import commands2.cmd 11 | 12 | import constants 13 | 14 | 15 | class ShooterSubsystem(commands2.PIDSubsystem): 16 | def __init__(self) -> None: 17 | super().__init__( 18 | wpimath.controller.PIDController( 19 | constants.ShooterConstants.kP, 20 | constants.ShooterConstants.kI, 21 | constants.ShooterConstants.kD, 22 | ) 23 | ) 24 | 25 | self.shooterMotor = wpilib.PWMSparkMax( 26 | constants.ShooterConstants.kShooterMotorPort 27 | ) 28 | self.feederMotor = wpilib.PWMSparkMax( 29 | constants.ShooterConstants.kFeederMotorPort 30 | ) 31 | self.shooterEncoder = wpilib.Encoder( 32 | constants.ShooterConstants.kEncoderPorts[0], 33 | constants.ShooterConstants.kEncoderPorts[1], 34 | constants.ShooterConstants.kEncoderReversed, 35 | ) 36 | self.shooterFeedForward = wpimath.controller.SimpleMotorFeedforwardMeters( 37 | constants.ShooterConstants.kSVolts, 38 | constants.ShooterConstants.kVVoltSecondsPerRotation, 39 | ) 40 | self.getController().setTolerance( 41 | constants.ShooterConstants.kShooterToleranceRPS 42 | ) 43 | self.shooterEncoder.setDistancePerPulse(constants.ShooterConstants.kEncoderCPR) 44 | self.setSetpoint(constants.ShooterConstants.kShooterTargetRPS) 45 | 46 | def useOutput(self, output: float, setpoint: float): 47 | self.shooterMotor.setVoltage( 48 | output + self.shooterFeedForward.calculate(setpoint) 49 | ) 50 | 51 | def getMeasurement(self) -> float: 52 | return self.shooterEncoder.getRate() 53 | 54 | def runFeeder(self): 55 | self.feederMotor.set(constants.ShooterConstants.kFeederSpeed) 56 | 57 | def stopFeeder(self): 58 | self.feederMotor.set(0) 59 | -------------------------------------------------------------------------------- /StatefulAutonomous/autonomous/feature_example.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | This autonomous mode doesn't do anything particularly useful, it's just 9 | here to test out features and make sure that things work. 10 | The motors will timeout if you do not set them to 0. 11 | """ 12 | 13 | from robotpy_ext.autonomous import StatefulAutonomous, state, timed_state 14 | 15 | 16 | class FeatureExample(StatefulAutonomous): 17 | MODE_NAME = "Feature Example" 18 | 19 | def initialize(self): 20 | self.initial_called = None 21 | 22 | # 23 | # Weird argument combinations are supported 24 | # 25 | 26 | @state(first=True) 27 | def first(self): 28 | self.next_state("weird0") 29 | 30 | @timed_state(duration=0.5, next_state="weird1") 31 | def weird0(self): 32 | pass 33 | 34 | @timed_state(duration=0.5, next_state="weird2") 35 | def weird1(self, tm): 36 | pass 37 | 38 | @timed_state(duration=0.5, next_state="weird3") 39 | def weird2(self, state_tm): 40 | pass 41 | 42 | @timed_state(duration=0.5, next_state="weird4") 43 | def weird3(self, state_tm, tm): 44 | pass 45 | 46 | @timed_state(duration=0.5, next_state="initial_call_test") 47 | def weird4(self, tm, state_tm): 48 | pass 49 | 50 | @state() 51 | def weird5(self, tm): 52 | # Just make sure the timing mechanism works 53 | assert int(tm) == 2 54 | self.next_state("initial_call_test") 55 | 56 | @state() 57 | def initial_call_test(self, initial_call): 58 | if initial_call: 59 | assert self.initial_called is None 60 | self.initial_called = True 61 | else: 62 | assert self.initial_called == True 63 | self.next_state("none1") 64 | 65 | @state() 66 | def none1(self, initial_call): 67 | self.done() 68 | 69 | # Run for N number of iterations 70 | 71 | # Just do something until something changes 72 | 73 | # @state() 74 | 75 | # sdef 76 | -------------------------------------------------------------------------------- /Ultrasonic/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | from wpilib.shuffleboard import Shuffleboard 10 | from wpilib import SmartDashboard 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | """ 15 | This is a sample program demonstrating how to read from a ping-response ultrasonic sensor with 16 | the :class:`.Ultrasonic` class. 17 | """ 18 | 19 | def robotInit(self): 20 | # Creates a ping-response Ultrasonic object on DIO 1 and 2. 21 | self.rangeFinder = wpilib.Ultrasonic(1, 2) 22 | 23 | # Add the ultrasonic to the "Sensors" tab of the dashboard 24 | # Data will update automatically 25 | Shuffleboard.getTab("Sensors").add(self.rangeFinder) 26 | 27 | def teleopPeriodic(self): 28 | # We can read the distance in millimeters 29 | distanceMillimeters = self.rangeFinder.getRangeMM() 30 | # ... or in inches 31 | distanceInches = self.rangeFinder.getRangeInches() 32 | 33 | # We can also publish the data itself periodically 34 | SmartDashboard.putNumber("Distance[mm]", distanceMillimeters) 35 | SmartDashboard.putNumber("Distance[in]", distanceInches) 36 | 37 | def testInit(self): 38 | # By default, the Ultrasonic class polls all ultrasonic sensors every in a round-robin to prevent 39 | # them from interfering from one another. 40 | # However, manual polling is also possible -- notes that this disables automatic mode! 41 | self.rangeFinder.ping() 42 | 43 | def testPeriodic(self): 44 | if self.rangeFinder.isRangeValid(): 45 | # Data is valid, publish it 46 | SmartDashboard.putNumber("Distance[mm]", self.rangeFinder.getRangeMM()) 47 | SmartDashboard.putNumber("Distance[in]", self.rangeFinder.getRangeInches()) 48 | 49 | # Ping for next measurement 50 | self.rangeFinder.ping() 51 | 52 | def testExit(self): 53 | # Enable automatic mode 54 | self.rangeFinder.setAutomaticMode(True) 55 | -------------------------------------------------------------------------------- /I2CCommunication/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """ 13 | This is a sample program demonstrating how to communicate to a light controller from the robot 14 | code using the roboRIO's I2C port. 15 | """ 16 | 17 | PORT = wpilib.I2C.Port.kOnboard 18 | DEVICE_ADDRESS = 4 19 | 20 | def robotInit(self): 21 | """Robot initialization function""" 22 | 23 | self.arduino = wpilib.I2C(self.PORT, self.DEVICE_ADDRESS) 24 | 25 | def writeString(self, s: str): 26 | # Creates a char array from the input string 27 | chars = s.encode("ascii") 28 | 29 | # Writes bytes over I2C 30 | self.arduino.writeBulk(chars) 31 | 32 | def robotPeriodic(self): 33 | # Creates a string to hold current robot state information, including 34 | # alliance, enabled state, operation mode, and match time. The message 35 | # is sent in format "AEM###" where A is the alliance color, (R)ed or 36 | # (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the 37 | # operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded 38 | # time remaining in the match. 39 | # 40 | # For example, "RET043" would indicate that the robot is on the red 41 | # alliance, enabled in teleop mode, with 43 seconds left in the match. 42 | allianceString = "U" 43 | alliance = wpilib.DriverStation.getAlliance() 44 | if alliance is not None: 45 | allianceString = ( 46 | "R" if alliance == wpilib.DriverStation.Alliance.kRed else "B" 47 | ) 48 | 49 | enabledString = "E" if wpilib.DriverStation.isEnabled() else "D" 50 | autoString = "A" if wpilib.DriverStation.isAutonomous() else "T" 51 | matchTime = wpilib.DriverStation.getMatchTime() 52 | 53 | stateMessage = f"{allianceString}{enabledString}{autoString}{matchTime:03f}" 54 | 55 | self.writeString(stateMessage) 56 | -------------------------------------------------------------------------------- /FlywheelBangBangController/physics.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import typing 8 | 9 | import wpilib.simulation 10 | from wpimath.system import plant 11 | 12 | from pyfrc.physics.core import PhysicsInterface 13 | 14 | if typing.TYPE_CHECKING: 15 | from robot import MyRobot 16 | 17 | 18 | class PhysicsEngine: 19 | """ 20 | Simulates a flywheel 21 | """ 22 | 23 | def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): 24 | """ 25 | :param physics_controller: `pyfrc.physics.core.Physics` object 26 | to communicate simulation effects to 27 | """ 28 | 29 | self.physics_controller = physics_controller 30 | 31 | # Motors 32 | self.flywheelMotor = wpilib.simulation.PWMSim(robot.flywheelMotor.getChannel()) 33 | 34 | # Sensors 35 | self.encoder = wpilib.simulation.EncoderSim(robot.encoder) 36 | 37 | # Flywheel 38 | self.gearbox = plant.DCMotor.NEO(1) 39 | self.plant = plant.LinearSystemId.flywheelSystem( 40 | self.gearbox, robot.kFlywheelGearing, robot.kFlywheelMomentOfInertia 41 | ) 42 | self.flywheel = wpilib.simulation.FlywheelSim(self.plant, self.gearbox) 43 | 44 | def update_sim(self, now: float, tm_diff: float) -> None: 45 | """ 46 | Called when the simulation parameters for the program need to be 47 | updated. 48 | 49 | :param now: The current time as a float 50 | :param tm_diff: The amount of time that has passed since the last 51 | time that this function was called 52 | """ 53 | 54 | # To update our simulation, we set motor voltage inputs, update the 55 | # simulation, and write the simulated velocities to our simulated encoder 56 | self.flywheel.setInputVoltage( 57 | self.flywheelMotor.getSpeed() * wpilib.RobotController.getInputVoltage() 58 | ) 59 | self.flywheel.update(tm_diff) 60 | self.encoder.setRate(self.flywheel.getAngularVelocity()) 61 | -------------------------------------------------------------------------------- /RamseteCommand/constants.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | A place for the constant values in the code that may be used in more than one place. 9 | This offers a convenient resources to teams who need to make both quick and universal 10 | changes. 11 | """ 12 | 13 | from wpimath.kinematics import DifferentialDriveKinematics 14 | 15 | import math 16 | 17 | # ID for the driver's joystick. 18 | kDriverControllerPort = 0 19 | 20 | # The PWM IDs for the drivetrain motor controllers. 21 | kLeftMotor1Port = 0 22 | kLeftMotor2Port = 1 23 | kRightMotor1Port = 2 24 | kRightMotor2Port = 3 25 | 26 | # Encoders and their respective motor controllers. 27 | kLeftEncoderPorts = (0, 1) 28 | kRightEncoderPorts = (2, 3) 29 | kLeftEncoderReversed = False 30 | kRightEncoderReversed = True 31 | 32 | # In meters, distance between wheels on each side of robot. 33 | kTrackWidthMeters = 0.69 34 | kDriveKinematics = DifferentialDriveKinematics(kTrackWidthMeters) 35 | 36 | # Encoder counts per revolution/rotation. 37 | kEncoderCPR = 1024 38 | kWheelDiameterMeters = 0.15 39 | 40 | # Assumes the encoders are directly mounted on the wheel shafts 41 | kEncoderDistancePerPulse = (kWheelDiameterMeters * math.pi) / kEncoderCPR 42 | 43 | # These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! 44 | # These characterization values MUST be determined either experimentally or theoretically 45 | # for *your* robot's drive. 46 | # The Robot Characterization Toolsuite provides a convenient tool for obtaining these 47 | # values for your robot. 48 | ksVolts = 0.22 49 | kvVoltSecondsPerMeter = 1.98 50 | kaVoltSecondsSquaredPerMeter = 0.2 51 | 52 | # Example value only - as above, this must be tuned for your drive! 53 | kPDriveVel = 8.5 54 | 55 | # The max velocity and acceleration for our autonomous. 56 | kMaxSpeedMetersPerSecond = 3 57 | kMaxAccelerationMetersPerSecondSquared = 1 58 | 59 | # Reasonable baseline values for a RAMSETE follower in units of meters and seconds. 60 | kRamseteB = 2 61 | kRamseteZeta = 0.7 62 | 63 | # The number of motors on the robot. 64 | kDrivetrainMotorCount = 4 65 | -------------------------------------------------------------------------------- /Mechanism2d/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """ 13 | This sample program shows how to use Mechanism2d - a visual representation of arms, elevators, 14 | and other mechanisms on dashboards; driven by a node-based API. 15 | 16 | Ligaments are based on other ligaments or roots, and roots are contained in the base 17 | Mechanism2d object. 18 | """ 19 | 20 | kMetersPerPulse = 0.01 21 | kElevatorMinimumLength = 0.5 22 | 23 | def robotInit(self): 24 | self.elevatorMotor = wpilib.PWMSparkMax(0) 25 | self.wristMotor = wpilib.PWMSparkMax(1) 26 | self.wristPot = wpilib.AnalogPotentiometer(1, 90) 27 | self.elevatorEncoder = wpilib.Encoder(0, 1) 28 | self.joystick = wpilib.Joystick(0) 29 | 30 | self.elevatorEncoder.setDistancePerPulse(self.kMetersPerPulse) 31 | 32 | # the main mechanism object 33 | self.mech = wpilib.Mechanism2d(3, 3) 34 | # the mechanism root node 35 | self.root = self.mech.getRoot("climber", 2, 0) 36 | 37 | # MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based 38 | # off the root node or another ligament object 39 | self.elevator = self.root.appendLigament( 40 | "elevator", self.kElevatorMinimumLength, 90 41 | ) 42 | self.wrist = self.elevator.appendLigament( 43 | "wrist", 0.5, 90, 6, wpilib.Color8Bit(wpilib.Color.kPurple) 44 | ) 45 | 46 | # post the mechanism to the dashboard 47 | wpilib.SmartDashboard.putData("Mech2d", self.mech) 48 | 49 | def robotPeriodic(self): 50 | # update the dashboard mechanism's state 51 | self.elevator.setLength( 52 | self.kElevatorMinimumLength + self.elevatorEncoder.getDistance() 53 | ) 54 | self.wrist.setAngle(self.wristPot.get()) 55 | 56 | def teleopPeriodic(self): 57 | self.elevatorMotor.set(self.joystick.getRawAxis(0)) 58 | self.wristMotor.set(self.joystick.getRawAxis(1)) 59 | -------------------------------------------------------------------------------- /MecanumBot/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | import wpimath.filter 11 | 12 | from drivetrain import Drivetrain 13 | 14 | 15 | class MyRobot(wpilib.TimedRobot): 16 | def robotInit(self): 17 | self.controller = wpilib.XboxController(0) 18 | self.mecanum = Drivetrain() 19 | 20 | # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. 21 | self.xspeedLimiter = wpimath.filter.SlewRateLimiter(3) 22 | self.yspeedLimiter = wpimath.filter.SlewRateLimiter(3) 23 | self.rotLimiter = wpimath.filter.SlewRateLimiter(3) 24 | 25 | def autonomousPeriodic(self): 26 | self._driveWithJoystick(False) 27 | self.mecanum.updateOdometry() 28 | 29 | def teleopPeriodic(self): 30 | self._driveWithJoystick(True) 31 | 32 | def _driveWithJoystick(self, fieldRelative: bool): 33 | # Get the x speed. We are inverting this because Xbox controllers return 34 | # negative values when we push forward. 35 | xSpeed = ( 36 | -self.xspeedLimiter.calculate(self.controller.getLeftY()) 37 | * Drivetrain.kMaxSpeed 38 | ) 39 | 40 | # Get the y speed or sideways/strafe speed. We are inverting this because 41 | # we want a positive value when we pull to the left. Xbox controllers 42 | # return positive values when you pull to the right by default. 43 | ySpeed = ( 44 | -self.yspeedLimiter.calculate(self.controller.getLeftX()) 45 | * Drivetrain.kMaxSpeed 46 | ) 47 | 48 | # Get the rate of angular rotation. We are inverting this because we want a 49 | # positive value when we pull to the left (remember, CCW is positive in 50 | # mathematics). Xbox controllers return positive values when you pull to 51 | # the right by default. 52 | rot = ( 53 | -self.rotLimiter.calculate(self.controller.getRightX()) 54 | * Drivetrain.kMaxAngularSpeed 55 | ) 56 | 57 | self.mecanum.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) 58 | -------------------------------------------------------------------------------- /GettingStarted/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | def robotInit(self): 14 | """ 15 | This function is called upon program startup and 16 | should be used for any initialization code. 17 | """ 18 | self.leftDrive = wpilib.PWMSparkMax(0) 19 | self.rightDrive = wpilib.PWMSparkMax(1) 20 | self.robotDrive = wpilib.drive.DifferentialDrive( 21 | self.leftDrive, self.rightDrive 22 | ) 23 | self.controller = wpilib.XboxController(0) 24 | self.timer = wpilib.Timer() 25 | 26 | # We need to invert one side of the drivetrain so that positive voltages 27 | # result in both sides moving forward. Depending on how your robot's 28 | # gearbox is constructed, you might have to invert the left side instead. 29 | self.rightDrive.setInverted(True) 30 | 31 | def autonomousInit(self): 32 | """This function is run once each time the robot enters autonomous mode.""" 33 | self.timer.restart() 34 | 35 | def autonomousPeriodic(self): 36 | """This function is called periodically during autonomous.""" 37 | 38 | # Drive for two seconds 39 | if self.timer.get() < 2.0: 40 | # Drive forwards half speed, make sure to turn input squaring off 41 | self.robotDrive.arcadeDrive(0.5, 0, squareInputs=False) 42 | else: 43 | self.robotDrive.stopMotor() # Stop robot 44 | 45 | def teleopInit(self): 46 | """This function is called once each time the robot enters teleoperated mode.""" 47 | 48 | def teleopPeriodic(self): 49 | """This function is called periodically during teleoperated mode.""" 50 | self.robotDrive.arcadeDrive( 51 | -self.controller.getLeftY(), -self.controller.getRightX() 52 | ) 53 | 54 | def testInit(self): 55 | """This function is called once each time the robot enters test mode.""" 56 | 57 | def testPeriodic(self): 58 | """This function is called periodically during test mode.""" 59 | -------------------------------------------------------------------------------- /UltrasonicPID/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpilib.drive 10 | import wpimath.controller 11 | import wpimath.filter 12 | 13 | 14 | class MyRobot(wpilib.TimedRobot): 15 | """ 16 | This is a sample program to demonstrate the use of a PIDController with an ultrasonic sensor to 17 | reach and maintain a set distance from an object. 18 | """ 19 | 20 | # distance the robot wants to stay from an object 21 | # (one meter) 22 | kHoldDistanceMillimeters = 1.0e3 23 | 24 | # proportional speed constant 25 | # negative because applying positive voltage will bring us closer to the target 26 | kP = -0.001 27 | # integral speed constant 28 | kI = 0.0 29 | # derivative speed constant 30 | kD = 0.0 31 | 32 | kLeftMotorPort = 0 33 | kRightMotorPort = 1 34 | 35 | kUltrasonicPingPort = 0 36 | kUltrasonicEchoPort = 1 37 | 38 | def robotInit(self): 39 | # Ultrasonic sensors tend to be quite noisy and susceptible to sudden outliers, 40 | # so measurements are filtered with a 5-sample median filter 41 | self.filter = wpimath.filter.MedianFilter(5) 42 | 43 | self.ultrasonic = wpilib.Ultrasonic( 44 | self.kUltrasonicPingPort, self.kUltrasonicEchoPort 45 | ) 46 | self.leftMotor = wpilib.PWMSparkMax(self.kLeftMotorPort) 47 | self.rightMotor = wpilib.PWMSparkMax(self.kRightMotorPort) 48 | self.robotDrive = wpilib.drive.DifferentialDrive( 49 | self.leftMotor, self.rightMotor 50 | ) 51 | self.pidController = wpimath.controller.PIDController(self.kP, self.kI, self.kD) 52 | 53 | def autonomousInit(self): 54 | # Set setpoint of the pid controller 55 | self.pidController.setSetpoint(self.kHoldDistanceMillimeters) 56 | 57 | def autonomousPeriodic(self): 58 | measurement = self.ultrasonic.getRangeMM() 59 | filteredMeasurement = self.filter.calculate(measurement) 60 | pidOutput = self.pidController.calculate(filteredMeasurement) 61 | 62 | # disable input squaring -- PID output is linear 63 | self.robotDrive.arcadeDrive(pidOutput, 0, False) 64 | -------------------------------------------------------------------------------- /CANPDP/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | 11 | class MyRobot(wpilib.TimedRobot): 12 | """ 13 | This is a sample program showing how to retrieve information from the Power 14 | Distribution Panel via CAN. The information will be displayed under variables 15 | through the SmartDashboard. 16 | """ 17 | 18 | def robotInit(self): 19 | """Robot initialization function""" 20 | 21 | # Object for dealing with the Power Distribution Panel (PDP). 22 | self.pdp = wpilib.PowerDistribution() 23 | 24 | # Put the PDP itself to the dashboard 25 | wpilib.SmartDashboard.putData("PDP", self.pdp) 26 | 27 | def robotPeriodic(self): 28 | # Get the current going through channel 7, in Amperes. 29 | # The PDP returns the current in increments of 0.125A. 30 | # At low currents the current readings tend to be less accurate. 31 | current7 = self.pdp.getCurrent(7) 32 | wpilib.SmartDashboard.putNumber("Current Channel 7", current7) 33 | 34 | # Get the voltage going into the PDP, in Volts. 35 | # The PDP returns the voltage in increments of 0.05 Volts. 36 | voltage = self.pdp.getVoltage() 37 | wpilib.SmartDashboard.putNumber("Voltage", voltage) 38 | 39 | # Retrieves the temperature of the PDP, in degrees Celsius. 40 | temperatureCelsius = self.pdp.getTemperature() 41 | wpilib.SmartDashboard.putNumber("Temperature", temperatureCelsius) 42 | 43 | # Get the total current of all channels. 44 | totalCurrent = self.pdp.getTotalCurrent() 45 | wpilib.SmartDashboard.putNumber("Total Current", totalCurrent) 46 | 47 | # Get the total power of all channels. 48 | # Power is the bus voltage multiplied by the current with the units Watts. 49 | totalPower = self.pdp.getTotalPower() 50 | wpilib.SmartDashboard.putNumber("Total Power", totalPower) 51 | 52 | # Get the total energy of all channels. 53 | # Energy is the power summed over time with units Joules. 54 | totalEnergy = self.pdp.getTotalEnergy() 55 | wpilib.SmartDashboard.putNumber("Total Energy", totalEnergy) 56 | -------------------------------------------------------------------------------- /ArmBot/subsystems/armsubsystem.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | import commands2 9 | import wpimath.controller 10 | import wpimath.trajectory 11 | 12 | import constants 13 | 14 | 15 | class ArmSubsystem(commands2.ProfiledPIDSubsystem): 16 | """A robot arm subsystem that moves with a motion profile.""" 17 | 18 | # Create a new ArmSubsystem 19 | def __init__(self) -> None: 20 | super().__init__( 21 | wpimath.controller.ProfiledPIDController( 22 | constants.ArmConstants.kP, 23 | 0, 24 | 0, 25 | wpimath.trajectory.TrapezoidProfile.Constraints( 26 | constants.ArmConstants.kMaxVelocityRadPerSecond, 27 | constants.ArmConstants.kMaxAccelerationRadPerSecSquared, 28 | ), 29 | ), 30 | 0, 31 | ) 32 | 33 | self.motor = wpilib.PWMSparkMax(constants.ArmConstants.kMotorPort) 34 | self.encoder = wpilib.Encoder( 35 | constants.ArmConstants.kEncoderPorts[0], 36 | constants.ArmConstants.kEncoderPorts[1], 37 | ) 38 | self.feedforward = wpimath.controller.ArmFeedforward( 39 | constants.ArmConstants.kSVolts, 40 | constants.ArmConstants.kGVolts, 41 | constants.ArmConstants.kVVoltSecondPerRad, 42 | constants.ArmConstants.kAVoltSecondSquaredPerRad, 43 | ) 44 | 45 | self.encoder.setDistancePerPulse( 46 | constants.ArmConstants.kEncoderDistancePerPulse 47 | ) 48 | 49 | # Start arm at rest in neutral position 50 | self.setGoal(constants.ArmConstants.kArmOffsetRads) 51 | 52 | def useOutput( 53 | self, output: float, setpoint: wpimath.trajectory.TrapezoidProfile.State 54 | ) -> None: 55 | # Calculate the feedforward from the setpoint 56 | feedforward = self.feedforward.calculate(setpoint.position, setpoint.velocity) 57 | 58 | # Add the feedforward to the PID output to get the motor output 59 | self.motor.setVoltage(output + feedforward) 60 | 61 | def getMeasurement(self) -> float: 62 | return self.encoder.getDistance() + constants.ArmConstants.kArmOffsetRads 63 | -------------------------------------------------------------------------------- /HatchbotInlined/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import typing 9 | import wpilib 10 | import commands2 11 | 12 | from robotcontainer import RobotContainer 13 | 14 | 15 | class MyRobot(commands2.TimedCommandRobot): 16 | """ 17 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 18 | has an implementation of robotPeriodic which runs the scheduler for you 19 | """ 20 | 21 | autonomousCommand: typing.Optional[commands2.Command] = None 22 | 23 | def robotInit(self) -> None: 24 | """ 25 | This function is run when the robot is first started up and should be used for any 26 | initialization code. 27 | """ 28 | 29 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 30 | # autonomous chooser on the dashboard. 31 | self.container = RobotContainer() 32 | 33 | def disabledInit(self) -> None: 34 | """This function is called once each time the robot enters Disabled mode.""" 35 | 36 | def disabledPeriodic(self) -> None: 37 | """This function is called periodically when disabled""" 38 | 39 | def autonomousInit(self) -> None: 40 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 41 | self.autonomousCommand = self.container.getAutonomousCommand() 42 | 43 | if self.autonomousCommand: 44 | self.autonomousCommand.schedule() 45 | 46 | def autonomousPeriodic(self) -> None: 47 | """This function is called periodically during autonomous""" 48 | 49 | def teleopInit(self) -> None: 50 | # This makes sure that the autonomous stops running when 51 | # teleop starts running. If you want the autonomous to 52 | # continue until interrupted by another command, remove 53 | # this line or comment it out. 54 | if self.autonomousCommand: 55 | self.autonomousCommand.cancel() 56 | 57 | def teleopPeriodic(self) -> None: 58 | """This function is called periodically during operator control""" 59 | 60 | def testInit(self) -> None: 61 | # Cancels all running commands at the start of test mode 62 | commands2.CommandScheduler.getInstance().cancelAll() 63 | -------------------------------------------------------------------------------- /ArmSimulation/subsytems/arm.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | 9 | from wpimath.controller import PIDController 10 | from wpimath.system.plant import DCMotor 11 | from wpimath import units 12 | 13 | from wpilib.simulation import BatterySim, EncoderSim, RoboRioSim, SingleJointedArmSim 14 | 15 | from constants import Constants 16 | 17 | 18 | class Arm: 19 | def __init__(self): 20 | # The P gain for the PID controller that drives this arm. 21 | self.armKp = Constants.kDefaultArmKp 22 | self.armSetpointDegrees = Constants.kDefaultArmSetpointDegrees 23 | 24 | # The arm gearbox represents a gearbox containing two Vex 775pro motors. 25 | self.armGearbox = DCMotor.vex775Pro(2) 26 | 27 | # Standard classes for controlling our arm 28 | self.controller = PIDController(self.armKp, 0, 0) 29 | self.encoder = wpilib.Encoder( 30 | Constants.kEncoderAChannel, Constants.kEncoderBChannel 31 | ) 32 | self.motor = wpilib.PWMSparkMax(Constants.kMotorPort) 33 | 34 | # Subsystem constructor. 35 | self.encoder.setDistancePerPulse(Constants.kArmEncoderDistPerPulse) 36 | 37 | # Set the Arm position setpoint and P constant to Preferences if the keys don't already exist 38 | wpilib.Preferences.initDouble( 39 | Constants.kArmPositionKey, self.armSetpointDegrees 40 | ) 41 | wpilib.Preferences.initDouble(Constants.kArmPKey, self.armKp) 42 | 43 | def loadPreferences(self): 44 | # Read Preferences for Arm setpoint and kP on entering Teleop 45 | self.armSetpointDegrees = wpilib.Preferences.getDouble( 46 | Constants.kArmPositionKey, self.armSetpointDegrees 47 | ) 48 | if self.armKp != wpilib.Preferences.getDouble(Constants.kArmPKey, self.armKp): 49 | self.armKp = wpilib.Preferences.getDouble(Constants.kArmPKey, self.armKp) 50 | self.controller.setP(self.armKp) 51 | 52 | def reachSetpoint(self): 53 | pidOutput = self.controller.calculate( 54 | self.encoder.getDistance(), 55 | units.degreesToRadians(self.armSetpointDegrees), 56 | ) 57 | self.motor.setVoltage(pidOutput) 58 | 59 | def stop(self): 60 | self.motor.set(0.0) 61 | -------------------------------------------------------------------------------- /GyroDriveCommands/commands/turntoangleprofiled.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib 8 | import commands2 9 | import commands2.cmd 10 | import wpimath.controller 11 | import wpimath.trajectory 12 | 13 | from subsystems.drivesubsystem import DriveSubsystem 14 | 15 | import constants 16 | 17 | 18 | class TurnToAngleProfiled(commands2.ProfiledPIDCommand): 19 | """A command that will turn the robot to the specified angle using a motion profile.""" 20 | 21 | def __init__(self, targetAngleDegrees: float, drive: DriveSubsystem) -> None: 22 | """ 23 | Turns to robot to the specified angle. 24 | 25 | :param: targetAngleDegrees The angle to turn to 26 | :param: drive The drive subsystem to 27 | """ 28 | super().__init__( 29 | wpimath.controller.ProfiledPIDController( 30 | constants.DriveConstants.kTurnP, 31 | constants.DriveConstants.kTurnI, 32 | constants.DriveConstants.kTurnD, 33 | wpimath.trajectory.TrapezoidProfile.Constraints( 34 | constants.DriveConstants.kMaxTurnRateDegPerS, 35 | constants.DriveConstants.kMaxTurnAccelerationDegPerSSquared, 36 | ), 37 | ), 38 | # Close loop on heading 39 | drive.getHeading, 40 | # Set reference to target 41 | targetAngleDegrees, 42 | # Pipe output to turn robot 43 | lambda output, setpoint: drive.arcadeDrive(0, output), 44 | # Require the drive 45 | drive, 46 | ) 47 | 48 | # Set the controller to be continuous (because it is an angle controller) 49 | self.getController().enableContinuousInput(-180, 180) 50 | 51 | # Set the controller tolerance - the delta tolerance ensures the robot is stationary at the 52 | # setpoint before it is considered as having reached the reference 53 | self.getController().setTolerance( 54 | constants.DriveConstants.kTurnToleranceDeg, 55 | constants.DriveConstants.kTurnRateToleranceDegPerS, 56 | ) 57 | 58 | def isFinished(self) -> bool: 59 | # End when the controller is at the reference. 60 | return self.getController().atSetpoint() 61 | -------------------------------------------------------------------------------- /HatchbotTraditional/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import typing 9 | import wpilib 10 | import commands2 11 | 12 | from robotcontainer import RobotContainer 13 | 14 | 15 | class MyRobot(commands2.TimedCommandRobot): 16 | """ 17 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 18 | has an implementation of robotPeriodic which runs the scheduler for you 19 | """ 20 | 21 | autonomousCommand: typing.Optional[commands2.Command] = None 22 | 23 | def robotInit(self) -> None: 24 | """ 25 | This function is run when the robot is first started up and should be used for any 26 | initialization code. 27 | """ 28 | 29 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 30 | # autonomous chooser on the dashboard. 31 | self.container = RobotContainer() 32 | 33 | def disabledInit(self) -> None: 34 | """This function is called once each time the robot enters Disabled mode.""" 35 | 36 | def disabledPeriodic(self) -> None: 37 | """This function is called periodically when disabled""" 38 | 39 | def autonomousInit(self) -> None: 40 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 41 | self.autonomousCommand = self.container.getAutonomousCommand() 42 | 43 | if self.autonomousCommand: 44 | self.autonomousCommand.schedule() 45 | 46 | def autonomousPeriodic(self) -> None: 47 | """This function is called periodically during autonomous""" 48 | 49 | def teleopInit(self) -> None: 50 | # This makes sure that the autonomous stops running when 51 | # teleop starts running. If you want the autonomous to 52 | # continue until interrupted by another command, remove 53 | # this line or comment it out. 54 | if self.autonomousCommand: 55 | self.autonomousCommand.cancel() 56 | 57 | def teleopPeriodic(self) -> None: 58 | """This function is called periodically during operator control""" 59 | 60 | def testInit(self) -> None: 61 | # Cancels all running commands at the start of test mode 62 | commands2.CommandScheduler.getInstance().cancelAll() 63 | -------------------------------------------------------------------------------- /PhysicsMecanum/src/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | from wpilib.drive import MecanumDrive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """Main robot class""" 14 | 15 | # Channels on the roboRIO that the motor controllers are plugged in to 16 | frontLeftChannel = 1 17 | rearLeftChannel = 2 18 | frontRightChannel = 3 19 | rearRightChannel = 4 20 | 21 | # The channel on the driver station that the joystick is connected to 22 | lStickChannel = 0 23 | rStickChannel = 1 24 | 25 | def robotInit(self): 26 | """Robot initialization function""" 27 | self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel) 28 | self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel) 29 | self.frontRightMotor = wpilib.Talon(self.frontRightChannel) 30 | self.rearRightMotor = wpilib.Talon(self.rearRightChannel) 31 | 32 | self.drive = MecanumDrive( 33 | self.frontLeftMotor, 34 | self.rearLeftMotor, 35 | self.frontRightMotor, 36 | self.rearRightMotor, 37 | ) 38 | 39 | self.lstick = wpilib.Joystick(self.lStickChannel) 40 | self.rstick = wpilib.Joystick(self.rStickChannel) 41 | 42 | # Position gets automatically updated as robot moves 43 | self.gyro = wpilib.AnalogGyro(1) 44 | 45 | def disabled(self): 46 | """Called when the robot is disabled""" 47 | while self.isDisabled(): 48 | wpilib.Timer.delay(0.01) 49 | 50 | def autonomousInit(self): 51 | """Called when autonomous mode is enabled""" 52 | self.timer = wpilib.Timer() 53 | self.timer.start() 54 | 55 | def autonomousPeriodic(self): 56 | if self.timer.get() < 2.0: 57 | self.drive.driveCartesian(0, -1, 1) 58 | else: 59 | self.drive.driveCartesian(0, 0, 0) 60 | 61 | def teleopPeriodic(self): 62 | """Called when operation control mode is enabled""" 63 | 64 | # self.drive.driveCartesian( 65 | # self.lstick.getX(), -self.lstick.getY(), self.rstick.getX(), 0 66 | # ) 67 | 68 | self.drive.driveCartesian( 69 | self.lstick.getX(), -self.lstick.getY(), self.lstick.getRawAxis(2) 70 | ) 71 | -------------------------------------------------------------------------------- /PotentiometerPID/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpimath.controller 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a sample program to demonstrate how to use a soft potentiometer and a PID controller to 15 | reach and maintain position setpoints on an elevator mechanism. 16 | """ 17 | 18 | kPotChannel = 1 19 | kMotorChannel = 7 20 | kJoystickChannel = 3 21 | 22 | # The elevator can move 1.5 meters from top to bottom 23 | kFullHeightMeters = 1.5 24 | 25 | # Bottom, middle, and top elevator setpoints 26 | kSetpointMeters = [0.2, 0.8, 1.4] 27 | 28 | # proportional, integral, and derivative speed constants 29 | # DANGER: when tuning PID constants, high/inappropriate values for kP, kI, 30 | # and kD may cause dangerous, uncontrollable, or undesired behavior! 31 | kP = 0.7 32 | kI = 0.35 33 | kD = 0.25 34 | 35 | def robotInit(self): 36 | self.pidController = wpimath.controller.PIDController(self.kP, self.kI, self.kD) 37 | # Scaling is handled internally 38 | self.potentiometer = wpilib.AnalogPotentiometer( 39 | self.kPotChannel, self.kFullHeightMeters 40 | ) 41 | self.elevatorMotor = wpilib.PWMSparkMax(self.kMotorChannel) 42 | self.joystick = wpilib.Joystick(self.kJoystickChannel) 43 | 44 | def teleopInit(self): 45 | # Move to the bottom setpoint when teleop starts 46 | self.index = 0 47 | self.pidController.setSetpoint(self.kSetpointMeters[self.index]) 48 | 49 | def teleopPeriodic(self): 50 | # Read from the sensor 51 | position = self.potentiometer.get() 52 | 53 | # Run the PID Controller 54 | pidOut = self.pidController.calculate(position) 55 | 56 | # Apply PID output 57 | self.elevatorMotor.set(pidOut) 58 | 59 | # when the button is pressed once, the selected elevator setpoint is incremented 60 | if self.joystick.getTriggerPressed(): 61 | # index of the elevator setpoint wraps around. 62 | self.index = (self.index + 1) % len(self.kSetpointMeters) 63 | print(f"m_index = {self.index}") 64 | self.pidController.setSetpoint(self.kSetpointMeters[self.index]) 65 | -------------------------------------------------------------------------------- /ArmBotOffboard/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import commands2 10 | import typing 11 | 12 | from robotcontainer import RobotContainer 13 | 14 | 15 | class MyRobot(commands2.TimedCommandRobot): 16 | """ 17 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 18 | has an implementation of robotPeriodic which runs the scheduler for you 19 | """ 20 | 21 | autonomousCommand: typing.Optional[commands2.Command] = None 22 | 23 | def robotInit(self) -> None: 24 | """ 25 | This function is run when the robot is first started up and should be used for any 26 | initialization code. 27 | """ 28 | 29 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 30 | # autonomous chooser on the dashboard. 31 | self.container = RobotContainer() 32 | 33 | def disabledInit(self) -> None: 34 | """This function is called once each time the robot enters Disabled mode.""" 35 | pass 36 | 37 | def disabledPeriodic(self) -> None: 38 | """This function is called periodically when disabled""" 39 | pass 40 | 41 | def autonomousInit(self) -> None: 42 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 43 | self.autonomousCommand = self.container.getAutonomousCommand() 44 | 45 | if self.autonomousCommand: 46 | self.autonomousCommand.schedule() 47 | 48 | def autonomousPeriodic(self) -> None: 49 | """This function is called periodically during autonomous""" 50 | pass 51 | 52 | def teleopInit(self) -> None: 53 | # This makes sure that the autonomous stops running when 54 | # teleop starts running. If you want the autonomous to 55 | # continue until interrupted by another command, remove 56 | # this line or comment it out. 57 | if self.autonomousCommand: 58 | self.autonomousCommand.cancel() 59 | 60 | def teleopPeriodic(self) -> None: 61 | """This function is called periodically during operator control""" 62 | pass 63 | 64 | def testInit(self) -> None: 65 | # Cancels all running commands at the start of test mode 66 | commands2.CommandScheduler.getInstance().cancelAll() 67 | -------------------------------------------------------------------------------- /SelectCommand/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import commands2 10 | import typing 11 | 12 | from robotcontainer import RobotContainer 13 | 14 | 15 | class MyRobot(commands2.TimedCommandRobot): 16 | """ 17 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 18 | has an implementation of robotPeriodic which runs the scheduler for you 19 | """ 20 | 21 | autonomousCommand: typing.Optional[commands2.Command] = None 22 | 23 | def robotInit(self) -> None: 24 | """ 25 | This function is run when the robot is first started up and should be used for any 26 | initialization code. 27 | """ 28 | 29 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 30 | # autonomous chooser on the dashboard. 31 | self.container = RobotContainer() 32 | 33 | def disabledInit(self) -> None: 34 | """This function is called once each time the robot enters Disabled mode.""" 35 | pass 36 | 37 | def disabledPeriodic(self) -> None: 38 | """This function is called periodically when disabled""" 39 | pass 40 | 41 | def autonomousInit(self) -> None: 42 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 43 | self.autonomousCommand = self.container.getAutonomousCommand() 44 | 45 | if self.autonomousCommand: 46 | self.autonomousCommand.schedule() 47 | 48 | def autonomousPeriodic(self) -> None: 49 | """This function is called periodically during autonomous""" 50 | pass 51 | 52 | def teleopInit(self) -> None: 53 | # This makes sure that the autonomous stops running when 54 | # teleop starts running. If you want the autonomous to 55 | # continue until interrupted by another command, remove 56 | # this line or comment it out. 57 | if self.autonomousCommand: 58 | self.autonomousCommand.cancel() 59 | 60 | def teleopPeriodic(self) -> None: 61 | """This function is called periodically during operator control""" 62 | pass 63 | 64 | def testInit(self) -> None: 65 | # Cancels all running commands at the start of test mode 66 | commands2.CommandScheduler.getInstance().cancelAll() 67 | -------------------------------------------------------------------------------- /run_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | cd "$(dirname $0)" 4 | 5 | # Keep this list alphabetically sorted 6 | BASE_TESTS=" 7 | AddressableLED 8 | AprilTagsVision 9 | ArcadeDrive 10 | ArcadeDriveXboxController 11 | ArmBot 12 | ArmBotOffboard 13 | ArmSimulation 14 | CANPDP 15 | DifferentialDriveBot 16 | DigitalCommunication 17 | DriveDistanceOffboard 18 | DutyCycleEncoder 19 | DutyCycleInput 20 | ElevatorProfiledPID 21 | ElevatorSimulation 22 | ElevatorTrapezoidProfile 23 | Encoder 24 | FlywheelBangBangController 25 | FrisbeeBot 26 | GameData 27 | GettingStarted 28 | Gyro 29 | GyroDriveCommands 30 | GyroMecanum 31 | HatchbotInlined 32 | HatchbotTraditional 33 | HidRumble 34 | HttpCamera 35 | I2CCommunication 36 | IntermediateVision 37 | MagicbotSimple 38 | MecanumBot 39 | MecanumDrive 40 | MecanumDriveXbox 41 | Mechanism2d 42 | MotorControl 43 | Physics/src 44 | Physics4Wheel/src 45 | PhysicsMecanum/src 46 | PhysicsSPI/src 47 | PotentiometerPID 48 | QuickVision 49 | RamseteController 50 | Relay 51 | RomiReference 52 | SchedulerEventLogging 53 | SelectCommand 54 | ShuffleBoard 55 | Solenoid 56 | StatefulAutonomous 57 | StateSpaceFlywheel 58 | StateSpaceFlywheelSysId 59 | SwerveBot 60 | SysId 61 | TankDrive 62 | TankDriveXboxController 63 | Timed/src 64 | Ultrasonic 65 | UltrasonicPID 66 | " 67 | 68 | IGNORED_TESTS=" 69 | RamseteCommand 70 | PhysicsCamSim/src 71 | StateSpaceArm 72 | StateSpaceElevator 73 | XrpReference 74 | " 75 | 76 | ALL_TESTS="${BASE_TESTS}" 77 | EVERY_TESTS="${ALL_TESTS} ${IGNORED_TESTS}" 78 | TESTS="${ALL_TESTS}" 79 | 80 | TMPD=$(mktemp -d) 81 | trap 'rm -rf "$TMPD"' EXIT 82 | 83 | # Ensure that when new samples are added, they are added to the list of things 84 | # to test. Otherwise, exit. 85 | for i in ${EVERY_TESTS}; do 86 | echo ./$i/robot.py 87 | done | sort > $TMPD/a 88 | 89 | find . -name robot.py | sort > $TMPD/b 90 | 91 | if ! diff -u $TMPD/a $TMPD/b; then 92 | 93 | if [ -z "$FORCE_ANYWAYS" ]; then 94 | echo "ERROR: Not every robot.py file is in the list of tests!" 95 | exit 1 96 | fi 97 | fi 98 | 99 | for t in ${TESTS}; do 100 | pushd $t > /dev/null 101 | pwd 102 | if ! python3 -m robotpy test --builtin "${@:2}"; then 103 | EC=$? 104 | echo "Test in $(pwd) failed" 105 | exit 1 106 | fi 107 | popd > /dev/null 108 | done 109 | 110 | echo "All tests successful!" 111 | -------------------------------------------------------------------------------- /SchedulerEventLogging/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import commands2 10 | import typing 11 | 12 | from robotcontainer import RobotContainer 13 | 14 | 15 | class MyRobot(commands2.TimedCommandRobot): 16 | """ 17 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 18 | has an implementation of robotPeriodic which runs the scheduler for you 19 | """ 20 | 21 | autonomousCommand: typing.Optional[commands2.Command] = None 22 | 23 | def robotInit(self) -> None: 24 | """ 25 | This function is run when the robot is first started up and should be used for any 26 | initialization code. 27 | """ 28 | 29 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 30 | # autonomous chooser on the dashboard. 31 | self.container = RobotContainer() 32 | 33 | def disabledInit(self) -> None: 34 | """This function is called once each time the robot enters Disabled mode.""" 35 | pass 36 | 37 | def disabledPeriodic(self) -> None: 38 | """This function is called periodically when disabled""" 39 | pass 40 | 41 | def autonomousInit(self) -> None: 42 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 43 | self.autonomousCommand = self.container.getAutonomousCommand() 44 | 45 | if self.autonomousCommand: 46 | self.autonomousCommand.schedule() 47 | 48 | def autonomousPeriodic(self) -> None: 49 | """This function is called periodically during autonomous""" 50 | pass 51 | 52 | def teleopInit(self) -> None: 53 | # This makes sure that the autonomous stops running when 54 | # teleop starts running. If you want the autonomous to 55 | # continue until interrupted by another command, remove 56 | # this line or comment it out. 57 | if self.autonomousCommand: 58 | self.autonomousCommand.cancel() 59 | 60 | def teleopPeriodic(self) -> None: 61 | """This function is called periodically during operator control""" 62 | pass 63 | 64 | def testInit(self) -> None: 65 | # Cancels all running commands at the start of test mode 66 | commands2.CommandScheduler.getInstance().cancelAll() 67 | -------------------------------------------------------------------------------- /PhysicsMecanum/src/physics.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | # 8 | # See the notes for the other physics sample 9 | # 10 | 11 | import wpilib.simulation 12 | 13 | from pyfrc.physics.core import PhysicsInterface 14 | from pyfrc.physics import drivetrains 15 | 16 | import typing 17 | 18 | if typing.TYPE_CHECKING: 19 | from robot import MyRobot 20 | 21 | 22 | class PhysicsEngine: 23 | """ 24 | Simulates a 4-wheel mecanum robot using Tank Drive joystick control 25 | """ 26 | 27 | def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"): 28 | """ 29 | :param physics_controller: `pyfrc.physics.core.Physics` object 30 | to communicate simulation effects to 31 | """ 32 | 33 | self.physics_controller = physics_controller 34 | 35 | # Motors 36 | self.lf_motor = wpilib.simulation.PWMSim(robot.frontLeftMotor.getChannel()) 37 | self.lr_motor = wpilib.simulation.PWMSim(robot.rearLeftMotor.getChannel()) 38 | self.rf_motor = wpilib.simulation.PWMSim(robot.frontRightMotor.getChannel()) 39 | self.rr_motor = wpilib.simulation.PWMSim(robot.rearRightMotor.getChannel()) 40 | 41 | # Gyro 42 | self.gyro = wpilib.simulation.AnalogGyroSim(robot.gyro) 43 | 44 | self.drivetrain = drivetrains.MecanumDrivetrain() 45 | 46 | def update_sim(self, now: float, tm_diff: float) -> None: 47 | """ 48 | Called when the simulation parameters for the program need to be 49 | updated. 50 | 51 | :param now: The current time as a float 52 | :param tm_diff: The amount of time that has passed since the last 53 | time that this function was called 54 | """ 55 | 56 | # Simulate the drivetrain 57 | lf_motor = self.lf_motor.getSpeed() 58 | lr_motor = self.lr_motor.getSpeed() 59 | rf_motor = self.rf_motor.getSpeed() 60 | rr_motor = self.rr_motor.getSpeed() 61 | 62 | speeds = self.drivetrain.calculate(lf_motor, lr_motor, rf_motor, rr_motor) 63 | pose = self.physics_controller.drive(speeds, tm_diff) 64 | 65 | # Update the gyro simulation 66 | # -> FRC gyros are positive clockwise, but the returned pose is positive 67 | # counter-clockwise 68 | self.gyro.setAngle(-pose.rotation().degrees()) 69 | -------------------------------------------------------------------------------- /XrpReference/commands/turndegrees.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import math 8 | import commands2 9 | 10 | from subsystems.drivetrain import Drivetrain 11 | 12 | 13 | class TurnDegrees(commands2.Command): 14 | def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None: 15 | """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in 16 | degrees) and rotational speed. 17 | 18 | :param speed: The speed which the robot will drive. Negative is in reverse. 19 | :param degrees: Degrees to turn. Leverages encoders to compare distance. 20 | :param drive: The drive subsystem on which this command will run 21 | """ 22 | super().__init__() 23 | 24 | self.degrees = degrees 25 | self.speed = speed 26 | self.drive = drive 27 | self.addRequirements(drive) 28 | 29 | def initialize(self) -> None: 30 | """Called when the command is initially scheduled.""" 31 | # Set motors to stop, read encoder values for starting point 32 | self.drive.arcadeDrive(0, 0) 33 | self.drive.resetEncoders() 34 | 35 | def execute(self) -> None: 36 | """Called every time the scheduler runs while the command is scheduled.""" 37 | self.drive.arcadeDrive(0, self.speed) 38 | 39 | def end(self, interrupted: bool) -> None: 40 | """Called once the command ends or is interrupted.""" 41 | self.drive.arcadeDrive(0, 0) 42 | 43 | def isFinished(self) -> bool: 44 | """Returns true when the command should end.""" 45 | 46 | # Need to convert distance travelled to degrees. The Standard 47 | # XRP Chassis found here, https://www.sparkfun.com/products/22230, 48 | # has a wheel placement diameter (163 mm) - width of the wheel (8 mm) = 155 mm 49 | # or 6.102 inches. We then take into consideration the width of the tires. 50 | inchPerDegree = math.pi * 6.102 / 360 51 | 52 | # Compare distance travelled from start to distance based on degree turn 53 | return self._getAverageTurningDistance() >= inchPerDegree * self.degrees 54 | 55 | def _getAverageTurningDistance(self) -> float: 56 | leftDistance = abs(self.drive.getLeftDistanceInch()) 57 | rightDistance = abs(self.drive.getRightDistanceInch()) 58 | return (leftDistance + rightDistance) / 2.0 59 | -------------------------------------------------------------------------------- /RomiReference/commands/turndegrees.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import math 8 | import commands2 9 | 10 | from subsystems.drivetrain import Drivetrain 11 | 12 | 13 | class TurnDegrees(commands2.Command): 14 | def __init__(self, speed: float, degrees: float, drive: Drivetrain) -> None: 15 | """Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in 16 | degrees) and rotational speed. 17 | 18 | :param speed: The speed which the robot will drive. Negative is in reverse. 19 | :param degrees: Degrees to turn. Leverages encoders to compare distance. 20 | :param drive: The drive subsystem on which this command will run 21 | """ 22 | super().__init__() 23 | 24 | self.degrees = degrees 25 | self.speed = speed 26 | self.drive = drive 27 | self.addRequirements(drive) 28 | 29 | def initialize(self) -> None: 30 | """Called when the command is initially scheduled.""" 31 | # Set motors to stop, read encoder values for starting point 32 | self.drive.arcadeDrive(0, 0) 33 | self.drive.resetEncoders() 34 | 35 | def execute(self) -> None: 36 | """Called every time the scheduler runs while the command is scheduled.""" 37 | self.drive.arcadeDrive(0, self.speed) 38 | 39 | def end(self, interrupted: bool) -> None: 40 | """Called once the command ends or is interrupted.""" 41 | self.drive.arcadeDrive(0, 0) 42 | 43 | def isFinished(self) -> bool: 44 | """Returns true when the command should end.""" 45 | 46 | # Need to convert distance travelled to degrees. The Standard 47 | # Romi Chassis found here, https://www.pololu.com/category/203/romi-chassis-kits, 48 | # has a wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm 49 | # or 5.551 inches. We then take into consideration the width of the tires. 50 | inchPerDegree = math.pi * 5.551 / 360.0 51 | 52 | # Compare distance travelled from start to distance based on degree turn 53 | return self._getAverageTurningDistance() >= inchPerDegree * self.degrees 54 | 55 | def _getAverageTurningDistance(self) -> float: 56 | leftDistance = abs(self.drive.getLeftDistanceInch()) 57 | rightDistance = abs(self.drive.getRightDistanceInch()) 58 | return (leftDistance + rightDistance) / 2.0 59 | -------------------------------------------------------------------------------- /SwerveBot/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | import wpimath 10 | import wpilib.drive 11 | import wpimath.filter 12 | import wpimath.controller 13 | import drivetrain 14 | 15 | 16 | class MyRobot(wpilib.TimedRobot): 17 | def robotInit(self) -> None: 18 | """Robot initialization function""" 19 | self.controller = wpilib.XboxController(0) 20 | self.swerve = drivetrain.Drivetrain() 21 | 22 | # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. 23 | self.xspeedLimiter = wpimath.filter.SlewRateLimiter(3) 24 | self.yspeedLimiter = wpimath.filter.SlewRateLimiter(3) 25 | self.rotLimiter = wpimath.filter.SlewRateLimiter(3) 26 | 27 | def autonomousPeriodic(self) -> None: 28 | self.driveWithJoystick(False) 29 | self.swerve.updateOdometry() 30 | 31 | def teleopPeriodic(self) -> None: 32 | self.driveWithJoystick(True) 33 | 34 | def driveWithJoystick(self, fieldRelative: bool) -> None: 35 | # Get the x speed. We are inverting this because Xbox controllers return 36 | # negative values when we push forward. 37 | xSpeed = ( 38 | -self.xspeedLimiter.calculate( 39 | wpimath.applyDeadband(self.controller.getLeftY(), 0.02) 40 | ) 41 | * drivetrain.kMaxSpeed 42 | ) 43 | 44 | # Get the y speed or sideways/strafe speed. We are inverting this because 45 | # we want a positive value when we pull to the left. Xbox controllers 46 | # return positive values when you pull to the right by default. 47 | ySpeed = ( 48 | -self.yspeedLimiter.calculate( 49 | wpimath.applyDeadband(self.controller.getLeftX(), 0.02) 50 | ) 51 | * drivetrain.kMaxSpeed 52 | ) 53 | 54 | # Get the rate of angular rotation. We are inverting this because we want a 55 | # positive value when we pull to the left (remember, CCW is positive in 56 | # mathematics). Xbox controllers return positive values when you pull to 57 | # the right by default. 58 | rot = ( 59 | -self.rotLimiter.calculate( 60 | wpimath.applyDeadband(self.controller.getRightX(), 0.02) 61 | ) 62 | * drivetrain.kMaxSpeed 63 | ) 64 | 65 | self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) 66 | -------------------------------------------------------------------------------- /RamseteCommand/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import typing 9 | import wpilib 10 | import commands2 11 | 12 | from robotcontainer import RobotContainer 13 | 14 | 15 | class MyRobot(commands2.TimedCommandRobot): 16 | """ 17 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 18 | has an implementation of robotPeriodic which runs the scheduler for you 19 | """ 20 | 21 | autonomousCommand: typing.Optional[commands2.Command] = None 22 | 23 | def robotInit(self) -> None: 24 | """ 25 | This function is run when the robot is first started up and should be used for any 26 | initialization code. 27 | """ 28 | 29 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 30 | # autonomous chooser on the dashboard. 31 | self.robotContainer = RobotContainer() 32 | 33 | def disabledInit(self) -> None: 34 | """This function is called once each time the robot enters Disabled mode.""" 35 | 36 | def disabledPeriodic(self) -> None: 37 | """This function is called periodically when disabled""" 38 | 39 | def autonomousInit(self) -> None: 40 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 41 | self.autonomousCommand = self.robotContainer.getAutonomousCommand() 42 | 43 | # schedule the autonomous command (example) 44 | if self.autonomousCommand: 45 | self.autonomousCommand.schedule() 46 | 47 | def autonomousPeriodic(self) -> None: 48 | """This function is called periodically during autonomous""" 49 | 50 | def teleopInit(self) -> None: 51 | # This makes sure that the autonomous stops running when 52 | # teleop starts running. If you want the autonomous to 53 | # continue until interrupted by another command, remove 54 | # this line or comment it out. 55 | if self.autonomousCommand: 56 | self.autonomousCommand.cancel() 57 | 58 | def teleopPeriodic(self) -> None: 59 | """This function is called periodically during operator control""" 60 | 61 | def testInit(self) -> None: 62 | # Cancels all running commands at the start of test mode 63 | commands2.CommandScheduler.getInstance().cancelAll() 64 | 65 | def testPeriodic(self) -> None: 66 | """This function is called periodically during test mode""" 67 | -------------------------------------------------------------------------------- /MecanumDriveXbox/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | from wpilib.drive import MecanumDrive 10 | 11 | 12 | class MyRobot(wpilib.TimedRobot): 13 | """ 14 | This is a demo program showing how to use Mecanum control with the 15 | MecanumDrive class. 16 | """ 17 | 18 | # Channels on the roboRIO that the motor controllers are plugged in to 19 | frontLeftChannel = 2 20 | rearLeftChannel = 3 21 | frontRightChannel = 1 22 | rearRightChannel = 0 23 | 24 | # The channel on the driver station that the joystick is connected to 25 | joystickChannel = 0 26 | 27 | def robotInit(self): 28 | """Robot initialization function""" 29 | self.frontLeftMotor = wpilib.Talon(self.frontLeftChannel) 30 | self.rearLeftMotor = wpilib.Talon(self.rearLeftChannel) 31 | self.frontRightMotor = wpilib.Talon(self.frontRightChannel) 32 | self.rearRightMotor = wpilib.Talon(self.rearRightChannel) 33 | 34 | # invert the left side motors 35 | self.frontLeftMotor.setInverted(True) 36 | 37 | # you may need to change or remove this to match your robot 38 | self.rearLeftMotor.setInverted(True) 39 | 40 | self.drive = MecanumDrive( 41 | self.frontLeftMotor, 42 | self.rearLeftMotor, 43 | self.frontRightMotor, 44 | self.rearRightMotor, 45 | ) 46 | # Define the Xbox Controller. 47 | self.stick = wpilib.XboxController(self.joystickChannel) 48 | 49 | def teleopInit(self): 50 | self.drive.setSafetyEnabled(True) 51 | 52 | def teleopPeriodic(self): 53 | """Runs the motors with Mecanum drive.""" 54 | # Use the joystick X axis for lateral movement, Y axis for forward movement, and Z axis for rotation. 55 | # This sample does not use field-oriented drive, so the gyro input is set to zero. 56 | # This Stick configuration is created by K.E. on our team. Left stick Y axis is speed, Left Stick X axis is strafe, and Right Stick Y axis is turn. 57 | self.drive.driveCartesian( 58 | self.stick.getLeftX(), 59 | self.stick.getLeftY(), 60 | self.stick.getRightY(), 61 | ) 62 | 63 | """Alternatively, to match the driver station enumeration, you may use ---> self.drive.driveCartesian( 64 | self.stick.getRawAxis(1), self.stick.getRawAxis(3), self.stick.getRawAxis(2), 0 65 | )""" 66 | -------------------------------------------------------------------------------- /Encoder/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import wpilib 9 | 10 | import math 11 | 12 | 13 | class MyRobot(wpilib.TimedRobot): 14 | """ 15 | Sample program displaying the value of a quadrature encoder on the SmartDashboard. Quadrature 16 | Encoders are digital sensors which can detect the amount the encoder has rotated since starting 17 | as well as the direction in which the encoder shaft is rotating. However, encoders can not tell 18 | you the absolute position of the encoder shaft (ie, it considers where it starts to be the zero 19 | position, no matter where it starts), and so can only tell you how much the encoder has rotated 20 | since starting. Depending on the precision of an encoder, it will have fewer or greater ticks per 21 | revolution; the number of ticks per revolution will affect the conversion between ticks and 22 | distance, as specified by DistancePerPulse. One of the most common uses of encoders is in the 23 | drivetrain, so that the distance that the robot drives can be precisely controlled during the 24 | autonomous mode. 25 | """ 26 | 27 | def robotInit(self): 28 | """Robot initialization function""" 29 | 30 | self.encoder = wpilib.Encoder(1, 2, False, wpilib.Encoder.EncodingType.k4X) 31 | 32 | # Defines the number of samples to average when determining the rate. 33 | # On a quadrature encoder, values range from 1-255; 34 | # larger values result in smoother but potentially 35 | # less accurate rates than lower values. 36 | self.encoder.setSamplesToAverage(5) 37 | 38 | # Defines how far the mechanism attached to the encoder moves per pulse. In 39 | # this case, we assume that a 360 count encoder is directly 40 | # attached to a 3 inch diameter (1.5inch radius) wheel, 41 | # and that we want to measure distance in inches. 42 | self.encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * math.pi * 1.5) 43 | 44 | # Defines the lowest rate at which the encoder will 45 | # not be considered stopped, for the purposes of 46 | # the GetStopped() method. Units are in distance / second, 47 | # where distance refers to the units of distance 48 | # that you are using, in this case inches. 49 | self.encoder.setMinRate(1.0) 50 | 51 | def teleopPeriodic(self): 52 | wpilib.SmartDashboard.putNumber("Encoder Distance", self.encoder.getDistance()) 53 | wpilib.SmartDashboard.putNumber("Encoder Rate", self.encoder.getRate()) 54 | -------------------------------------------------------------------------------- /HatchbotTraditional/subsystems/drivesubsystem.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import wpilib 9 | import wpilib.drive 10 | 11 | import constants 12 | 13 | 14 | class DriveSubsystem(commands2.Subsystem): 15 | def __init__(self) -> None: 16 | super().__init__() 17 | 18 | self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port) 19 | self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port) 20 | self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) 21 | self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) 22 | 23 | # We need to invert one side of the drivetrain so that positive speeds 24 | # result in both sides moving forward. Depending on how your robot's 25 | # drivetrain is constructed, you might have to invert the left side instead. 26 | self.right1.setInverted(True) 27 | 28 | # The robot's drive 29 | self.drive = wpilib.drive.DifferentialDrive(self.left1, self.right1) 30 | 31 | # The left-side drive encoder 32 | self.leftEncoder = wpilib.Encoder( 33 | *constants.kLeftEncoderPorts, 34 | reverseDirection=constants.kLeftEncoderReversed 35 | ) 36 | 37 | # The right-side drive encoder 38 | self.rightEncoder = wpilib.Encoder( 39 | *constants.kRightEncoderPorts, 40 | reverseDirection=constants.kRightEncoderReversed 41 | ) 42 | 43 | # Sets the distance per pulse for the encoders 44 | self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) 45 | self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) 46 | 47 | def arcadeDrive(self, fwd: float, rot: float) -> None: 48 | """ 49 | Drives the robot using arcade controls. 50 | 51 | :param fwd: the commanded forward movement 52 | :param rot: the commanded rotation 53 | """ 54 | self.drive.arcadeDrive(fwd, rot) 55 | 56 | def resetEncoders(self) -> None: 57 | """Resets the drive encoders to currently read a position of 0.""" 58 | 59 | def getAverageEncoderDistance(self) -> float: 60 | """Gets the average distance of the TWO encoders.""" 61 | return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 62 | 63 | def setMaxOutput(self, maxOutput: float): 64 | """ 65 | Sets the max output of the drive. Useful for scaling the 66 | drive to drive more slowly. 67 | """ 68 | self.drive.setMaxOutput(maxOutput) 69 | -------------------------------------------------------------------------------- /SysId/sysidroutinebot.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | from commands2 import Command 8 | from commands2.button import CommandXboxController 9 | from commands2.sysid import SysIdRoutine 10 | 11 | from subsystems.drive import Drive 12 | 13 | from constants import OIConstants 14 | 15 | 16 | class SysIdRoutineBot: 17 | """This class is where the bulk of the robot should be declared. Since Command-based is a 18 | "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` 19 | periodic methods (other than the scheduler calls). Instead, the structure of the robot (including 20 | subsystems, commands, and button mappings) should be declared here. 21 | """ 22 | 23 | def __init__(self) -> None: 24 | # The robot's subsystems 25 | self.drive = Drive() 26 | 27 | # The driver's controller 28 | self.controller = CommandXboxController(OIConstants.kDriverControllerPort) 29 | 30 | def configureBindings(self) -> None: 31 | """Use this method to define bindings between conditions and commands. These are useful for 32 | automating robot behaviors based on button and sensor input. 33 | 34 | Should be called during :meth:`.Robot.robotInit`. 35 | 36 | Event binding methods are available on the :class:`.Trigger` class. 37 | """ 38 | 39 | # Control the drive with split-stick arcade controls 40 | self.drive.setDefaultCommand( 41 | self.drive.arcadeDriveCommand( 42 | lambda: -self.controller.getLeftY(), 43 | lambda: -self.controller.getRightX(), 44 | ) 45 | ) 46 | 47 | # Bind full set of SysId routine tests to buttons; a complete routine should run each of these 48 | # once. 49 | self.controller.a().whileTrue( 50 | self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward) 51 | ) 52 | self.controller.b().whileTrue( 53 | self.drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse) 54 | ) 55 | self.controller.x().whileTrue( 56 | self.drive.sysIdDynamic(SysIdRoutine.Direction.kForward) 57 | ) 58 | self.controller.y().whileTrue( 59 | self.drive.sysIdDynamic(SysIdRoutine.Direction.kReverse) 60 | ) 61 | 62 | def getAutonomousCommand(self) -> Command: 63 | """Use this to define the command that runs during autonomous. 64 | 65 | Scheduled during :meth:`.Robot.autonomousInit`. 66 | """ 67 | 68 | # Do nothing 69 | return self.drive.run(lambda: None) 70 | -------------------------------------------------------------------------------- /HttpCamera/vision.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | """ 8 | This is a demo program showing the use of OpenCV to do vision processing. The image is acquired 9 | from an HTTP camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has 10 | many methods for different types of processing. 11 | """ 12 | 13 | import ntcore 14 | import numpy 15 | import cscore 16 | from cscore import CameraServer 17 | import cv2 18 | 19 | 20 | # 21 | # This code will work both on a RoboRIO and on other platforms. The exact mechanism 22 | # to run it differs depending on whether you’re on a RoboRIO or a coprocessor 23 | # 24 | # https://robotpy.readthedocs.io/en/stable/vision/code.html 25 | 26 | 27 | def main(): 28 | # Create an HTTP camera. The address will need to be modified to have the 29 | # correct team number. The exact path will depend on the source. 30 | camera = cscore.HttpCamera("HTTP Camera", "http://10.x.y.11/video/stream.mjpg") 31 | # Start capturing images 32 | CameraServer.startAutomaticCapture(camera) 33 | # Set the resolution 34 | camera.setResolution(640, 480) 35 | 36 | # Get a CvSink. This will capture Mats from the camera 37 | cvSink = CameraServer.getVideo() 38 | 39 | # Setup a CvSource. This will send images back to the Dashboard 40 | outputStream = CameraServer.putVideo("Rectangle", 640, 480) 41 | 42 | # Mats are very memory expensive. Lets reuse this Mat. 43 | mat = numpy.zeros((480, 640, 3), dtype="uint8") 44 | 45 | # Declare the color of the rectangle 46 | rectColor = (255, 255, 255) 47 | 48 | # The camera code will be killed when the robot.py program exits. If you wish to perform cleanup, 49 | # you should register an atexit handler. The child process will NOT be launched when running the robot code in 50 | # simulation or unit testing mode 51 | 52 | while True: 53 | # Tell the CvSink to grab a frame from the camera and put it in the source mat. If there is an error notify the 54 | # output. 55 | 56 | if cvSink.grabFrame(mat) == 0: 57 | # Send the output the error. 58 | outputStream.notifyError(cvSink.getError()) 59 | 60 | # skip the rest of the current iteration 61 | continue 62 | 63 | # Put a rectangle on the image 64 | mat = cv2.rectangle( 65 | img=mat, 66 | pt1=(100, 100), 67 | pt2=(400, 400), 68 | color=rectColor, 69 | lineType=5, 70 | ) 71 | 72 | # Give the output stream a new image to display 73 | outputStream.putFrame(mat) 74 | -------------------------------------------------------------------------------- /check_header.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | from pathlib import Path 9 | 10 | 11 | def check_file_content(file_path): 12 | with open(file_path, "r") as file: 13 | lines = file.readlines() 14 | 15 | if file.name.endswith("robot.py"): 16 | expected_lines = [ 17 | "#!/usr/bin/env python3\n", 18 | "#\n", 19 | "# Copyright (c) FIRST and other WPILib contributors.\n", 20 | "# Open Source Software; you can modify and/or share it under the terms of\n", 21 | "# the WPILib BSD license file in the root directory of this project.\n", 22 | "#\n", 23 | "\n", 24 | ] 25 | else: 26 | expected_lines = [ 27 | "#\n", 28 | "# Copyright (c) FIRST and other WPILib contributors.\n", 29 | "# Open Source Software; you can modify and/or share it under the terms of\n", 30 | "# the WPILib BSD license file in the root directory of this project.\n", 31 | "#\n", 32 | "\n", 33 | ] 34 | 35 | if lines[: len(expected_lines)] != expected_lines: 36 | print( 37 | "\n".join( 38 | [ 39 | f"{file_path}", 40 | "ERROR: File must start with the following lines", 41 | "------------------------------", 42 | "".join(expected_lines[:-1]), 43 | "------------------------------", 44 | "Found:", 45 | "".join(lines[: len(expected_lines)]), 46 | "------------------------------", 47 | ] 48 | ) 49 | ) 50 | 51 | return False 52 | return True 53 | 54 | 55 | def main(): 56 | current_directory = Path(__file__).parent 57 | python_files = [ 58 | x 59 | for x in current_directory.glob("./**/*.py") 60 | if not x.parent == current_directory and x.stat().st_size != 0 61 | ] 62 | 63 | non_compliant_files = [ 64 | file for file in python_files if not check_file_content(file) 65 | ] 66 | 67 | non_compliant_files.sort() 68 | 69 | if non_compliant_files: 70 | print("Non-compliant files:") 71 | for file in non_compliant_files: 72 | print(f"- {file}") 73 | exit(1) # Exit with an error code 74 | else: 75 | print("All files are compliant.") 76 | 77 | 78 | if __name__ == "__main__": 79 | main() 80 | -------------------------------------------------------------------------------- /HatchbotInlined/subsystems/drivesubsystem.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import commands2 8 | import wpilib 9 | import wpilib.drive 10 | 11 | import constants 12 | 13 | 14 | class DriveSubsystem(commands2.Subsystem): 15 | def __init__(self) -> None: 16 | super().__init__() 17 | 18 | self.left1 = wpilib.PWMVictorSPX(constants.kLeftMotor1Port) 19 | self.left2 = wpilib.PWMVictorSPX(constants.kLeftMotor2Port) 20 | self.right1 = wpilib.PWMVictorSPX(constants.kRightMotor1Port) 21 | self.right2 = wpilib.PWMVictorSPX(constants.kRightMotor2Port) 22 | 23 | self.left1.addFollower(self.left2) 24 | self.right1.addFollower(self.right2) 25 | 26 | # We need to invert one side of the drivetrain so that positive speeds 27 | # result in both sides moving forward. Depending on how your robot's 28 | # drivetrain is constructed, you might have to invert the left side instead. 29 | self.right1.setInverted(True) 30 | 31 | # The robot's drive 32 | self.drive = wpilib.drive.DifferentialDrive(self.left1, self.right1) 33 | 34 | # The left-side drive encoder 35 | self.leftEncoder = wpilib.Encoder( 36 | *constants.kLeftEncoderPorts, 37 | reverseDirection=constants.kLeftEncoderReversed 38 | ) 39 | 40 | # The right-side drive encoder 41 | self.rightEncoder = wpilib.Encoder( 42 | *constants.kRightEncoderPorts, 43 | reverseDirection=constants.kRightEncoderReversed 44 | ) 45 | 46 | # Sets the distance per pulse for the encoders 47 | self.leftEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) 48 | self.rightEncoder.setDistancePerPulse(constants.kEncoderDistancePerPulse) 49 | 50 | def arcadeDrive(self, fwd: float, rot: float) -> None: 51 | """ 52 | Drives the robot using arcade controls. 53 | 54 | :param fwd: the commanded forward movement 55 | :param rot: the commanded rotation 56 | """ 57 | self.drive.arcadeDrive(fwd, rot) 58 | 59 | def resetEncoders(self) -> None: 60 | """Resets the drive encoders to currently read a position of 0.""" 61 | 62 | def getAverageEncoderDistance(self) -> float: 63 | """Gets the average distance of the TWO encoders.""" 64 | return (self.leftEncoder.getDistance() + self.rightEncoder.getDistance()) / 2.0 65 | 66 | def setMaxOutput(self, maxOutput: float): 67 | """ 68 | Sets the max output of the drive. Useful for scaling the 69 | drive to drive more slowly. 70 | """ 71 | self.drive.setMaxOutput(maxOutput) 72 | -------------------------------------------------------------------------------- /ArmBotOffboard/robotcontainer.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import constants 8 | import commands2.button 9 | import commands2.cmd 10 | 11 | from subsystems.armsubsystem import ArmSubsystem 12 | from subsystems.drivesubsystem import DriveSubsystem 13 | 14 | 15 | class RobotContainer: 16 | """ 17 | This class is where the bulk of the robot should be declared. Since Command-based is a 18 | "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` 19 | periodic methods (other than the scheduler calls). Instead, the structure of the robot (including 20 | subsystems, commands, and button mappings) should be declared here. 21 | """ 22 | 23 | def __init__(self) -> None: 24 | # The robot's subsystems 25 | self.robotDrive = DriveSubsystem() 26 | self.robotArm = ArmSubsystem() 27 | 28 | # The driver's controller 29 | self.driverController = commands2.button.CommandXboxController( 30 | constants.kDriverControllerPort 31 | ) 32 | # self.driverController = wpilib.Joystick(constants.kDriverControllerPort) 33 | 34 | # Configure the button bindings 35 | self.configureButtonBindings() 36 | 37 | # Set the default drive command 38 | self.robotDrive.setDefaultCommand( 39 | self.robotDrive.arcadeDriveCommand( 40 | lambda: -self.driverController.getLeftY(), 41 | lambda: -self.driverController.getRightX(), 42 | ) 43 | ) 44 | 45 | def configureButtonBindings(self) -> None: 46 | """ 47 | Use this method to define your button->command mappings. Buttons can be created by 48 | instantiating a :GenericHID or one of its subclasses (Joystick or XboxController), 49 | and then passing it to a JoystickButton. 50 | """ 51 | 52 | # Move the arm to 2 radians above horizontal when the 'A' button is pressed. 53 | self.driverController.a().onTrue(self.robotArm.setArmGoalCommand(2)) 54 | 55 | # Move the arm to neutral position when the 'B' button is pressed. 56 | self.driverController.b().onTrue( 57 | self.robotArm.setArmGoalCommand(constants.kArmOffsetRads) 58 | ) 59 | 60 | # Drive at half speed when some of bumpers are held. 61 | self.driverController.rightBumper().onTrue( 62 | self.robotDrive.limitOutputCommand(0.5) 63 | ) 64 | self.driverController.rightBumper().onFalse( 65 | self.robotDrive.limitOutputCommand(1) 66 | ) 67 | 68 | def getAutonomousCommand(self) -> commands2.Command: 69 | return commands2.cmd.none() 70 | -------------------------------------------------------------------------------- /ArmBotOffboard/examplesmartmotorcontroller.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib.interfaces 8 | import enum 9 | 10 | 11 | class ExampleSmartMotorController(wpilib.interfaces.MotorController): 12 | """A simplified stub class that simulates the API of a common "smart" motor controller. 13 | Has no actual functionality. 14 | """ 15 | 16 | class PIDMode(enum.Enum): 17 | kPosition = enum.auto() 18 | kVelocity = enum.auto() 19 | kMovementWitchcraft = enum.auto() 20 | 21 | def __init__(self, port: int) -> None: 22 | """Creates a new ExampleSmartMotorController. 23 | 24 | Args: 25 | port: The port for the controller. 26 | """ 27 | super().__init__() 28 | 29 | def setPID(self, kp: float, ki: float, kd: float) -> None: 30 | """Example method for setting the PID gains of the smart controller. 31 | 32 | Args: 33 | kp: The proportional gain. 34 | ki: The integral gain. 35 | kd: The derivative gain. 36 | """ 37 | pass 38 | 39 | def setSetPoint( 40 | self, mode: PIDMode, setpoint: float, arbfeedforward: float 41 | ) -> None: 42 | """Example method for setting the setpoint of the smart controller in PID mode. 43 | 44 | Args: 45 | mode: The mode of the PID controller. 46 | setpoint: The controller setpoint. 47 | arbfeedforward: An arbitrary feedforward output (from -1 to 1). 48 | """ 49 | pass 50 | 51 | def follow(self, leader: "ExampleSmartMotorController") -> None: 52 | """Places this motor controller in follower mode. 53 | 54 | Args: 55 | leader: The leader to follow. 56 | """ 57 | pass 58 | 59 | def getEncoderDistance(self) -> float: 60 | """Returns the encoder distance. 61 | 62 | Returns: 63 | The current encoder distance. 64 | """ 65 | return 0 66 | 67 | def getEncoderRate(self) -> float: 68 | """Returns the encoder rate. 69 | 70 | Returns: 71 | The current encoder rate. 72 | """ 73 | return 0 74 | 75 | def resetEncoder(self) -> None: 76 | """Resets the encoder to zero distance.""" 77 | pass 78 | 79 | def set(self, speed: float) -> None: 80 | pass 81 | 82 | def get(self) -> float: 83 | pass 84 | 85 | def setInverted(self, isInverted: bool) -> None: 86 | pass 87 | 88 | def getInverted(self) -> bool: 89 | pass 90 | 91 | def disable(self) -> None: 92 | pass 93 | 94 | def stopMotor(self) -> None: 95 | pass 96 | -------------------------------------------------------------------------------- /DriveDistanceOffboard/examplesmartmotorcontroller.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib.interfaces 8 | import enum 9 | 10 | 11 | class ExampleSmartMotorController(wpilib.interfaces.MotorController): 12 | """A simplified stub class that simulates the API of a common "smart" motor controller. 13 | Has no actual functionality. 14 | """ 15 | 16 | class PIDMode(enum.Enum): 17 | kPosition = enum.auto() 18 | kVelocity = enum.auto() 19 | kMovementWitchcraft = enum.auto() 20 | 21 | def __init__(self, port: int) -> None: 22 | """Creates a new ExampleSmartMotorController. 23 | 24 | Args: 25 | port: The port for the controller. 26 | """ 27 | super().__init__() 28 | 29 | def setPID(self, kp: float, ki: float, kd: float) -> None: 30 | """Example method for setting the PID gains of the smart controller. 31 | 32 | Args: 33 | kp: The proportional gain. 34 | ki: The integral gain. 35 | kd: The derivative gain. 36 | """ 37 | pass 38 | 39 | def setSetPoint( 40 | self, mode: PIDMode, setpoint: float, arbfeedforward: float 41 | ) -> None: 42 | """Example method for setting the setpoint of the smart controller in PID mode. 43 | 44 | Args: 45 | mode: The mode of the PID controller. 46 | setpoint: The controller setpoint. 47 | arbfeedforward: An arbitrary feedforward output (from -1 to 1). 48 | """ 49 | pass 50 | 51 | def follow(self, leader: "ExampleSmartMotorController") -> None: 52 | """Places this motor controller in follower mode. 53 | 54 | Args: 55 | leader: The leader to follow. 56 | """ 57 | pass 58 | 59 | def getEncoderDistance(self) -> float: 60 | """Returns the encoder distance. 61 | 62 | Returns: 63 | The current encoder distance. 64 | """ 65 | return 0 66 | 67 | def getEncoderRate(self) -> float: 68 | """Returns the encoder rate. 69 | 70 | Returns: 71 | The current encoder rate. 72 | """ 73 | return 0 74 | 75 | def resetEncoder(self) -> None: 76 | """Resets the encoder to zero distance.""" 77 | pass 78 | 79 | def set(self, speed: float) -> None: 80 | pass 81 | 82 | def get(self) -> float: 83 | pass 84 | 85 | def setInverted(self, isInverted: bool) -> None: 86 | pass 87 | 88 | def getInverted(self) -> bool: 89 | pass 90 | 91 | def disable(self) -> None: 92 | pass 93 | 94 | def stopMotor(self) -> None: 95 | pass 96 | -------------------------------------------------------------------------------- /ElevatorTrapezoidProfile/examplesmartmotorcontroller.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import wpilib.interfaces 8 | import enum 9 | 10 | 11 | class ExampleSmartMotorController(wpilib.interfaces.MotorController): 12 | """A simplified stub class that simulates the API of a common "smart" motor controller. 13 | Has no actual functionality. 14 | """ 15 | 16 | class PIDMode(enum.Enum): 17 | kPosition = enum.auto() 18 | kVelocity = enum.auto() 19 | kMovementWitchcraft = enum.auto() 20 | 21 | def __init__(self, port: int) -> None: 22 | """Creates a new ExampleSmartMotorController. 23 | 24 | Args: 25 | port: The port for the controller. 26 | """ 27 | super().__init__() 28 | 29 | def setPID(self, kp: float, ki: float, kd: float) -> None: 30 | """Example method for setting the PID gains of the smart controller. 31 | 32 | Args: 33 | kp: The proportional gain. 34 | ki: The integral gain. 35 | kd: The derivative gain. 36 | """ 37 | pass 38 | 39 | def setSetPoint( 40 | self, mode: PIDMode, setpoint: float, arbfeedforward: float 41 | ) -> None: 42 | """Example method for setting the setpoint of the smart controller in PID mode. 43 | 44 | Args: 45 | mode: The mode of the PID controller. 46 | setpoint: The controller setpoint. 47 | arbfeedforward: An arbitrary feedforward output (from -1 to 1). 48 | """ 49 | pass 50 | 51 | def follow(self, leader: "ExampleSmartMotorController") -> None: 52 | """Places this motor controller in follower mode. 53 | 54 | Args: 55 | leader: The leader to follow. 56 | """ 57 | pass 58 | 59 | def getEncoderDistance(self) -> float: 60 | """Returns the encoder distance. 61 | 62 | Returns: 63 | The current encoder distance. 64 | """ 65 | return 0 66 | 67 | def getEncoderRate(self) -> float: 68 | """Returns the encoder rate. 69 | 70 | Returns: 71 | The current encoder rate. 72 | """ 73 | return 0 74 | 75 | def resetEncoder(self) -> None: 76 | """Resets the encoder to zero distance.""" 77 | pass 78 | 79 | def set(self, speed: float) -> None: 80 | pass 81 | 82 | def get(self) -> float: 83 | pass 84 | 85 | def setInverted(self, isInverted: bool) -> None: 86 | pass 87 | 88 | def getInverted(self) -> bool: 89 | pass 90 | 91 | def disable(self) -> None: 92 | pass 93 | 94 | def stopMotor(self) -> None: 95 | pass 96 | -------------------------------------------------------------------------------- /FrisbeeBot/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import typing 9 | 10 | import wpilib 11 | import commands2 12 | import commands2.cmd 13 | 14 | import robotcontainer 15 | 16 | """ 17 | The VM is configured to automatically run this class, and to call the functions corresponding to 18 | each mode, as described in the TimedRobot documentation. If you change the name of this class or 19 | the package after creating this project, you must also update the build.gradle file in the 20 | project. 21 | """ 22 | 23 | 24 | class MyRobot(commands2.TimedCommandRobot): 25 | """ 26 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 27 | has an implementation of robotPeriodic which runs the scheduler for you 28 | """ 29 | 30 | def robotInit(self) -> None: 31 | """ 32 | This function is run when the robot is first started up and should be used for any 33 | initialization code. 34 | """ 35 | self.autonomousCommand: typing.Optional[commands2.Command] = None 36 | 37 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 38 | # autonomous chooser on the dashboard. 39 | self.container = robotcontainer.RobotContainer() 40 | 41 | def disabledInit(self) -> None: 42 | """This function is called once each time the robot enters Disabled mode.""" 43 | 44 | def disabledPeriodic(self) -> None: 45 | """This function is called periodically when disabled""" 46 | 47 | def autonomousInit(self) -> None: 48 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 49 | self.autonomousCommand = self.container.getAutonomousCommand() 50 | 51 | # schedule the autonomous command (example) 52 | if self.autonomousCommand is not None: 53 | self.autonomousCommand.schedule() 54 | else: 55 | print("no auto command?") 56 | 57 | def autonomousPeriodic(self) -> None: 58 | """This function is called periodically during autonomous""" 59 | 60 | def teleopInit(self) -> None: 61 | # This makes sure that the autonomous stops running when 62 | # teleop starts running. If you want the autonomous to 63 | # continue until interrupted by another command, remove 64 | # this line or comment it out. 65 | if self.autonomousCommand is not None: 66 | self.autonomousCommand.cancel() 67 | 68 | def teleopPeriodic(self) -> None: 69 | """This function is called periodically during operator control""" 70 | 71 | def testInit(self) -> None: 72 | # Cancels all running commands at the start of test mode 73 | commands2.CommandScheduler.getInstance().cancelAll() 74 | -------------------------------------------------------------------------------- /GyroDriveCommands/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import typing 9 | 10 | import wpilib 11 | import commands2 12 | import commands2.cmd 13 | 14 | import robotcontainer 15 | 16 | """ 17 | The VM is configured to automatically run this class, and to call the functions corresponding to 18 | each mode, as described in the TimedRobot documentation. If you change the name of this class or 19 | the package after creating this project, you must also update the build.gradle file in the 20 | project. 21 | """ 22 | 23 | 24 | class MyRobot(commands2.TimedCommandRobot): 25 | """ 26 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 27 | has an implementation of robotPeriodic which runs the scheduler for you 28 | """ 29 | 30 | def robotInit(self) -> None: 31 | """ 32 | This function is run when the robot is first started up and should be used for any 33 | initialization code. 34 | """ 35 | self.autonomousCommand: typing.Optional[commands2.Command] = None 36 | 37 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 38 | # autonomous chooser on the dashboard. 39 | self.container = robotcontainer.RobotContainer() 40 | 41 | def disabledInit(self) -> None: 42 | """This function is called once each time the robot enters Disabled mode.""" 43 | 44 | def disabledPeriodic(self) -> None: 45 | """This function is called periodically when disabled""" 46 | 47 | def autonomousInit(self) -> None: 48 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 49 | self.autonomousCommand = self.container.getAutonomousCommand() 50 | 51 | # schedule the autonomous command (example) 52 | if self.autonomousCommand is not None: 53 | self.autonomousCommand.schedule() 54 | else: 55 | print("no auto command?") 56 | 57 | def autonomousPeriodic(self) -> None: 58 | """This function is called periodically during autonomous""" 59 | 60 | def teleopInit(self) -> None: 61 | # This makes sure that the autonomous stops running when 62 | # teleop starts running. If you want the autonomous to 63 | # continue until interrupted by another command, remove 64 | # this line or comment it out. 65 | if self.autonomousCommand is not None: 66 | self.autonomousCommand.cancel() 67 | 68 | def teleopPeriodic(self) -> None: 69 | """This function is called periodically during operator control""" 70 | 71 | def testInit(self) -> None: 72 | # Cancels all running commands at the start of test mode 73 | commands2.CommandScheduler.getInstance().cancelAll() 74 | -------------------------------------------------------------------------------- /DriveDistanceOffboard/robot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) FIRST and other WPILib contributors. 4 | # Open Source Software; you can modify and/or share it under the terms of 5 | # the WPILib BSD license file in the root directory of this project. 6 | # 7 | 8 | import typing 9 | 10 | import wpilib 11 | import commands2 12 | import commands2.cmd 13 | 14 | import robotcontainer 15 | 16 | """ 17 | The VM is configured to automatically run this class, and to call the functions corresponding to 18 | each mode, as described in the TimedRobot documentation. If you change the name of this class or 19 | the package after creating this project, you must also update the build.gradle file in the 20 | project. 21 | """ 22 | 23 | 24 | class MyRobot(commands2.TimedCommandRobot): 25 | """ 26 | Command v2 robots are encouraged to inherit from TimedCommandRobot, which 27 | has an implementation of robotPeriodic which runs the scheduler for you 28 | """ 29 | 30 | def robotInit(self) -> None: 31 | """ 32 | This function is run when the robot is first started up and should be used for any 33 | initialization code. 34 | """ 35 | self.autonomousCommand: typing.Optional[commands2.Command] = None 36 | 37 | # Instantiate our RobotContainer. This will perform all our button bindings, and put our 38 | # autonomous chooser on the dashboard. 39 | self.container = robotcontainer.RobotContainer() 40 | 41 | def disabledInit(self) -> None: 42 | """This function is called once each time the robot enters Disabled mode.""" 43 | 44 | def disabledPeriodic(self) -> None: 45 | """This function is called periodically when disabled""" 46 | 47 | def autonomousInit(self) -> None: 48 | """This autonomous runs the autonomous command selected by your RobotContainer class.""" 49 | self.autonomousCommand = self.container.getAutonomousCommand() 50 | 51 | # schedule the autonomous command (example) 52 | if self.autonomousCommand is not None: 53 | self.autonomousCommand.schedule() 54 | else: 55 | print("no auto command?") 56 | 57 | def autonomousPeriodic(self) -> None: 58 | """This function is called periodically during autonomous""" 59 | 60 | def teleopInit(self) -> None: 61 | # This makes sure that the autonomous stops running when 62 | # teleop starts running. If you want the autonomous to 63 | # continue until interrupted by another command, remove 64 | # this line or comment it out. 65 | if self.autonomousCommand is not None: 66 | self.autonomousCommand.cancel() 67 | 68 | def teleopPeriodic(self) -> None: 69 | """This function is called periodically during operator control""" 70 | 71 | def testInit(self) -> None: 72 | # Cancels all running commands at the start of test mode 73 | commands2.CommandScheduler.getInstance().cancelAll() 74 | -------------------------------------------------------------------------------- /SelectCommand/robotcontainer.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) FIRST and other WPILib contributors. 3 | # Open Source Software; you can modify and/or share it under the terms of 4 | # the WPILib BSD license file in the root directory of this project. 5 | # 6 | 7 | import enum 8 | import commands2 9 | 10 | 11 | class RobotContainer: 12 | """This class is where the bulk of the robot should be declared. Since Command-based is a 13 | "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` 14 | periodic methods (other than the scheduler calls). Instead, the structure of the robot (including 15 | subsystems, commands, and button mappings) should be declared here. 16 | """ 17 | 18 | # The enum used as keys for selecting the command to run. 19 | class CommandSelector(enum.Enum): 20 | ONE = enum.auto() 21 | TWO = enum.auto() 22 | THREE = enum.auto() 23 | 24 | # An example selector method for the selectcommand. 25 | def select(self) -> CommandSelector: 26 | """Returns the selector that will select which command to run. 27 | Can base this choice on logical conditions evaluated at runtime. 28 | """ 29 | return self.CommandSelector.ONE 30 | 31 | def __init__(self) -> None: 32 | # An example selectcommand. Will select from the three commands based on the value returned 33 | # by the selector method at runtime. Note that selectcommand takes a generic type, so the 34 | # selector does not have to be an enum; it could be any desired type (string, integer, 35 | # boolean, double...) 36 | self.example_select_command = commands2.SelectCommand( 37 | # Maps selector values to commands 38 | { 39 | self.CommandSelector.ONE: commands2.PrintCommand( 40 | "Command one was selected!" 41 | ), 42 | self.CommandSelector.TWO: commands2.PrintCommand( 43 | "Command two was selected!" 44 | ), 45 | self.CommandSelector.THREE: commands2.PrintCommand( 46 | "Command three was selected!" 47 | ), 48 | }, 49 | self.select, 50 | ) 51 | 52 | # Configure the button bindings 53 | self.configureButtonBindings() 54 | 55 | def configureButtonBindings(self) -> None: 56 | """Use this method to define your button->command mappings. Buttons can be created by 57 | instantiating a {GenericHID} or one of its subclasses 58 | ({edu.wpi.first.wpilibj.Joystick} or {XboxController}), and then calling passing it to a 59 | {edu.wpi.first.wpilibj2.command.button.JoystickButton}. 60 | """ 61 | 62 | def getAutonomousCommand(self) -> commands2.Command: 63 | """Use this to pass the autonomous command to the main {Robot} class. 64 | 65 | :returns: the command to run in autonomous 66 | """ 67 | return self.example_select_command 68 | --------------------------------------------------------------------------------