Simple PID for EasyC (and ConVEX)

Everyone keeps asking about PID control in EasyC. So here is some simple code that shows the basics of how to do it.

There are some pre-requisites for any system using PID.

A. You need something to measure the position of the controlled motor. For many of the robots you are designing this will usually be an IME, a quadrature encoder or a potentiometer. It is, however, also possible to use a gyro, accelerometer or ultrasonic sensor as part of a PID closed loop controller.

B. You need a way of updating the PID variables at a constant rate OR you need to modify the calculations to include time (or rather the change in time). I recommend you just update the PID variables at a constant rate.

The PID calculation consists of the following steps.

  1. Read the current position of the motor using the designated sensor.

  2. Calculate the difference between where you want the motor to be and where it actually is, we will call this variable the “error”

  3. Calculate the difference between the current error and the error last time you did the calculation, we will call this variable the “derivative”

  4. Add the error into the sum of all the errors from previous calculations, we call this variable the “integral”.

  5. Limit the integral variable so it doesn’t get to large.

  6. Do the PID calculation, this calculates the motor control value to send to the motor.

  7. limit the motor control value to the range +/- 127 and send to the motor.

All of the above are put into a function I’m calling simplePid in the example code. We call this function at a constant rate, either in the main loop of the program (not so good) or running in the background by using a repeating timer in EasyC or a task in ConVEX/PROS or ROBOTC.

To move the motor to a new position we have to change the position where we want the motor to be. I call this position the “target position”, I have a function called simpleSetTarget to do that.

For EasyC the pid code looks like this.

I’m using a quadrature encoder on inputs 3&4 to measure the motor’s position.
The motor is on port 1.

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*                        Copyright (c) James Pearman                          */
/*                                   2015                                      */
/*                            All Rights Reserved                              */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    Module:     simplePid.c                                                  */
/*    Author:     James Pearman                                                */
/*    Created:    2 Feb 2015                                                   */
/*                                                                             */
/*    Revisions:                                                               */
/*                V1.00  XX XXX 2015 - Initial release                         */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    The author is supplying this software for use with the VEX cortex        */
/*    control system. This file can be freely distributed and teams are        */
/*    authorized to freely use this program , however, it is requested that    */
/*    improvements or additions be shared with the Vex community via the vex   */
/*    forum.  Please acknowledge the work of the authors when appropriate.     */
/*    Thanks.                                                                  */
/*                                                                             */
/*    Licensed under the Apache License, Version 2.0 (the "License");          */
/*    you may not use this file except in compliance with the License.         */
/*    You may obtain a copy of the License at                                  */
/*                                                                             */
/*      http://www.apache.org/licenses/LICENSE-2.0                             */
/*                                                                             */
/*    Unless required by applicable law or agreed to in writing, software      */
/*    distributed under the License is distributed on an "AS IS" BASIS,        */
/*    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */
/*    See the License for the specific language governing permissions and      */
/*    limitations under the License.                                           */
/*                                                                             */
/*    The author can be contacted on the vex forums as jpearman                */
/*    or electronic mail using jbpearman_at_mac_dot_com                        */
/*    Mentor for team 8888 RoboLancers, Pasadena CA.                           */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

#include "main.h"       // needs for all ChibiOS programs

// I'm using static variables and only have one PID controller
// better to group into a structure but this is simplified code.
static  volatile int    target_position = 0;
static  int derivative = 0;
static  int integral   = 0;
static  int last_error = 0;
static  int integral_limit = 500;

// PID constants, these are the things you need to change and tune
const float Kp    = 0.1;
const float Kd    = 0.1;
const float Ki    = 0.0;
const float Kbias = 0.0;

// Set a new target position
void
simpleSetTarget( int target )
{
    target_position = target;
}
// PID controller for the motor on port 1
// sensor is a quadrature encoder on digital port 3 & 4
//
void
simplePid()
{
    int     current_position;
    int     error;
    float   drive;
    int     drive_raw;

    // Get current position of the encoder
    current_position = GetQuadEncoder ( 3 , 4 );
  
    // calculate the error
    error = target_position - current_position;

    // put a small deadband on the error
    if( error < 4 && error > -4 ) error = 0;

    // derivative
    derivative = error - last_error;
    last_error = error;

    // Integral
    integral += error;

    // limit to avoid windup
    if( integral >  integral_limit ) integral =  integral_limit;
    if( integral < -integral_limit ) integral = -integral_limit;

    // calculate drive - no delta T in this version
    drive = (Kp * error) + (Ki * integral) + (Kd * derivative) + Kbias;

    // drive should be in the range +/- 1.0
    if( drive >  1.0 ) drive =  1.0;
    if( drive < -1.0 ) drive = -1.0;

    // final motor output
    drive_raw = drive * 127.0;
 
    // Send to the motor   
    SetMotor ( 1 , drive_raw ) ; 
}

The code in OperatorControl would look something like this.

#include "Main.h"

void OperatorControl ( unsigned long ulTime )
{
    StartQuadEncoder ( 3 , 4 , 0 ) ;
    
    // Start a timer to do the calculations
    RegisterRepeatingTimer( 25, simplePid );
    
    while ( 1 ) // Insert Your RC Code Below
        {
        if ( GetJoystickDigital( 1, 8, 2 ) )
            {
            simpleSetTarget( 360 );
            }
        if ( GetJoystickDigital( 1, 8, 1 ) )
            {
            simpleSetTarget( 0 );
            }
        
        // This would be an alternative, not so good, way of calling the
        // function that does the calculations
        //simplePid();
        Wait ( 25 ) ;
      }
}

I’m using two buttons to send the motor to position 360 or 0.

continued in the next post…

2 Likes

continued from post #1

So just to illustrate how similar all these languages are, here is the same PID code written for ConVEX.

/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*                        Copyright (c) James Pearman                          */
/*                                   2015                                      */
/*                            All Rights Reserved                              */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    Module:     simplePid.c                                                  */
/*    Author:     James Pearman                                                */
/*    Created:    2 Feb 2015                                                   */
/*                                                                             */
/*    Revisions:                                                               */
/*                V1.00  XX XXX 2015 - Initial release                         */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    The author is supplying this software for use with the VEX cortex        */
/*    control system. This file can be freely distributed and teams are        */
/*    authorized to freely use this program , however, it is requested that    */
/*    improvements or additions be shared with the Vex community via the vex   */
/*    forum.  Please acknowledge the work of the authors when appropriate.     */
/*    Thanks.                                                                  */
/*                                                                             */
/*    Licensed under the Apache License, Version 2.0 (the "License");          */
/*    you may not use this file except in compliance with the License.         */
/*    You may obtain a copy of the License at                                  */
/*                                                                             */
/*      http://www.apache.org/licenses/LICENSE-2.0                             */
/*                                                                             */
/*    Unless required by applicable law or agreed to in writing, software      */
/*    distributed under the License is distributed on an "AS IS" BASIS,        */
/*    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. */
/*    See the License for the specific language governing permissions and      */
/*    limitations under the License.                                           */
/*                                                                             */
/*    The author can be contacted on the vex forums as jpearman                */
/*    or electronic mail using jbpearman_at_mac_dot_com                        */
/*    Mentor for team 8888 RoboLancers, Pasadena CA.                           */
/*                                                                             */
/*-----------------------------------------------------------------------------*/

#include "ch.h"         // needs for all ChibiOS programs
#include "hal.h"        // hardware abstraction layer header
#include "vex.h"        // vex library header

// I'm using static variables and only have one PID controller
// better to group into a structure but this is simplified code.
static  volatile int    target_position = 0;
static  int derivative = 0;
static  int integral   = 0;
static  int last_error = 0;
static  int integral_limit = 500;

// PID constants, these are the things you need to change and tune
const float Kp    = 0.1;
const float Kd    = 0.1;
const float Ki    = 0.0;
const float Kbias = 0.0;

// Set a new target position
void
simpleSetTarget( int target )
{
    target_position = target;
}
// PID controller for the motor on port 1
// sensor is a quadrature encoder on digital port 3 & 4
//
void
simplePid()
{
    int     current_position;
    int     error;
    float   drive;
    int     drive_raw;

    // Get current position of the encoder
    current_position = vexEncoderGet( kVexQuadEncoder_1 );

    // calculate the error
    error = target_position - current_position;

    // put a small deadband on the error
    if( error < 4 && error > -4 ) error = 0;

    // derivative
    derivative = error - last_error;
    last_error = error;

    // Integral
    integral += error;

    // limit to avoid windup
    if( integral >  integral_limit ) integral =  integral_limit;
    if( integral < -integral_limit ) integral = -integral_limit;

    // calculate drive - no delta T in this version
    drive = (Kp * error) + (Ki * integral) + (Kd * derivative) + Kbias;

    // drive should be in the range +/- 1.0
    if( drive >  1.0 ) drive =  1.0;
    if( drive < -1.0 ) drive = -1.0;

    // final motor output
    drive_raw = drive * 127.0;

    // Send to the motor
    vexMotorSet( kVexMotor_1, drive_raw );
}

and the operator control task.

msg_t
vexOperator( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("operator");

    StartTask( simplePidTask );

    // Run until asked to terminate
    while(!chThdShouldTerminate())
        {
        if( vexControllerGet( Btn8U ) )
            simpleSetTarget( 360 );
        if( vexControllerGet( Btn8D ) )
            simpleSetTarget( 0 );

        //simplePid();

        // Don't hog cpu
        vexSleep( 25 );
        }

    return (msg_t)0;
}

I added one small task to call the pid calculation function.

msg_t
simplePidTask( void *arg )
{
    (void)arg;

    // Must call this
    vexTaskRegister("pid");

    // Run until asked to terminate
    while(!chThdShouldTerminate()) {
        simplePid();
        vexSleep( 25 );
    }
}

As the thread title says, this is very simple PID. If you try and use it note that it still needs tuning, that is, the values of the PID constants need to be set for your robot.

Kp is the proportional constant - how much of the “error” contributes to the motor drive.
Kd is the derivative constant - how much of the change in error, the “derivative” contributes to the motor drive.
Ki is the integral constant - how much of the accumulated error contributes to the final motor drive.
Kbias is a constant that is always added to the output, sometimes this is used to compensate for things like gravity, its usefulness seems marginal with VEX motors.

If you have questions about the C code then ask away, some of the syntax may not be familiar to EasyC users.

2 Likes

I was asked for the EasyC project, so here it is. This was done using EasyC V5 but should be compatible with V4, let me know if there is a problem.
simplepid.zip (4.93 KB)

2 Likes

Thanks for posting this, it was very helpful!

Good work James, on the full PID.

Just FIY for everyone there is a P Loop example in the Samples directory “PID Interrupt Service Routine”.

It isn’t compatible with easyc v4.

Yes it is, I just tried it in 4.2.1.9 and it opened right up.

Try opening it from inside easyC.

oh i’m running 4.2.1.8
quickly updating now

The code should be compatible with all versions of EasyC.

Even after updating it doesn’t open

Let me download the zip file and check to see if anything is corrupted.

Edit:
So the zip file is fine, this is what I did.

Click on link, this downloads the zip file.
goto the downloads folder, right click on the zip file and select “extract all”, extract to the default location.
Open EasyC, “select File->Open Project…”, navigate to the downloads folder, then the SimplePid folder and select the simplepid project.

1 Like

Did that and it still won’t open. oh well. it’s fine I’ll do some experimenting.

I have a question. I have the exact same program but I am not able to stop an arm with a potentiometer at resting position. What do you all suggest?

Can you make an example for PROS?