PIDCommand python example code

Technical Python
1

Does anyone have a functional scheduler/command based structure which uses the PIDCommand/PIDSubsystem class from robotpy as well as using the input from Encoders? We cannot find a clear example.

2

I’m not aware of an example, but the normal WPILib documentation should be pretty close.

I don’t use command based stuff myself, so I can’t provide an example.

3

Do you use a particular framework? We moved from java/command-based to python this year (mostly because of the simulator) and I’m considering ditching the command-based framework as well. I actually think it makes things more confusing for the kids.

4

My team has gone to Py this year as well. And while we use the Command framework, rather than subclassing a PIDCommand, we just instantiate a PIDController as an instance variable in the command class

def __init__(self, speed, distance):
    super().__init__('DriveToDistance')

    self.drivetrain = self.getRobot().drivetrain
    self.requires(self.drivetrain)
    self.speed = speed
    self.heading = 0
    self.rotation = 0
    self.targetDistance = distance

    kP = robotmap.gyro_kP
    kD = robotmap.gyro_kD
    kI = robotmap.gyro_kI
    self.pid = wpilib.PIDController(kP, kD, kI,
                                        lambda: self.drivetrain.getGyroAngle(),
                                        lambda r: self.setRotation(r))
    self.pid.setAbsoluteTolerance(0.5)
    self.pid.setInputRange(-180.0, 180.0)
    self.pid.setSetpoint(self.heading)
    self.pid.setOutputRange(-1.0, 1.0)
    self.pid.setContinuous(True)

def initialize(self):
    self.getRobot().logger.info("Drive To Distance")
    self.drivetrain.reset()
    self.pid.reset()
    self.pid.enable()

. Seems to work fine for us.

1 Like
5

I know this isn’t quite the point of this thread, but:

  • You pass PDI instead of PID, which goes against the code’s stated order of arguments
  • Instead of creating lambdas, you can use references to the functions directly:
self.pid = wpilib.PIDController(kP, kI, kD, self.drivetrain.getGyroAngle, self.setRotation)
1 Like
6

Many thanks - admittedly this is from some code written over the summer, when both kD and kI are zero, so it ends up being a no-op. But, you’re right - once they become non-zero… oopsie…

Good to know on the non-lambda as well. Thank you!

7

Thanks

8

Command based I think is a good skill but the pid loop part has me beat. Before realizing there was command structure in robotpy we were basing our code around subsystem and command modules just because it made sense for organization to me.

9

Am I correct that target distance shown in your photo is not used?

10

Correct - not used. It was intended (eventually) for encoders to track distance.

The design was to use the gyro & a PID loop to drive straight until the encoder said we’d gone as far as needed (the targetDistance). The the command’s isFinished() would return True.

11

I created the magicbot framework a few years ago, and it is distributed with RobotPy as part of the robotpy-wpilib-utilities package. See MagicBot Framework — RobotPy 2023 documentation for documentation.

It’s simpler for some uses, and more complex for others. I’ve been meaning to build better abstractions for buttons and PID related things, but haven’t yet done so.

12

So if the pid.setSetpoint is in the init of a command how do you continuously update the setpoint? In my case I am controlling the velocity of an arm and the velocity it should be at varies continuously. Thus I am unsure how often your setsetpoint is called in your example; I am also unsure how pid.setcontinuous affects how often the pid loop is called in general.

13

Also I can’t seem to find setSetpoint in the documentation for PIDController. I assume PIDBase has that function but I can’t find that in the docs either.

14

Short answer is, I don’t - it’s the target value (think your thermostat setting at home). Hence it’s constant, called once in initialize(). I suppose you could do something where the execute() method updates it, but I’ve never tried this, so have little advice to offer. Perhaps others have?

Re the frequency the PID loop runs at, IIRC, it runs in its own thread, but I’m not sure if it’s synchronized with the Scheduler().

Sorry I couldn’t be more help on that.

Re the doc - yes, setSetpoint is defined in PIDInterface and implemented in PIDBase. See the Javadoc (this java doc gave me better insight than the Pydocs).

15

Sorry about that. The docs for PIDBase are now available here: https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib/PIDBase.html#wpilib.pidbase.PIDBase