Easyelectronics.ru • Просмотр темы - Отследить разницу в позициях моторов.

Easyelectronics.ru

Электроника для всех
Текущее время: 19 авг 2018, 21:52

Часовой пояс: UTC + 5 часов



    • JLCPCB - Прототипы 10 PCBs всего за 2$ (100*100mm, 2-layer)
    • Как мы делаем платы, смотрите на YouTube
    • Крупнейшая китайская фабрика прототипов. 300000+ заказчиков и 10000+ заказов в день!
    • LCSC - Крупнейший китайский онлайн магазин комплектующих.

Начать новую тему Ответить на тему  [ Сообщений: 14 ] 
Автор Сообщение
 Заголовок сообщения: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 11:54 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
Когда было два мотора я по энкодерам брал их позицию, сравнивал, и если была разница в позициях больше пороговой - один мотор замедлял, другой ускорял.
Код:
uint32_t MOT_GetPosDif(uint32_t pos1, uint32_t pos2, uint32_t *gt)
{
    if( pos1 > pos2)
    {
        *gt = 0;
        return (pos1 - pos2);
    }
    else
    {
        *gt = 1;
        return (pos2 - pos1);
    }
}

void MOT_PositionLoopDouble(void)
{
    uint32_t position1, position2, posdif, gt;
    uint32_t pwm1, pwm2;
   
    position1 = motor1_data_double->position;
    position2 = motor2_data_double->position;
   
    posdif = MOT_GetPosDif(position1, position2, &gt);
   
    if (posdif > glob_mot_data.max_pos_diff)
    {
        //retry
        glob_mot_data.pos_diff_count++;
        if (glob_mot_data.pos_diff_count > glob_mot_data.max_pos_diff_count)
        {
            //maximum retries - unable to adjust speed
            glob_mot_data.pos_diff_count = 0;
            //send stop command
            MOT_SendData(motor1_data_double, CAN_COM_STOP, 0);
            MOT_SendData(motor2_data_double, CAN_COM_STOP, 0);
           
            sys_status |= MOT_POS_DIFF;
        }
    }
    else
        glob_mot_data.pos_diff_count = 0;
   
    //no position difference - maximum speed 
    if (posdif < glob_mot_data.max_pos_diff)
    {
        MOT_SendData(motor1_data_double, CAN_COM_SSET, glob_mot_data.pwm_max);
        MOT_SendData(motor2_data_double, CAN_COM_SSET, glob_mot_data.pwm_max);
    }
    else
    {
        pwm1 = motor1_data_double->speed; 
        pwm2 = motor2_data_double->speed;
   
        if (gt == 0)  //position1 > position2 -> motor1 runs faster
        {
            //adjust pwm
            pwm1 -= glob_mot_data.pwm_delta; //motor1 speed down
            pwm2 += glob_mot_data.pwm_delta; //motor2 speed up
       
            //check min pwm limit
            if (pwm1 < glob_mot_data.pwm_min)
                pwm1 = glob_mot_data.pwm_min;
            //check max pwm limit   
            if (pwm2 > glob_mot_data.pwm_max)
                pwm2 = glob_mot_data.pwm_max; 
        }
        else if (gt == 1)  //position2 > position1 -> motor2 runs faster
        {
            //adjust pwm
            pwm1 += glob_mot_data.pwm_delta;  //motor1 speed up
            pwm2 -= glob_mot_data.pwm_delta; //motor2 speed down
       
            //check max pwm limit
            if (pwm1 > glob_mot_data.pwm_max) 
                pwm1 = glob_mot_data.pwm_max;
            //check min pwm limit   
            if (pwm2 < glob_mot_data.pwm_min)
                pwm2 = glob_mot_data.pwm_min;
        }
    }
   
    //update speed
     MOT_SendData(motor1_data_double, CAN_COM_SSET, pwm1);
     MOT_SendData(motor2_data_double, CAN_COM_SSET, pwm2);
}

Теперь моторов 4 и алгоритмика намного усложнилась, Не соображу как отследить разницу в позициях нескольких моторов.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 13:25 
Старожил

Зарегистрирован: 10 окт 2014, 00:48
Сообщения: 4676
Vref = (V1+V2+V3+V4)/4;
Ошибка:
E1=V1-Vref;
E2=V2-Vref;
E3=V3-Vref;
E4=V4-Vref;
Задается и поддерживается Vref.
-----
Альтернативный вариант - один из моторов задается за ref и все остальные моторы синхронизируются по нему.
Vref=V1;
E1=0 (не подстраивается)
E2=V2-Vref; E3=V3-Vref; E4=V4-Vref;


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 13:49 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
u37 писал(а):
Vref = (V1+V2+V3+V4)/4;
Ошибка:
E1=V1-Vref;
E2=V2-Vref;
E3=V3-Vref;
E4=V4-Vref;
Задается и поддерживается Vref.
-----
Альтернативный вариант - один из моторов задается за ref и все остальные моторы синхронизируются по нему.
Vref=V1;
E1=0 (не подстраивается)
E2=V2-Vref; E3=V3-Vref; E4=V4-Vref;

спасибо. попробую.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 14:54 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
А что если так.
Создаю структуру содержащую сортированые позиции, указатели на моторы, и адейтированые значения скорости
Код:
typedef struct MOTOR_POS_DIFF_S
{
    MOTOR_DATA * mot;
    int32_t pos;
    uint32_t pwm;
    uint32_t retries;

}MOTOR_POS_DIFF;

MOTOR_POS_DIFF mot_pos_diff[4];

Сортирую позиции
Код:
void MOT_SortPositions(void)
{
    int i,j=0;
   
    for (i = 0; i < g_dbl_motors; i++)
    {
        if (j==0)
        {
            mot_pos_diff[j].pos =  motor_data_double[i]->position;
            mot_pos_diff[j].mot = motor_data_double[i];
        }
        else
        {
            if (motor_data_double[i]->position > mot_pos_diff[j-1].pos)
                mot_pos_diff[j].pos =  motor_data_double[i]->position;
            else
            {
                mot_pos_diff[j].pos = mot_pos_diff[j-1].pos;
                mot_pos_diff[j-1].pos = motor_data_double[i]->position;
            }
             mot_pos_diff[j].mot = motor_data_double[i];     
        }       
        j++;
    }
}

Принимаю решение относительно средней позиции
Код:
void MOT_ProcessPositions(void)
{
    int i;
    uint32_t mid_idx = g_dbl_motors/2;
   
    for (i = 0; i < g_dbl_motors; i++)
    {
        if (i != mid_idx)
        {
            if ( (abs(mot_pos_diff[i].pos-mot_pos_diff[mid_idx].pos)) > glob_mot_data.max_pos_diff)
            {
                if (mot_pos_diff[i].pos > mot_pos_diff[mid_idx].pos)
                {
                    mot_pos_diff[i].retries++;
                    mot_pos_diff[i].pwm -= glob_mot_data.pwm_delta;
                    if (mot_pos_diff[i].pwm < glob_mot_data.pwm_min)
                        mot_pos_diff[i].pwm = glob_mot_data.pwm_min;
                }
                else
                {
                    mot_pos_diff[i].pwm += glob_mot_data.pwm_delta;
                    if (mot_pos_diff[i].pwm > glob_mot_data.pwm_max)
                        mot_pos_diff[i].pwm = glob_mot_data.pwm_max;
                }
            }
            else
            {
                mot_pos_diff[i].retries = 0;
            }
        }
    } 
}


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 15:39 
Старожил

Зарегистрирован: 10 окт 2014, 00:48
Сообщения: 4676
Зачем эти сложности? Если вы собираетесь регулировать по принципу - если скорость больше, то PWM--, если скорость меньше, то PWM++, то вам никакая сортировка и, даже, величина отставания/ускорения не нужна -- только лишь > или <.
Лучше подумайте о другом - регулятор надо делать с PID.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 15:42 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
u37 писал(а):
Зачем эти сложности? Если вы собираетесь регулировать по принципу - если скорость больше, то PWM--, если скорость меньше, то PWM++, то вам никакая сортировка и, даже, величина отставания/ускорения не нужна -- только лишь > или <.
Лучше подумайте о другом - регулятор надо делать с PID.

так это и есть своего рода PID.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 15:44 
Старожил

Зарегистрирован: 10 окт 2014, 00:48
Сообщения: 4676
Нету D. )) Без нее хорошей реакции не будет. Вы будете вытягивать все больше и больше P и I, что выродится в самовозбуждение (колебательный процесс). Делайте полноценный PID, по всем 4 каналам.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 15:56 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
u37 писал(а):
Нету D. )) Без нее хорошей реакции не будет. Вы будете вытягивать все больше и больше P и I, что выродится в самовозбуждение (колебательный процесс). Делайте полноценный PID, по всем 4 каналам.

ага. делайте это хороший совет. :)
знал бы прикуп как сделать жил бы в Сочи. :)


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 16:15 
Старожил

Зарегистрирован: 10 окт 2014, 00:48
Сообщения: 4676
Ну здрасьте. Вы что, совсем ничего не читали по PID? Ээээ... может стоит почитать? Иначе, ваша модель будет постоянно рыскать, вместо движения по прямой.
Я бы сделал следующее:
Выяснил коэффициент пересчета PWM в скорость вращения мотора (это примерно константа) = К.
Завел пересчет изменения скорости в зависимости от ошибки. Ошибка Е, значит скорость надо понизить на Е, а это означает снижение PWM на E/K.
Это была P компонента. Но нужно добавить D. Она считается очень просто - у вас есть ошибка Е и предыдущая ошибка Е(-1). Дифференциальная ошибка будет Е - Е(-1) (т.е. скорость изменения ошибки). Просуммируем компоненты Р и D:
-- скорость надо понизить на Е*N1 + (E - E(-1))*N2, а это означает снижение PWM на (Е*N1 + (E - E(-1))*N2)/K.
При этом N1 и N2 = коэффициенты PD. Туда же можно вклеить и интегральную составляющую, если вам надо ставить абсолютно точное соответствие моторов.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 16:27 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
u37 писал(а):
Ну здрасьте. Вы что, совсем ничего не читали по PID? Ээээ... может стоит почитать? Иначе, ваша модель будет постоянно рыскать, вместо движения по прямой.
Я бы сделал следующее:
Выяснил коэффициент пересчета PWM в скорость вращения мотора (это примерно константа) = К.
Завел пересчет изменения скорости в зависимости от ошибки. Ошибка Е, значит скорость надо понизить на Е, а это означает снижение PWM на E/K.
Это была P компонента. Но нужно добавить D. Она считается очень просто - у вас есть ошибка Е и предыдущая ошибка Е(-1). Дифференциальная ошибка будет Е - Е(-1) (т.е. скорость изменения ошибки). Просуммируем компоненты Р и D:
-- скорость надо понизить на Е*N1 + (E - E(-1))*N2, а это означает снижение PWM на (Е*N1 + (E - E(-1))*N2)/K.
При этом N1 и N2 = коэффициенты PD. Туда же можно вклеить и интегральную составляющую, если вам надо ставить абсолютно точное соответствие моторов.

и как это все увязать на х моторов?


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 16:31 
Старожил

Зарегистрирован: 10 окт 2014, 00:48
Сообщения: 4676
Ошибка:
E1=V1-Vref;
E2=V2-Vref;
E3=V3-Vref;
E4=V4-Vref;
Задается и поддерживается Vref.
-----
Альтернативный вариант - один из моторов задается за ref и все остальные моторы синхронизируются по нему.
Vref=V1;
E1=0 (не подстраивается)
E2=V2-Vref; E3=V3-Vref; E4=V4-Vref;


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 06 фев 2018, 19:52 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
А что скажут алгоритмисты на такое решение?
Код:
uint32_t MOT_GetMedianPosition(void)
{
     int i;
     uint32_t position=0;
   
     for (i = 0; i < g_linked_motors; i++)
     {
         position += linked_motor_data[i]->position;
     }
     position /= g_linked_motors;
   
     return position;
}

uint32_t ProcessMedianPosition(void)
{
    int i;
    uint32_t pos_diff=0;
   
    uint32_t median_position = MOT_GetMedianPosition();
   
    for (i = 0; i < g_linked_motors; i++)
    {
        if ( abs(linked_motor_data[i]->position - median_position) > glob_mot_data.max_pos_diff)
        {
            pos_diff = 1;
           
            if (linked_motor_data[i]->position > median_position)  //motor runs faster
            {
                //adjust speed -> speed down
                linked_motor_data[i]->pwm -= glob_mot_data.pwm_delta;
                //check limit
                if (linked_motor_data[i]->pwm < glob_mot_data.pwm_min)
                    linked_motor_data[i]->pwm = glob_mot_data.pwm_min;
            }
            else   //motor runs slower
            {
                //adjust speed -> speed up
                linked_motor_data[i]->pwm += glob_mot_data.pwm_delta;
                //check limit
                if (linked_motor_data[i]->pwm > glob_mot_data.pwm_max)
                    linked_motor_data[i]->pwm = glob_mot_data.pwm_max;
            }
        }
    }
    return pos_diff;
}

void MOT_PositionLoopMulti(void)
{
    int i;
    if (ProcessMedianPosition())
    {
        //retry
        glob_mot_data.pos_diff_count++;
        //maximum retries - unable to adjust speed
        if (glob_mot_data.pos_diff_count > glob_mot_data.max_pos_diff_count)
        {
            glob_mot_data.pos_diff_count = 0;
             //send stop command
            for (i = 0; i < g_linked_motors; i++)
                MOT_SendData(linked_motor_data[i], CAN_COM_STOP, 0);
        }
        else
        {
            //update speed
            for (i = 0; i < g_linked_motors; i++)
                MOT_SendData(linked_motor_data[i], CAN_COM_SSET, linked_motor_data[i]->pwm);
        }
    }
    else
    {
       glob_mot_data.pos_diff_count = 0;
       //set maximum speed
       for (i = 0; i < g_linked_motors; i++)
           MOT_SendData(linked_motor_data[i], CAN_COM_SSET, glob_mot_data.pwm_max);
    } 


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 02 мар 2018, 16:25 
Только пришел

Зарегистрирован: 16 авг 2017, 15:09
Сообщения: 8
http://plc4good.org.ua/view_post.php?id=51 - алгоритм PID от siemens'a с хорошими доками на русском.


Вернуться к началу
 Профиль  
 
 Заголовок сообщения: Re: Отследить разницу в позициях моторов.
СообщениеДобавлено: 12 мар 2018, 18:27 
Старожил

Зарегистрирован: 17 фев 2016, 17:31
Сообщения: 202
спасибо.


Вернуться к началу
 Профиль  
 
Показать сообщения за:  Поле сортировки  
Начать новую тему Ответить на тему  [ Сообщений: 14 ] 

Часовой пояс: UTC + 5 часов


Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей


Вы не можете начинать темы
Вы не можете отвечать на сообщения
Вы не можете редактировать свои сообщения
Вы не можете удалять свои сообщения
Вы не можете добавлять вложения

Найти:
Перейти:  

Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
Русская поддержка phpBB