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.
-
Read the current position of the motor using the designated sensor.
-
Calculate the difference between where you want the motor to be and where it actually is, we will call this variable the “error”
-
Calculate the difference between the current error and the error last time you did the calculation, we will call this variable the “derivative”
-
Add the error into the sum of all the errors from previous calculations, we call this variable the “integral”.
-
Limit the integral variable so it doesn’t get to large.
-
Do the PID calculation, this calculates the motor control value to send to the motor.
-
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…