PID Controller, Auto-tuning Library and Example for DC Motor

PID Controller, Auto-tuning Library and Example for DC Motor

Postby Khanh » Mon Nov 20, 2017 4:19 pm

This article provides libraries and examples code of controlling position and speed of DC motor using PID controller and auto-tuning.

Introduction
Have you ever heard about PID controller?

Refer to: Wikipedia Definition

It is a controller that is widely used in industrial to control various process variables such as temperature, pressure, force, feed rate, flow rate, chemical composition (component concentrations), weight, position, speed, and practically every other variable for which a measurement exists.

In this article, I focus on using PID to control position and speed of Brushed DC motor.

What decides performance of a PID controller?

Performance of PID is decided by Kp, Ki and Kd gain (coefficients for the proportional, integral, and derivative terms). Each system has its own optimal PID gain. Finding the best PID gains is difficult task. Therefore, we usually use an acceptable gain. This link give an illustration of impact of Kp, Ki and Kd on system

The process of finding value of these parameters is called "tuning". PID controller can manually tuning and auto-tuning...
- Manually tuning: This method requires experienced personnel
- Auto-tuning: the tuning is done by a software.

I implemented Auto-tuning library for position and speed of DC motor (see the source code) using Relay On/Off method. This code is written for PHPoC platform.

PID gain from auto-tuning is not the best gain. You can manually fine-tune based on PID gain from auto-tuning.

Thing used in this project
- PHPoC Blue or Black
- PHPoC DC Motor Controller
- DC Motor with Encoder

System Architecture of PID Controller
system_architecture.jpg
system_architecture.jpg (62.04 KiB) Viewed 163 times

PID is closed-loop system, we need a feedback from DC motor. Therefore, to use PID control, DC motor need to has an encoder.

Encoder will output the signal, which is used to calculated the real position and speed. The calculation of position and speed is performed by DC motor controller. DC motor sends the calculated value (called feedback value) back to PHPoC Blue or Black. Library on PHPoC will perform adjustment based on the feedback value, desired value, Kp, Ki and Kd gain, and staling factor. after adjusting, PHPoC send command along with PWM duty-cycle value to DC motor controller, DC motor will output PWM signal to control DC motor. This process is repeated in a infinite loop.

Wiring
1. Stack DC motor controller on PHPoC Blue/Black

2. Connect DC motor to DC motor controller (see User Manual of DC motor Controller )

Examples
Note that, different motor has different behavior. You may need to adjust some parameters such as:
- Scaling factor: $pid[INDEX_PID_SCALE]
- Sampling time: $pid[INDEX_SAMPLE_TIME]
- Limit of integral: $pid[INDEX_INT_MAX] and $pid[INDEX_INT_MIN]
to meet your system requirement.

Position Control

In the example, setpoint of position is changed. No overshoot occurs
position_control.jpg
position_control.jpg (16.01 KiB) Viewed 163 times


source code (task0.php)
Code: Select all
<?php
include_once 
"/lib/sd_340.php";
include_once "/lib/sd_spc.php";
include_once "vc_pid.php";

$sid = 14;
$dc_id = 1;
$pwm_period = 10000; //10000 us

st_free_setup(0, "us");

spc_reset();
spc_sync_baud(460800);
spc_request_dev($sid, "dc$dc_id pwm set period $pwm_period");
spc_request_dev($sid, "dc$dc_id enc set psr 64");
spc_request_dev($sid, "dc$dc_id pwm set decay slow");
spc_request_dev($sid, "dc$dc_id lpf set freq 5000");

/*----------- check enc pol-------------- */
$pwm = 0; $pos = 0;
while(!
$pos)
{
    $pwm += 10;
    spc_request_dev($sid, "dc$dc_id pwm set width $pwm");
    $pos = (int)spc_request_dev($sid, "dc$dc_id enc get pos");
    usleep(1000);
}

if(
$pos < 0)
    spc_request_dev($sid, "dc$dc_id enc set pol -");
    
spc_request_dev
($sid, "dc$dc_id pwm set width 0");

/*-----------PID parameters-----------*/
$pid = array(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);

//$pid[INDEX_KP_GAIN]    = 0; // manually set by user or automatically set by auto-tuning function
//$pid[INDEX_KI_GAIN]    = 0; // manually set by user or automatically set by auto-tuning function
//$pid[INDEX_KD_GAIN]    = 0; // manually set by user or automatically set by auto-tuning function
$pid[INDEX_INT_TERM]    = 0; // initial value
$pid[INDEX_PRE_ERROR]    = 0; // initial value
$pid[INDEX_INT_MAX]      = 6000; // recommended vaule <= 100% of pwm width for position control
$pid[INDEX_INT_MIN]        = -6000; // opposite of upper limit
$pid[INDEX_PRE_TIME]    = st_free_get_count(0); //current time
$pid[INDEX_SAMPLE_TIME]    = 10000; // in micro-second
$pid[INDEX_PID_TYPE]    = PID_TYPE_POS; // position control
$pid[INDEX_PID_SCALE]    = 16; // depending on motor, should test manually to have the best value
$pid[INDEX_PWM_PERIOD]    = $pwm_period; 

$setpoint 
= 2000; //desired position

/*----------- PID auto-tuning-------------- */
dc_pid_tune_start();
while(!
dc_pid_tune_loop($sid, $dc_id, $pid))
    usleep(10000);

spc_request_dev($sid, "dc$dc_id enc set pos 0");

/*----------- loop -----------*/
$i = 0;
while(
$i < 3000)
{
    if($i == 0)
        $setpoint = 2000;
    else if($i == 1000)
        $setpoint = 3000;
    else if($i == 2000)
        $setpoint = 1000;
    
    dc_pid_loop
($sid, $dc_id, $setpoint, $pid);

    $pos = (int)spc_request_dev($sid, "dc$dc_id enc get pos");    
    echo 
"$pos $setpoint\r\n";
    $i++;
    
    usleep
(1000);
}

spc_request_dev($sid, "dc$dc_id pwm set width 0");

?>


Speed Control

In the example, setpoint of speed is changed.
speed_control.jpg
speed_control.jpg (18.89 KiB) Viewed 163 times


source code (task0.php)
Code: Select all
<?php
include_once 
"/lib/sd_340.php";
include_once "/lib/sd_spc.php";
include_once "vc_pid.php";

function dc_get_speed($sid, $dc_id)
{
    $enc_period = (int)spc_request_dev($sid, "dc$dc_id enc get period");

    if(!$enc_period)
        $speed = 1;
    else
        $speed 
= 1000000 / $enc_period;
    
    return $speed
;
}

$sid = 14;
$dc_id = 1;
$pwm_period = 10000; //10000 us

st_free_setup(0, "us");

spc_reset();
spc_sync_baud(460800);
spc_request_dev($sid, "dc$dc_id pwm set period $pwm_period");
spc_request_dev($sid, "dc$dc_id enc set psr 64");
spc_request_dev($sid, "dc$dc_id pwm set decay slow");
spc_request_dev($sid, "dc$dc_id lpf set freq 5000");


/*-----------PID parameters-----------*/
$pid = array(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);

//$pid[INDEX_KP_GAIN]    = 0; // manually set by user or automatically set by auto-tuning function
//$pid[INDEX_KI_GAIN]    = 0; // manually set by user or automatically set by auto-tuning function
//$pid[INDEX_KD_GAIN]    = 0; // manually set by user or automatically set by auto-tuning function
$pid[INDEX_INT_TERM]    = 0; // initial value
$pid[INDEX_PRE_ERROR]    = 0; // initial value
$pid[INDEX_INT_MAX]      = 0; // disble limit
$pid[INDEX_INT_MIN]        = 0; // disble limit
$pid[INDEX_PRE_TIME]    = st_free_get_count(0); //current time
$pid[INDEX_SAMPLE_TIME]    = 10000; // in micro-second
$pid[INDEX_PID_TYPE]    = PID_TYPE_SPEED; // speed control
$pid[INDEX_PID_SCALE]    = 8; // depending on motor, should test manually to have the best value
$pid[INDEX_PWM_PERIOD]    = $pwm_period; 

$setpoint 
= 3000; //desired speed

/*----------- PID auto-tuning-------------- */
dc_pid_tune_start();
while(!
dc_pid_tune_loop($sid, $dc_id, $pid))
    usleep(10000);

/*----------- loop -----------*/
$i = 0;
while(
$i < 3000)
{
    if($i == 0)
        $setpoint = 3000;
    else if($i == 1000)
        $setpoint = 4000;
    else if($i == 2000)
        $setpoint = 1000;
    
    dc_pid_loop
($sid, $dc_id, $setpoint, $pid);

    $speed = dc_get_speed($sid, $dc_id);    
    echo 
"$speed\r\n";
    $i++;
    
    usleep
(1000);
}

spc_request_dev($sid, "dc$dc_id pwm set width 0");

?>


Library Code

source code (vc_pid.php)
Code: Select all
<?php

define
("INDEX_KP_GAIN",        0); // index of proportional gain in pid array
define("INDEX_KI_GAIN",        1); // index of integral gain in pid array 
define("INDEX_KD_GAIN",        2); // index of differential gain in pid array 
define("INDEX_INT_TERM",    3); // index of integral term in pid array
define("INDEX_PRE_ERROR",    4); // index of last error in pid array 
define("INDEX_INT_MAX",        5); // index of maximum integral in pid array 
define("INDEX_INT_MIN",        6); // index of minimum integral in pid array 
define("INDEX_PRE_TIME",    7); // index of last calculated time in pid array
define("INDEX_SAMPLE_TIME",    8); // index of sampling time
define("INDEX_PID_TYPE",     9); // index of pid type: speed: PID_TYPE_SPEED or position: PID_TYPE_POS
define("INDEX_PID_SCALE",     10); // index of pid scale factor
define("INDEX_PWM_PERIOD",     11); // index of PWM period

define("PID_TYPE_NONE",  0);
define("PID_TYPE_POS",   1);
define("PID_TYPE_SPEED", 2);

define("INT_OVERFLOW",     0x7fffffffffffffff);
define("INT_UNDERFLOW",     0x8000000000000000);

/* PID tuning state machine */
define("PID_TUNE_STATE_IDLE",   0);
define("PID_TUNE_STATE_START",  1);
define("PID_TUNE_STATE_WAIT",   2);
define("PID_TUNE_STATE_LOOP",   3);
define("PID_TUNE_STATE_END",    4);

$pid_tune_state         = PID_TUNE_STATE_START;

$pid_tune_input         = 0;
$pid_tune_base_input    = 0;
$pid_tune_step             = 0;
$pid_tune_setpoint         = 0;
$pid_tune_pre_feedback     = 0;
$pid_tune_cur_feedback     = 0;

$pid_tune_change_dir     = 0; //0: initial, 1: go up, -1 go down
$pid_tune_max_sum         = 0.0;
$pid_tune_min_sum         = 0.0;
$pid_tune_max_count     = 0;
$pid_tune_min_count     = 0;
$pid_tune_noiseband     = 2;
$pid_tune_start_time     = 0;
$pid_tune_end_time         = 0;
$pid_tune_wait_count     = 0;

function dc_pid_compute($target, $feedback, &$dc_pid)
{
    $cur_time = st_free_get_count(0);
    $sample_time = ($cur_time - $dc_pid[INDEX_PRE_TIME]) / 1000000.0;
    
    if
($sample_time <=0) 
        return 0
;
    
    $Kp 
= $dc_pid[INDEX_KP_GAIN];
    $Ki = $dc_pid[INDEX_KI_GAIN];
    $Kd = $dc_pid[INDEX_KD_GAIN];
    $I_term = $dc_pid[INDEX_INT_TERM];
    $pre_err = $dc_pid[INDEX_PRE_ERROR];
    
    $cur_err 
= $target - $feedback;
    
    
/* proportional term computation */
    $P_term = $Kp * $cur_err;
    
    
/* integral term computation */
    if((float)$Ki == 0.0)
        $I_term = 0;
    else
    
{
        $partition = $Ki * $cur_err * $sample_time;
        
        if
($partition > 0)
            $partition  = ceil($partition);
        else
            $partition  
= floor($partition);
        
        $I_term 
+= $partition;
        
        
/*
        // Check  integral overflow    
        if($I_term > 0 && $partition < 0 && $dc_pid[INDEX_INT_TERM] < 0)
                $I_term = INT_UNDERFLOW;
        
        else if($I_term < 0 && $partition > 0 && $dc_pid[INDEX_INT_TERM] > 0)
                $I_term = INT_OVERFLOW;
        */
        
        
// Check integral bound
        if($dc_pid[INDEX_INT_MAX] != 0 &&($I_term > $dc_pid[INDEX_INT_MAX]))
            $I_term = $dc_pid[INDEX_INT_MAX];
        
        if
($dc_pid[INDEX_INT_MIN] != 0 &&($I_term < $dc_pid[INDEX_INT_MIN]))
            $I_term = $dc_pid[INDEX_INT_MIN];
    }
    
    
/* differential term computation */
    $D_term = $Kd * ($cur_err - $pre_err) / $sample_time;
    
    $output 
= round($P_term + $I_term + $D_term);
    
    $dc_pid
[INDEX_INT_TERM] = $I_term;
    $dc_pid[INDEX_PRE_ERROR] = $cur_err;
    $dc_pid[INDEX_PRE_TIME] = $cur_time;
    
    return 
(int)$output;
}

function dc_pid_loop($sid, $dc_id, $target, &$pid)
{
    $cur_time = st_free_get_count(0);
    $sample_time = $cur_time - $pid[INDEX_PRE_TIME];

    if($sample_time >= $pid[INDEX_SAMPLE_TIME])
    {
        if($pid[INDEX_PID_TYPE] == PID_TYPE_POS)
        {
            $cur_pos = (int)spc_request_dev($sid, "dc$dc_id enc get pos");

            $pwm_width = (int)dc_pid_compute($target, $cur_pos, $pid) / $pid[INDEX_PID_SCALE];

            if($pwm_width >= 0)
                $dir = "+";
            else
            
{
                $dir = "-";
                $pwm_width *= -1;
            }

            if($target == $cur_pos)
            {
                $pid[INDEX_INT_TERM] = 0;
                $pwm_width = 0;
            }

            spc_request_dev($sid, "dc$dc_id pwm set dir $dir");
            spc_request_dev($sid, "dc$dc_id pwm set width $pwm_width");
        }
        else if($pid[INDEX_PID_TYPE] == PID_TYPE_SPEED)
        {
            $enc_period = (int)spc_request_dev($sid, "dc$dc_id enc get period");

            if(!$enc_period)
                $cur_speed = 1;
            else
                $cur_speed 
= 1000000 / $enc_period;

            $pwm_width = (int)dc_pid_compute($target, $cur_speed, $pid) / $pid[INDEX_PID_SCALE];

            if($target == $cur_speed)
                return;

            if($pwm_width < 0)
                $pwm_width = 0;

            spc_request_dev($sid, "dc$dc_id pwm set width $pwm_width");
        }
    }
}

function dc_pid_tune_start()
{
    global $pid_tune_state;
    $pid_tune_state = PID_TUNE_STATE_START;
}

function dc_pid_tune_loop($sid, $dc_id, &$pid)
{
    global $pid_tune_state;

    global $pid_tune_input;
    global $pid_tune_base_input;
    global $pid_tune_step;
    global $pid_tune_setpoint;
    global $pid_tune_pre_feedback;
    global $pid_tune_cur_feedback;

    global $pid_tune_change_dir;
    global $pid_tune_max_sum;
    global $pid_tune_min_sum;
    global $pid_tune_max_count;
    global $pid_tune_min_count;
    global $pid_tune_noiseband;
    global $pid_tune_start_time;
    global $pid_tune_end_time;

    global $pid_tune_wait_count;

    switch($pid_tune_state)
    {
        case PID_TUNE_STATE_START:
            //echo "PID: Tuning...\r\n";

            $pwm_period = $pid[INDEX_PWM_PERIOD];

            if($pid[INDEX_PID_TYPE] == PID_TYPE_POS)
            {
                $pid_tune_base_input = 0;
                $pid_tune_step = $pwm_period * 3 / 10; /* 30% */
            }
            else if($pid[INDEX_PID_TYPE] == PID_TYPE_SPEED)
            {
                $pid_tune_base_input = $pwm_period * 4 / 10; /* 40% */
                $pid_tune_step = $pwm_period / 10; /* 10% */
            }
            else
                return false
;

            $pid_tune_input = $pid_tune_base_input;

            spc_request_dev($sid, "dc$dc_id pwm set width $pid_tune_input");

            $pid_tune_wait_count = 500000 / $pid[INDEX_SAMPLE_TIME]; // 500000 us
            $pid_tune_state = PID_TUNE_STATE_WAIT;
            return false;

        case PID_TUNE_STATE_WAIT:
            /* wait to let process settle to a steady state */
            if($pid_tune_wait_count--)
                return false;

            if($pid[INDEX_PID_TYPE] == PID_TYPE_POS)
                $pid_tune_setpoint = (int)spc_request_dev($sid, "dc$dc_id enc get pos");
            else if($pid[INDEX_PID_TYPE] == PID_TYPE_SPEED)
            {
                $enc_period = (int)spc_request_dev($sid, "dc$dc_id enc get period");

                if(!$enc_period)
                    $enc_pps = 1;
                else
                    $enc_pps 
= 1000000 / $enc_period;
                
                $pid_tune_setpoint 
= $enc_pps;
            }
            else
                return false
;

            $pid_tune_pre_feedback = $pid_tune_setpoint;

            $pid_tune_change_dir = 0;
            $pid_tune_max_sum = 0.0;
            $pid_tune_min_sum = 0.0;
            $pid_tune_max_count = 0;
            $pid_tune_min_count = 0;
            $pid_tune_noiseband = 4;
            $pid_tune_start_time = 0;
            $pid_tune_end_time = 0;

            $pid_tune_input = $pid_tune_base_input + $pid_tune_step;

            $pid_tune_state = PID_TUNE_STATE_LOOP;
            return false;

        case PID_TUNE_STATE_LOOP:
            if($pid[INDEX_PID_TYPE] == PID_TYPE_POS)
                $pid_tune_cur_feedback = (int)spc_request_dev($sid, "dc$dc_id enc get pos");
            else if($pid[INDEX_PID_TYPE] == PID_TYPE_SPEED)
            {
                $enc_period = (int)spc_request_dev($sid, "dc$dc_id enc get period");
                
                if
(!$enc_period)
                    $enc_pps = 1;
                else
                    $enc_pps 
= 1000000 / $enc_period;
                
                $pid_tune_cur_feedback 
= $enc_pps;
            }
            else
                return false
;

            if($pid_tune_cur_feedback > $pid_tune_pre_feedback)
            {
                if($pid_tune_change_dir == -1)
                {
                    if($pid_tune_max_count >= 2) // Ignore the first maximum
                    {
                        $pid_tune_min_sum += $pid_tune_pre_feedback;
                        $pid_tune_min_count++;
                    }
                }

                $pid_tune_change_dir = 1;
            }
            else
            if
($pid_tune_cur_feedback < $pid_tune_pre_feedback)
            {
                if($pid_tune_change_dir == 1)
                {
                    if($pid_tune_max_count >= 1)
                        $pid_tune_max_sum += $pid_tune_pre_feedback;

                    $pid_tune_max_count++;

                    if($pid_tune_max_count == 2)
                        $pid_tune_start_time = st_free_get_count(0);
                    else if($pid_tune_max_count == 12)
                    {
                        $pid_tune_end_time = st_free_get_count(0);
                        $pid_tune_state = PID_TUNE_STATE_END;
                        return false;
                    }
                }

                $pid_tune_change_dir = -1;
            }

            if($pid_tune_setpoint > ($pid_tune_cur_feedback + $pid_tune_noiseband))
                $pid_tune_input = $pid_tune_base_input + $pid_tune_step;
            else if($pid_tune_setpoint < ($pid_tune_cur_feedback - $pid_tune_noiseband))
                $pid_tune_input = $pid_tune_base_input - $pid_tune_step;

            if($pid_tune_input >= 0)
            {
                spc_request_dev($sid, "dc$dc_id pwm set dir +");
                spc_request_dev($sid, "dc$dc_id pwm set width $pid_tune_input");
            }
            else
            
{
                $width = -* $pid_tune_input;
                spc_request_dev($sid, "dc$dc_id pwm set dir -");
                spc_request_dev($sid, "dc$dc_id pwm set width $width");
            }

            $pid_tune_pre_feedback = $pid_tune_cur_feedback;
            return false;

        case PID_TUNE_STATE_END:
            /*stop motor*/
            spc_request_dev($sid, "dc$dc_id pwm set width 0");

            /*evaluate ultimate gain using autotune formulas*/

            $avr_max = ((float)$pid_tune_max_sum) / ($pid_tune_max_count - 1);
            $avr_min = ((float)$pid_tune_min_sum) / $pid_tune_min_count;
            $a = ($avr_max - $avr_min) / 2.0;

            $Tu = ($pid_tune_end_time - $pid_tune_start_time) / 10.0 / 1000000; //average of 10 cycles in second
            $Ku = 4 * $pid_tune_step / (M_PI * $a);

            /*
            //classic PID according to Ziegler–Nichols method
            $pid[INDEX_KP_GAIN] = 0.6 * $Ku;
            $pid[INDEX_KI_GAIN] = 2 / $Tu ;
            $pid[INDEX_KD_GAIN] = $Tu / 8;
            */

            /*no overshoot according to Ziegler–Nichols method*/
            $pid[INDEX_KP_GAIN] = 0.2 * $Ku;
            $pid[INDEX_KI_GAIN] = 2.0 / $Tu;
            $pid[INDEX_KD_GAIN] = $Tu / 3.0;

            $pid_tune_state = PID_TUNE_STATE_IDLE;

            echo "PID TUNED Kp: ", $pid[INDEX_KP_GAIN], "\r\n";
            echo "PID TUNED Ki: ", $pid[INDEX_KI_GAIN], "\r\n";
            echo "PID TUNED Kd: ", $pid[INDEX_KD_GAIN], "\r\n";
            return true;

        default:
            return true;
    }
}
?>
Khanh
 
Posts: 73
Joined: Fri Mar 11, 2016 10:57 am

Return to Project

Who is online

Users browsing this forum: No registered users and 1 guest