PID library

I’m releasing the code I use for PID control. I had meant to clean this up a bit more and write some documentation before releasing but I never seem to get around to it so it’s “as is”.

This library is a generic implementation of PID control, it is not aimed at anything specific and can use either an encoder or potentiometer as the source of feedback. It is designed for position control rather than speed control but could be adapted easily enough.

To use the library you need to first call an initialize function with the PID constants and feedback sensor. This will return a pointer to an internal structure that the library uses to maintain the control variables. An update function then needs to be called on a periodic basis in a task, this will return a new drive variable that can be sent to the motors.

Here is an example showing its most simple use.

#pragma config(Sensor, dgtl1,  arm_position,   sensorQuadEncoder)
#pragma config(Motor,  port1,  ArmMotor,      tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*  Example for PID library                                                    */
/*  James Pearman                                                              */
/*  March 12 2013                                                              */
/*                                                                             */
/*  One motor arm control using quad encoder                                   */

#include "PidLib.c"

// global to hold arm pid controller pointer
pidController   *arm_pid;

/*  Task to update arm pid loop and control arm motor                          */

task ArmControl()
        if( arm_pid != NULL )
            // update pid control
            PidControllerUpdate( arm_pid );    
            // drive motor
            motor ArmMotor ] = arm_pid->drive_cmd;
/*  Send arm to position                                                       */

ArmSetPosition( short position )
    if( arm_pid != NULL )
        arm_pid->target_value = position;

/*  Wait for arm to be in position                                             */

ArmWaitInPosition( short timeout = 3000 )
    // default is 3 seconds
    short   count = timeout;

    while(count >= 0 )
        // wait so that pid has calculated error

        // Check the pid error for the arm
        if( (abs(arm_pid->error) < 50 ) )

        // decrease timeout
        count -= 50;


task main()
    // Init - no bias
    // Kp = 0.004, Ki = 0, Kd = 0.01
    arm_pid = PidControllerInit( 0.004, 0.0, 0.01, arm_position, 0 );

    // Start the arm pid controller
    StartTask( ArmControl );
        // Send arm up
        ArmSetPosition( 1000 );
        // Wait for it to get there
        // send arm down
        ArmSetPosition( 100 );
        // Wait for it to get there

The library has the following public functions.

pidController *PidControllerInit( float Kp, float Ki, float Kd, tSensors port, short sensor_reverse = 0 );
pidController *PidControllerInit( float Kp, float Ki, float Kd, float Kbias, tSensors port, short sensor_reverse = 0 );
short PidControllerUpdate( pidController *p );

and one private function not meant to be called by the user.

void PidControllerMakeLut();

This initializes my non-linear motor control LUT, you get that for free :slight_smile:

The library can be downloaded from here.

pid library

The next part is just for information.

Normally in my code the pointer to the pid controller will be part of a larger structure holding other variables associated with whatever is being controlled. For example, here is what I use as a top level arm controller.

/*  All the variables for arm control                                          */

typedef struct {
    // local copy of pid feedback port
    tSensors        sensor_port;

    // absolute arm limits
    long            upper_limit;
    long            lower_limit;

    // pid controller ptr
    pidController   *arm_pid;

    // Up to 4 motors for arm
    short           motor_num;
    tMotor          motors[kMaxArmMotors];

    // override power to arm in case PTC trips
    short           motor_off;

    // manual overide of PID control
    short           user_manual;

    // vexnet controller
    vexrtController controller;

    // presets
    vexPreset       presets;

    // limit switches if they are installed
    tSensors        limitSwitchLow;
    tSensors        limitSwitchHigh;

    // sensor counts per degree after gearing
    float           CountsPerDeg;
    } armController;

static  armController   theArm;

You can see that there are many other things associated with arm control beyond just the pid. The “real” arm control code handles things like upper and lower limits for the arm, preset positions (another library I may release sometime) and connections between the joystick control and arm movement.

Anyway, enjoy the library.

1 Like

I wanted to add two more points.

  1. This is ROBOTC only.
  2. Although this code simplifies adding PID control to your code it does not do anything to help you tune the PID constants, that’s the hard part.
1 Like

How do I turn this into a speed control though?

So this thread is roughly 3 years old. I would suggest starting with James’ thread about velocity PID.