Merge pull request #124 from robotpy/2025

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(

run_tests.sh

@@ -54,8 +54,6 @@ BASE_TESTS="
   ShuffleBoard
   Solenoid
   StatefulAutonomous
-  StateSpaceArm
-  StateSpaceElevator
   StateSpaceFlywheel
   StateSpaceFlywheelSysId
   SwerveBot
@@ -70,6 +68,8 @@ BASE_TESTS="
 IGNORED_TESTS="
   RamseteCommand
   PhysicsCamSim/src
+  StateSpaceArm
+  StateSpaceElevator
 "
 
 ALL_TESTS="${BASE_TESTS}"