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                                      */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    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.                                                                  */
/*                                                                             */
/*    you may not use this file except in compliance with the License.         */
/*    You may obtain a copy of the License at                                  */
/*                                                                             */
/*                                                                             */
/*    Unless required by applicable law or agreed to in writing, software      */
/*    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                                      */
/*                                                                             */
/*-----------------------------------------------------------------------------*/
/*                                                                             */
/*    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.                                                                  */
/*                                                                             */
/*    you may not use this file except in compliance with the License.         */
/*    You may obtain a copy of the License at                                  */
/*                                                                             */
/*                                                                             */
/*    Unless required by applicable law or agreed to in writing, software      */
/*    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

// 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 );
}

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

// Must call this

// 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;
}

msg_t
{
(void)arg;

// Must call this

// 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.