PID module

[jlamperez]

PID module

This module is divided in two files of source. The interface .h file and the implementation in the .c file. In the next image we can see the implemented functions and variables:

---

Declared variables:

// The minimum and maximum servo position as defined by 10-bit ADC values.
#define MIN_POSITION            (0)
#if ENCODER_ENABLED
#define MAX_POSITION            (4095)
#else
#define MAX_POSITION            (1023)
#endif

// The minimum and maximum output.
#define MAX_OUTPUT              (255)
#define MIN_OUTPUT              (-MAX_OUTPUT)

// Values preserved across multiple PID iterations.
static int16_t previous_seek=-1; // NOTE: previous_seek==-1 used to indicate initialisation required
static int16_t previous_position=0;
static int16_t i_component=0;
static int16_t seek_delta=-1;
static int16_t position_at_start_of_new_seek=-1;
static uint8_t previous_pwm_is_enabled=0;

Functions:

• pid_init(): Initialize the PID algorithm module.

previous_seek = -1;

• pid_registers_defaults(): Initialize the PID related register values.

    // Default deadband.
    registers_write_byte(REG_PID_DEADBAND, DEFAULT_PID_DEADBAND);

    // Default gain values.
    registers_write_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO, DEFAULT_PID_PGAIN);
    registers_write_word(REG_PID_IGAIN_HI, REG_PID_IGAIN_LO, DEFAULT_PID_IGAIN);
    registers_write_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO, DEFAULT_PID_DGAIN);

    // Default position limits.
    registers_write_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO, DEFAULT_MIN_SEEK);
    registers_write_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO, DEFAULT_MAX_SEEK);

    // Default reverse seek setting.
    registers_write_byte(REG_REVERSE_SEEK, 0x00);

• pid_position_to_pwm(int16_t position, uint8_t tick): Take the 10-bit position as input and output a signed PWM to be applied to the servo motors.

Is a modified pid algorithm by which the seek position and seek velocity are assumed to be a moving target. The algorithm attempts to output a pwm value that will achieve a predicted position and velocity.

• Declare static variables to keep them off the stack.

    static int16_t deadband;
    static int16_t p_component;
    static int16_t d_component;
    static int16_t seek_position;
    static int16_t seek_velocity;
    static int16_t minimum_position;
    static int16_t maximum_position;
    static int16_t current_velocity;
    static int16_t filtered_position;
    static int32_t pwm_output;
    static uint16_t p_gain;
    static uint16_t i_gain;
    static uint16_t d_gain;
    static uint8_t pwm_is_enabled;

• Get flag that indicates from registers if PWM is enabled or not. This saves fetching it each time we want to know.

     pwm_is_enabled=registers_read_byte(REG_FLAGS_LO)&(1<<FLAGS_LO_PWM_ENABLED);

• Filter the current position thru a digital low-pass filter (WHY?)

    filtered_position = filter_update(current_position);

• Use the filtered position to determine velocity. It depends on full rotation is enabled or not.

#if FULL_ROTATION_ENABLED
    current_velocity = normalize_position_difference(filtered_position - previous_position);
#else
    current_velocity = filtered_position - previous_position;
#endif

• Get from registers the position and velocity we are seeking.

    seek_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
    seek_velocity = (int16_t) registers_read_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO);

• Get the minimum and maximum position from registers.

    minimum_position = (int16_t) registers_read_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
    maximum_position = (int16_t) registers_read_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);

• Reverse seek(TODO, not clear)
• Get the deadband for the PID(what is deadband?)

   deadband = (int16_t) registers_read_byte(REG_PID_DEADBAND);

• Keep the seek position bound within the set minimum and maximum position and the hardware defined limits.

    if (seek_position < minimum_position) seek_position = minimum_position;
    if (seek_position > maximum_position) seek_position = maximum_position;
    if (seek_position < MIN_POSITION) seek_position = MIN_POSITION;
    if (seek_position > MAX_POSITION) seek_position = MAX_POSITION;

• Check for new seek target: If new seek position has been set or PWM enable state has changed we initialize and assigne values to variables.

    if(previous_seek != seek_position ||             // New seek position has been set...
       previous_pwm_is_enabled != pwm_is_enabled)    // PWM enable state has changed...
    { 
       if(previous_seek == -1)                       // Initialisation
       {
          previous_position = current_position;
          i_component = 0;
       }
       previous_seek = seek_position;
       seek_delta = current_position;
       position_at_start_of_new_seek = current_position;
       previous_pwm_is_enabled = pwm_is_enabled;
    }

• Update seek target if tick (the out time constant) and seek_delta is different of seek_position and seek_velocity is higher than 0.

    if(tick && seek_delta!=seek_position && seek_velocity>0) // Tick is our time constant
    {
       if(position_at_start_of_new_seek<seek_position)
       {
          seek_delta+=seek_velocity;    //increment the seek_delta (s = s0 +vt)
          if(seek_delta>=seek_position) // if seek_delta is higher we have reached to the seek_position
          {
             seek_delta=seek_position;
          }
       } else
       {
          if(position_at_start_of_new_seek>seek_position)
          {
             seek_delta-=seek_velocity;   //decrement the seek_delta (s = s0 -vt)
             if(seek_delta<=seek_position)// if seek_delta is less we have reached to the seek_position
             {
                seek_delta=seek_position;
             }
          }
       }
    }
    if(seek_delta==seek_position)
    {
       current_position = filtered_position;
    }

• Calculate the PWM

1) First p component:

#if FULL_ROTATION_ENABLED
    p_component = normalize_position_difference(seek_delta - current_position);
#else
    // The proportional component to the PID is the position error.
    p_component = seek_delta - current_position;
#endif

2) Then the i component

    // The integral component
    if(tick) // Tick is our time constant
    {
       i_component += p_component;
       if(i_component<-128) // Somewhat arbitrary anti integral wind-up; we're experimenting
       {
          i_component=-128;
       } else
       {
          if(i_component>128)
          {
             i_component=128;
          }
       }
    }

3) The d component of the PID, is the change in position

    d_component = previous_position - current_position;
    previous_position = current_position;

4)Get the proportional, derivative and integral gains from the registers.

    p_gain = registers_read_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO);
    i_gain = registers_read_word(REG_PID_IGAIN_HI, REG_PID_IGAIN_LO);
    d_gain = registers_read_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO);

5)Initialize the pwm output to zero and start adding p,i and d components multiplied by the gain pwm_output = 0;

// Apply proportional component to the PWM output if outside the deadband.
if ((p_component > deadband) || (p_component < -deadband))
{
    // Apply the proportional component of the PWM output.
    pwm_output += (int32_t) p_component * (int32_t) p_gain;

// Apply the integral component of the PWM output.
    pwm_output += (int32_t) i_component * (int32_t) i_gain;

// Apply the derivative component of the PWM output.
    pwm_output += (int32_t) d_component * (int32_t) d_gain;
} else
{
   i_component = 0;
}

6) Shift by 8 to account for the multiply by the 8:8 fixed point gain value. pot measurements are typically approaching 180 degrees across the 0 to 1023 ADC range. OpenEncoder is 360 degrees across the 0 to 4096 range.

#if ENCODER_ENABLED
    pwm_output >>= 9;
#else
    pwm_output >>= 8;
#endif

7)Check for saturation

    if (pwm_output > MAX_OUTPUT)
    {
        // Can't go higher than the maximum output value.
        pwm_output = MAX_OUTPUT;
    }
    else if (pwm_output < MIN_OUTPUT)
    {
        // Can't go lower than the minimum output value.
        pwm_output = MIN_OUTPUT;
    }

8)return the pwm_output

    return (int16_t) pwm_output;

Functions if full rotation is enabled:

• _normalize_positiondifference

#if FULL_ROTATION_ENABLED
static int16_t normalize_position_difference(int16_t posdiff)
{
    if (posdiff > ((MAX_POSITION - MIN_POSITION) / 2))
    {
        posdiff -= (MAX_POSITION - MIN_POSITION);
    }

    if (posdiff < -((MAX_POSITION - MIN_POSITION) / 2))
    {
        posdiff += (MAX_POSITION - MIN_POSITION);
    }

    return posdiff;
}
#endif

• Digital Lowpass Filter Implementation

// Digital Lowpass Filter Implementation
//
// See: A Simple Software Lowpass Filter Suits Embedded-system Applications
// http://www.edn.com/article/CA6335310.html
//
// k    Bandwidth (Normalized to 1Hz)   Rise Time (samples)
// 1    0.1197                          3
// 2    0.0466                          8
// 3    0.0217                          16
// 4    0.0104                          34
// 5    0.0051                          69
// 6    0.0026                          140
// 7    0.0012                          280
// 8    0.0007                          561
//

#define FILTER_SHIFT 1

static int32_t filter_reg = 0;

static int16_t filter_update(int16_t input)
{
#if 0
    // Update the filter with the current input.
#if FULL_ROTATION_ENABLED
    filter_reg += normalize_position_difference(input - (filter_reg >> FILTER_SHIFT));
#else
    filter_reg = filter_reg - (filter_reg >> FILTER_SHIFT) + input;
#endif

    // Scale output for unity gain.
    return (int16_t) (filter_reg >> FILTER_SHIFT);
#else
    return input;
#endif
}