[Hellper 0]

Система управления стабилизацией скорости основана на использование ПИД регулятора:

#define SCALING_FACTOR  128

#include <limits.h>

typedef struct PID_DATA
{
    int16_t lastProcessValue;

    int32_t sumError;

    int16_t P_Factor;

    int16_t I_Factor;

    int16_t D_Factor;

    int16_t maxError;

    int32_t maxSumError;
} pidData_t;

#define MAX_INT         INT_MAX
#define MAX_LONG        LONG_MAX
#define MAX_I_TERM      ( MAX_LONG / 2 )

void pidInit( int16_t, int16_t, int16_t, struct PID_DATA * );
int16_t regulator( int16_t, int16_t, struct PID_DATA * );

void pidInit( int16_t p_factor, int16_t i_factor, int16_t d_factor, struct PID_DATA *pid )
{
    // Start values for PID controller
    pid->sumError = 0;
    pid->lastProcessValue = 0;
    // Tuning constants for PID loop
    pid->P_Factor = p_factor;
    pid->I_Factor = i_factor;
    pid->D_Factor = d_factor;
    // Limits to avoid overflow
    pid->maxError = MAX_INT / (pid->P_Factor + 1);
    pid->maxSumError = MAX_I_TERM / (pid->I_Factor + 1);
}

int16_t regulator( int16_t need, int16_t current, struct PID_DATA *pid )
{
    int32_t pwmOutput;
    int32_t temp;
    int16_t pTerm;
    int16_t iTerm;
    int16_t dTerm;
    int16_t error;

    error = need - current; 

    if ( error > pid -> maxError )
    {
        pTerm = MAX_INT;
    }
    else 
        if ( error < -pid -> maxError )
        {
            pTerm = -MAX_INT;
        }
        else
        {
            pTerm = pid -> P_Factor * error;
        };

    temp = pid -> sumError + error;
    if ( temp > pid -> maxSumError )
    {
        iTerm = MAX_I_TERM;
        pid -> sumError = pid -> maxSumError;
    }
    else 
        if ( temp < -pid -> maxSumError )
        {
            iTerm = -MAX_I_TERM;
            pid -> sumError = -pid -> maxSumError;
        }
        else
        {
            pid -> sumError = temp;
            iTerm = pid -> I_Factor * pid -> sumError;
        };

    dTerm = pid -> D_Factor * ( pid -> lastProcessValue - current );

    pid -> lastProcessValue = current;

    pwmOutput = pTerm + iTerm - dTerm;

    pwmOutput >>= 7;

    if ( pwmOutput < MIN_RM_MOTOR_OUTPUT )
        pwmOutput = MIN_RM_MOTOR_OUTPUT;
    else
        if ( pwmOutput > MAX_RM_MOTOR_OUTPUT )
              pwmOutput = MAX_RM_MOTOR_OUTPUT;

    return (int16_t)pwmOutput;
}

 

возвращаемое значение используется для коррекции мощности двигателя.

 

хочется сделать изменение скорости в зависимости от пройденного пути, те задается закон( трапеция, парабола, треугольник и тп) и в течение перемещения производиться коррекция необходимой скорости.

как наиболее эффективно и красиво реализовать ?

есть вариант разбивать перемещение на элементарные отрезки и высчитывать заранее точки, в которых изменять скорость, основываясь на показаниях датчиков обратной связи, и устанавливать требуемые значения для скоростного ПИД регулятора. Функции, по которым необходимо изменять закон движения, ввести как таблицы и масштабировать с использованием коэффициентов. Устремляя значения длин отрезков к малой величине добиться наибольшего соответствия действительных значений.

есть другой вариант ? проблемы малых перемещений ?

 

ПС: все происходит в микроконтроллере(Silabs)

Изменено 20 октября, 2009 пользователем Hellper

[khlenar 5]

Видимо с помощью линейной апроксимацией, как на программах ЧПУ при обработки сложных траекторий. Т.е. как вы писали, делить на элементарные отрезки. Можно

написать простенькую програмку которая бы преобразовывала требуемую функцию в соответствующую програмку для вашего контроллера.

С малыми перемещениями если хороший отклик на рассоглосование, не должно быть. Как обычно отработал соответствующий отрезок с соотвествующей скоростью, переход на

следующий кадр и т.д.

[Visor 0]

Реализуется элементарно, если есть функция описывающая путь во времени, то функция скорости будет производной этого пути. (школьный курс) :)