Update for 2025

Author: auscompgeek

.github/workflows/test.yml

@@ -33,22 +33,20 @@ jobs:
  matrix:
  os: ["ubuntu-22.04", "macos-13", "windows-2022"]
  python_version:
- - '3.8'
  - '3.9'
  - '3.10'
  - '3.11'
  - '3.12'
+ - '3.13'
 
  steps:
  - uses: actions/checkout@v4
- - uses: actions/setup-python@v4
+ - uses: actions/setup-python@v5
  with:
  python-version: ${{ matrix.python_version }}
- architecture: ${{ matrix.architecture }}
  - name: Install deps
  run: |
- pip install -U pip
- pip install 'robotpy[commands2,romi]<2025.0.0,>=2024.2.1.1' 'numpy<2' pytest
+ pip install 'robotpy[commands2,romi]<2026.0.0,>=2025.0.0b3' numpy pytest
  - name: Run tests
  run: bash run_tests.sh
  shell: bash

DutyCycleEncoder/robot.py

@@ -5,16 +5,42 @@
 # the WPILib BSD license file in the root directory of this project.
 #
 
+"""
+This example shows how to use a duty cycle encoder for devices such as
+an arm or elevator.
+"""
+
 import wpilib
+import wpimath
+
+FULL_RANGE = 1.3
+EXPECTED_ZERO = 0.0
 
 
 class MyRobot(wpilib.TimedRobot):
  def robotInit(self):
- """Robot initialization function"""
+ """Called once at the beginning of the robot program."""
 
- self.dutyCycleEncoder = wpilib.DutyCycleEncoder(0)
+ # 2nd parameter is the range of values. This sensor will output between
+ # 0 and the passed in value.
+ # 3rd parameter is the the physical value where you want "0" to be. How
+ # to measure this is fairly easy. Set the value to 0, place the mechanism
+ # where you want "0" to be, and observe the value on the dashboard, That
+ # is the value to enter for the 3rd parameter.
+ self.dutyCycleEncoder = wpilib.DutyCycleEncoder(0, FULL_RANGE, EXPECTED_ZERO)
 
- self.dutyCycleEncoder.setDistancePerRotation(0.5)
+ # If you know the frequency of your sensor, uncomment the following
+ # method, and set the method to the frequency of your sensor.
+ # This will result in more stable readings from the sensor.
+ # Do note that occasionally the datasheet cannot be trusted
+ # and you should measure this value. You can do so with either
+ # an oscilloscope, or by observing the "Frequency" output
+ # on the dashboard while running this sample. If you find
+ # the value jumping between the 2 values, enter halfway between
+ # those values. This number doesn't have to be perfect,
+ # just having a fairly close value will make the output readings
+ # much more stable.
+ self.dutyCycleEncoder.setAssumedFrequency(967.8)
 
  def robotPeriodic(self):
  # Connected can be checked, and uses the frequency of the encoder
@@ -26,10 +52,22 @@ def robotPeriodic(self):
  # Output of encoder
  output = self.dutyCycleEncoder.get()
 
- # Output scaled by DistancePerPulse
- distance = self.dutyCycleEncoder.getDistance()
+ # By default, the output will wrap around to the full range value
+ # when the sensor goes below 0. However, for moving mechanisms this
+ # is not usually ideal, as if 0 is set to a hard stop, its still
+ # possible for the sensor to move slightly past. If this happens
+ # The sensor will assume its now at the furthest away position,
+ # which control algorithms might not handle correctly. Therefore
+ # it can be a good idea to slightly shift the output so the sensor
+ # can go a bit negative before wrapping. Usually 10% or so is fine.
+ # This does not change where "0" is, so no calibration numbers need
+ # to be changed.
+ percentOfRange = FULL_RANGE * 0.1
+ shiftedOutput = wpimath.inputModulus(
+ output, 0 - percentOfRange, FULL_RANGE - percentOfRange
+ )
 
  wpilib.SmartDashboard.putBoolean("Connected", connected)
  wpilib.SmartDashboard.putNumber("Frequency", frequency)
  wpilib.SmartDashboard.putNumber("Output", output)
- wpilib.SmartDashboard.putNumber("Distance", distance)
+ wpilib.SmartDashboard.putNumber("ShiftedOutput", shiftedOutput)

ElevatorSimulation/physics.py

@@ -53,7 +53,7 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"):
  robot.kMaxElevatorHeight,
  True,
  0,
- [0.01],
+ [0.01, 0.0],
  )
  self.encoderSim = wpilib.simulation.EncoderSim(robot.encoder)
  self.motorSim = wpilib.simulation.PWMSim(robot.motor.getChannel())

ElevatorTrapezoidProfile/robot.py

@@ -7,7 +7,8 @@
 
 import wpilib
 import wpimath.controller
-import wpimath.trajectory
+from wpimath.trajectory import TrapezoidProfile
+
 import examplesmartmotorcontroller
 
 
@@ -20,30 +21,25 @@ def robotInit(self):
  # Note: These gains are fake, and will have to be tuned for your robot.
  self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 1.5)
 
- self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75)
+ # Create a motion profile with the given maximum velocity and maximum
+ # acceleration constraints for the next setpoint.
+ self.profile = TrapezoidProfile(TrapezoidProfile.Constraints(1.75, 0.75))
 
- self.goal = wpimath.trajectory.TrapezoidProfile.State()
- self.setpoint = wpimath.trajectory.TrapezoidProfile.State()
+ self.goal = TrapezoidProfile.State()
+ self.setpoint = TrapezoidProfile.State()
 
  # Note: These gains are fake, and will have to be tuned for your robot.
  self.motor.setPID(1.3, 0.0, 0.7)
 
  def teleopPeriodic(self):
  if self.joystick.getRawButtonPressed(2):
- self.goal = wpimath.trajectory.TrapezoidProfile.State(5, 0)
+ self.goal = TrapezoidProfile.State(5, 0)
  elif self.joystick.getRawButtonPressed(3):
- self.goal = wpimath.trajectory.TrapezoidProfile.State(0, 0)
-
- # Create a motion profile with the given maximum velocity and maximum
- # acceleration constraints for the next setpoint, the desired goal, and the
- # current setpoint.
- profile = wpimath.trajectory.TrapezoidProfile(
- self.constraints, self.goal, self.setpoint
- )
+ self.goal = TrapezoidProfile.State(0, 0)
 
  # Retrieve the profiled setpoint for the next timestep. This setpoint moves
  # toward the goal while obeying the constraints.
- self.setpoint = profile.calculate(self.kDt)
+ self.setpoint = self.profile.calculate(self.kDt, self.setpoint, self.goal)
 
  # Send setpoint to offboard controller PID
  self.motor.setSetPoint(

FlywheelBangBangController/physics.py

@@ -4,13 +4,13 @@
 # the WPILib BSD license file in the root directory of this project.
 #
 
+import typing
+
 import wpilib.simulation
-from wpimath.system.plant import DCMotor
+from wpimath.system import plant
 
 from pyfrc.physics.core import PhysicsInterface
 
-import typing
-
 if typing.TYPE_CHECKING:
  from robot import MyRobot
 
@@ -35,9 +35,11 @@ def __init__(self, physics_controller: PhysicsInterface, robot: "MyRobot"):
  self.encoder = wpilib.simulation.EncoderSim(robot.encoder)
 
  # Flywheel
- self.flywheel = wpilib.simulation.FlywheelSim(
- DCMotor.NEO(1), robot.kFlywheelGearing, robot.kFlywheelMomentOfInertia
+ self.gearbox = plant.DCMotor.NEO(1)
+ self.plant = plant.LinearSystemId.flywheelSystem(
+ self.gearbox, robot.kFlywheelGearing, robot.kFlywheelMomentOfInertia
  )
+ self.flywheel = wpilib.simulation.FlywheelSim(self.plant, self.gearbox)
 
  def update_sim(self, now: float, tm_diff: float) -> None:
  """
@@ -54,5 +56,5 @@ def update_sim(self, now: float, tm_diff: float) -> None:
  self.flywheel.setInputVoltage(
  self.flywheelMotor.getSpeed() * wpilib.RobotController.getInputVoltage()
  )
- self.flywheel.update(0.02)
+ self.flywheel.update(tm_diff)
  self.encoder.setRate(self.flywheel.getAngularVelocity())

StateSpaceArm/robot.py

@@ -57,13 +57,13 @@ def robotInit(self) -> None:
  # The observer fuses our encoder data and voltage inputs to reject noise.
  self.observer = wpimath.estimator.KalmanFilter_2_1_1(
  self.armPlant,
- [
+ (
  0.015,
  0.17,
- ], # How accurate we think our model is, in radians and radians/sec.
- [
- 0.01
- ], # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
+ ), # How accurate we think our model is, in radians and radians/sec.
+ (
+ 0.01,
+ ), # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
  0.020,
  )
 

StateSpaceElevator/robot.py

@@ -64,13 +64,13 @@ def robotInit(self) -> None:
  # The observer fuses our encoder data and voltage inputs to reject noise.
  self.observer = wpimath.estimator.KalmanFilter_2_1_1(
  self.elevatorPlant,
- [
+ (
  wpimath.units.inchesToMeters(2),
  wpimath.units.inchesToMeters(40),
- ], # How accurate we think our model is, in meters and meters/second.
- [
- 0.001
- ], # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
+ ), # How accurate we think our model is, in meters and meters/second.
+ (
+ 0.001,
+ ), # How accurate we think our encoder position data is. In this case we very highly trust our encoder position reading.
  0.020,
  )
 

StateSpaceFlywheel/robot.py

@@ -6,12 +6,13 @@
 #
 
 import math
+
 import wpilib
-import wpimath.units
 import wpimath.controller
+import wpimath.estimator
 import wpimath.system
 import wpimath.system.plant
-import wpimath.estimator
+import wpimath.units
 
 kMotorPort = 0
 kEncoderAChannel = 0
@@ -48,18 +49,18 @@ def robotInit(self) -> None:
  # The observer fuses our encoder data and voltage inputs to reject noise.
  self.observer = wpimath.estimator.KalmanFilter_1_1_1(
  self.flywheelPlant,
- [3], # How accurate we think our model is
- [0.01], # How accurate we think our encoder data is
+ (3,), # How accurate we think our model is
+ (0.01,), # How accurate we think our encoder data is
  0.020,
  )
 
  # A LQR uses feedback to create voltage commands.
  self.controller = wpimath.controller.LinearQuadraticRegulator_1_1(
  self.flywheelPlant,
- [8], # qelms. Velocity error tolerance, in radians per second. Decrease
+ (8,), # qelms. Velocity error tolerance, in radians per second. Decrease
  # this to more heavily penalize state excursion, or make the controller behave more
  # aggressively.
- [12], # relms. Control effort (voltage) tolerance. Decrease this to more
+ (12,), # relms. Control effort (voltage) tolerance. Decrease this to more
  # heavily penalize control effort, or make the controller less aggressive. 12 is a good
  # starting point because that is the (approximate) maximum voltage of a battery.
  0.020, # Nominal time between loops. 0.020 for TimedRobot, but can be lower if using notifiers.
@@ -105,5 +106,5 @@ def teleopPeriodic(self) -> None:
  # Send the new calculated voltage to the motors.
  # voltage = duty cycle * battery voltage, so
  # duty cycle = voltage / battery voltage
- nextVoltage = self.loop.U()
+ nextVoltage = self.loop.U(0)
  self.motor.setVoltage(nextVoltage)

SwerveBot/swervemodule.py

@@ -5,10 +5,11 @@
 #
 
 import math
+
 import wpilib
-import wpimath.kinematics
-import wpimath.geometry
 import wpimath.controller
+import wpimath.geometry
+import wpimath.kinematics
 import wpimath.trajectory
 
 kWheelRadius = 0.0508
@@ -109,25 +110,23 @@ def setDesiredState(
  encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance())
 
  # Optimize the reference state to avoid spinning further than 90 degrees
- state = wpimath.kinematics.SwerveModuleState.optimize(
- desiredState, encoderRotation
- )
+ desiredState.optimize(encoderRotation)
 
  # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
  # direction of travel that can occur when modules change directions. This results in smoother
  # driving.
- state.speed *= (state.angle - encoderRotation).cos()
+ desiredState.cosineScale(encoderRotation)
 
  # Calculate the drive output from the drive PID controller.
  driveOutput = self.drivePIDController.calculate(
- self.driveEncoder.getRate(), state.speed
+ self.driveEncoder.getRate(), desiredState.speed
  )
 
- driveFeedforward = self.driveFeedforward.calculate(state.speed)
+ driveFeedforward = self.driveFeedforward.calculate(desiredState.speed)
 
  # Calculate the turning motor output from the turning PID controller.
  turnOutput = self.turningPIDController.calculate(
- self.turningEncoder.getDistance(), state.angle.radians()
+ self.turningEncoder.getDistance(), desiredState.angle.radians()
  )
 
  turnFeedforward = self.turnFeedforward.calculate(