Jump to content

    

Алгоритм закрытия позиционной разности моторов.

Есть два мотора. Я отслеживаю их позиции. Если при движении вперед позиция первого мотора больше – он двигается быстрей – я его притормаживаю а второй ускоряю. Ну и так далее.

Возник вопрос – что делать если разница в позициях достигла предельно допустимой?

То есть я конечно остановлю оба мотора. Но потом, если пользователь продолжает жать кнопку вперед – я подумал тот мотор который отстает все таки запустить – он сравняется с позицией со вторым мотором – я запущу второй мотор и продолжу отслеживать позиции.

Вобщем я начал кодить, добавлять эту часть – такая каша получается, мама не горюй. Есть какая то алгоритмика в этих вопросах?

Share this post


Link to post
Share on other sites
8 минут назад, jenya7 сказал:

Возник вопрос – что делать если разница в позициях достигла предельно допустимой?

То есть я конечно остановлю оба мотора.

Вобщем я начал кодить, добавлять эту часть – такая каша получается, мама не горюй. Есть какая то алгоритмика в этих вопросах?

Движение механических систем описывается уравнениями кинематики и динамики (Закон Ньютона). Плюс к тому сила пропорциональна току в моторе. Закон Ампера. А ток зависит еще от скорости вращения, что не должно забываться.

Share this post


Link to post
Share on other sites

Произвольно назначаете один мотор Мастером, а второй - Слэйвом.

Управляете Вы только Мастером.

Слэйв управляется программой без Вашего участия на основе информации о рассогласовании с мастером.

Это алгоритм.

Share this post


Link to post
Share on other sites
1 hour ago, Gorby said:

Произвольно назначаете один мотор Мастером, а второй - Слэйвом.

Управляете Вы только Мастером.

Слэйв управляется программой без Вашего участия на основе информации о рассогласовании с мастером.

Это алгоритм.

У меня есть отдельный мастер. он получает данные от моторов обрабатывает их и посылает команды моторам (слейвам). моторы тупо исполняют команды.

поэтому речь идет именно об алгоритме мастера.

Share this post


Link to post
Share on other sites

Гм. То у Вас " Есть два мотора. Я отслеживаю их позиции.  Ну и так далее. "

а оказывается у Вас  " У меня есть отдельный мастер. он получает данные от моторов обрабатывает их и посылает команды моторам (слейвам) "

Не видите сами разницу? У Вас проблемы с постановкой задачи. И алгоритмизацией. Причинно-следственная связь, знаете ли.

Обобщим Задачу:  Надо обеспечить наиболее синхронное движение N моторов.

В общем случае решений может быть много.

Например использовать моторы с гарантированным положением (шаговые, с энкодерами и тд., без обратной связи);

А в самом общем случае можно произвольно назначить один из моторов Главным Моторищем (Мастером), а все остальные становятся Ведомыми Моторишками.

Задача упрощается: Ведомые обязаны просто следовать за мастером (ПИД и много чего всякого).

Share this post


Link to post
Share on other sites
40 minutes ago, Gorby said:

Задача упрощается: Ведомые обязаны просто следовать за мастером (ПИД и много чего всякого).

Эт если рассуждать наивно, не зная как на самом деле выглядят такие алгоритмы. 
ПИД  меньше всего здесь участвует.  
Но эвристик  будет десятки. А вариантов их сочетаний сотни если не тысячи. 
Эвристики типа как часто тормозить, какие гистерезисы скорости задать на каких фазах движения, какой максимум расхождения позволять, как стартовать при разных расхождениях и т.д. и т.п. 
Эт работа качественно может быть сделана только в  стэйт-диаграммах. Stateflow в руки! 
 
 

Share this post


Link to post
Share on other sites
1 hour ago, AlexandrY said:

Эт если рассуждать наивно, не зная как на самом деле выглядят такие алгоритмы. 
ПИД  меньше всего здесь участвует.  
Но эвристик  будет десятки. А вариантов их сочетаний сотни если не тысячи. 
Эвристики типа как часто тормозить, какие гистерезисы скорости задать на каких фазах движения, какой максимум расхождения позволять, как стартовать при разных расхождениях и т.д. и т.п. 
Эт работа качественно может быть сделана только в  стэйт-диаграммах. Stateflow в руки! 
 
 

ну а если так

это до наступления критического состояния

Spoiler

uint32_t GetPosDiff(uint32_t group)
{
    uint32_t pos_dif;
    
    uint32_t mot_idx1 = pos_dif_control[group].mot_idx1;
    uint32_t mot_idx2 = pos_dif_control[group].mot_idx2;
    
    if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position)
        pos_dif = motor_rt_params[mot_idx1].position - motor_rt_params[mot_idx2].position;
    else
        pos_dif = motor_rt_params[mot_idx2].position - motor_rt_params[mot_idx1].position;
    
    return pos_dif;
}


uint32_t PosDiffAdjust (uint32_t mot_idx1, uint32_t mot_idx2, uint32_t pos_dif, uint32_t group)
{
    uint32_t skip = 0;
    
    if (pos_dif >= pos_dif_control[group].pos_dif)
    {
        sys_status |= MOT_POS_DIFF;
        MASTER_StopAll();
        critical = 1;
        return pos_dif;
    }
    
    if (pos_dif < pos_dif_control[group].pos_delta)
    {
        skip = 1;
        motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_max;
        motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_max;
    }
    
    if (skip == 0)
    {
        if (motor_rt_params[mot_idx1].direction == MOT_DIR_FWD)
        {
            if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position) //motor1 runs faster
            {
                motor_rt_params[mot_idx1].speed -= pos_dif_control[group].pwm_delta;
                motor_rt_params[mot_idx2].speed += pos_dif_control[group].pwm_delta;
                
                if (motor_rt_params[mot_idx1].speed < pos_dif_control[group].pwm_min)
                    motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_min;
                if (motor_rt_params[mot_idx2].speed > pos_dif_control[group].pwm_max)
                    motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_max; 
            }
            else if (motor_rt_params[mot_idx2].position > motor_rt_params[mot_idx1].position) //motor2 runs faster
            {
                motor_rt_params[mot_idx1].speed += pos_dif_control[group].pwm_delta;
                motor_rt_params[mot_idx2].speed -= pos_dif_control[group].pwm_delta;
                
                if (motor_rt_params[mot_idx1].speed > pos_dif_control[group].pwm_max)
                    motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_max;
                if (motor_rt_params[mot_idx2].speed < pos_dif_control[group].pwm_min)
                    motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_min;
            }
                
        }
        if (motor_rt_params[mot_idx1].direction == MOT_DIR_REV)
        {
            if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position) //motor1 runs slower
            {
                motor_rt_params[mot_idx1].speed += pos_dif_control[group].pwm_delta;
                motor_rt_params[mot_idx2].speed -= pos_dif_control[group].pwm_delta;
                
                if (motor_rt_params[mot_idx1].speed > pos_dif_control[group].pwm_max)
                    motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_max;
                if (motor_rt_params[mot_idx2].speed < pos_dif_control[group].pwm_min)
                    motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_min;
            }
            else if (motor_rt_params[mot_idx2].position > motor_rt_params[mot_idx1].position) //motor2 runs slower
            {
                motor_rt_params[mot_idx1].speed -= pos_dif_control[group].pwm_delta;
                motor_rt_params[mot_idx2].speed += pos_dif_control[group].pwm_delta;
                
                if (motor_rt_params[mot_idx1].speed < pos_dif_control[group].pwm_min)
                    motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_min;
                if (motor_rt_params[mot_idx2].speed > pos_dif_control[group].pwm_max)
                    motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_max; 
            }
        }
    }
    
     master_data[0] = motor_rt_params[mot_idx1].speed;
     master_data[1] = motor_rt_params[mot_idx1].speed >> 8;
     CAN_TX(motor_rt_params[mot_idx1].mot_id, MASTER_COM_VSET, master_data, 2);
     
     master_data[0] = motor_rt_params[mot_idx2].speed;
     master_data[1] = motor_rt_params[mot_idx2].speed >> 8;
     CAN_TX(motor_rt_params[mot_idx2].mot_id, MASTER_COM_VSET, master_data, 2);
     
     return pos_dif;
}

 

а это после наступления критического состояния

Spoiler

uint32_t CriticalPosDiffAdjust (uint32_t mot_idx1, uint32_t mot_idx2, uint32_t pos_dif, uint32_t group)
{   
    //we reached safe position
    if (pos_dif <= pos_dif_control[group].pos_delta)
    {
        critical = 0;
        
        if (motor_rt_params[mot_idx1].direction == MOT_DIR_STOP)
        {
           motor_rt_params[mot_idx1].speed = pos_dif_control[group].pwm_min;
           master_data[0] = motor_rt_params[mot_idx1].speed;
           master_data[1] = motor_rt_params[mot_idx1].speed >> 8;
           CAN_TX(motor_rt_params[mot_idx1].mot_id, MASTER_COM_VSET, master_data, 2);
        }
        
        if (motor_rt_params[mot_idx2].direction == MOT_DIR_STOP)
        {
           motor_rt_params[mot_idx2].speed = pos_dif_control[group].pwm_min;
           master_data[0] = motor_rt_params[mot_idx2].speed;
           master_data[1] = motor_rt_params[mot_idx2].speed >> 8;
           CAN_TX(motor_rt_params[mot_idx2].mot_id, MASTER_COM_VSET, master_data, 2);
        }
    }
        
    return 1;    
}

 

а общая регулировка

Spoiler

void PosDiffLoop()
{
    uint32_t group = 0;
    
    uint32_t pos_dif = GetPosDiff(group);
    uint32_t mot_idx1 = pos_dif_control[group].mot_idx1;
    uint32_t mot_idx2 = pos_dif_control[group].mot_idx2;
    
    if (pos_dif >= pos_dif_control[group].pos_dif)
    {
        if (critical)
            CriticalPosDiffAdjust (mot_idx1, mot_idx2, pos_dif, group);
        else
           PosDiffAdjust (mot_idx1, mot_idx2, pos_dif, group);
            
    }
    else
        PosDiffAdjust (mot_idx1, mot_idx2, pos_dif, group);  
} 

 

вроде логично получается?

 

или как вариант - в критической ситуации останавливать только убежавший вперед мотор, отстающий мотор пусть продолжает двигаться. но это намного усложняет алгоритм.

Edited by jenya7

Share this post


Link to post
Share on other sites
8 minutes ago, AlexandrY said:

Эт если рассуждать наивно, не зная как на самом деле выглядят такие алгоритмы. 

Эт работа качественно может быть сделана только в  стэйт-диаграммах. Stateflow в руки! 
 
 

Наивно - эт усложнять без необходимости, особенно в отсутствие исходных данных. Бритва Оккама остра, как никогда.

Ну эт да. В будущем не будет ни кино, ни театра, одно сплошное телевидение ой, одни стэйт-диаграммы...

Я может тоже недавно для себя свечи от геморроя открыл. Но не сую рекомендую их всем подряд  ;)

Вон в Печатных Платах один деятель про кортярд узнал. Так уже пол-года только и слышно...

Share this post


Link to post
Share on other sites
if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position) //motor1 runs faster

Это как так? Сравниваем положения , а выводы делаем о скорости?  То есть черепаха, стоящая впереди зайца быстрее его?

А если выбег уже случился, мотор ОСТАНОВИЛСЯ в ожидании , и что тогда? По-прежнему его скорость больше?

Четко задавайте условия задачи.

Share this post


Link to post
Share on other sites
18 минут назад, jenya7 сказал:

ну аесли так

это до наступления критического состояния

  Показать контент

 

а это после наступления критического состояния

  Показать контент

 

а общая регулировка

  Показать контент

 

вроде логично получается?

Зачем Вы коды постите в данном случае... Не думаю, что кто-то захочет это читать... Привели бы алгоритм... Их может быть бесконечное множество...

Вот, вы едете на машине, отстали от лидера, сколько способов его догнать и выравнять скорость?

Share this post


Link to post
Share on other sites
1 hour ago, Tanya said:

Зачем Вы коды постите в данном случае... Не думаю, что кто-то захочет это читать... Привели бы алгоритм... Их может быть бесконечное множество...

Вот, вы едете на машине, отстали от лидера, сколько способов его догнать и выровнять скорость?

вот я и привел алгоритм - всего две переменных - позиция против скорости.

скажем так, это идеальный вопрос для разработчика системы ABS - там система пытается скомпенсировать разность вращения колес. мой случай.

Share this post


Link to post
Share on other sites
2 hours ago, jenya7 said:

Есть два мотора. Я отслеживаю их позиции. Если при движении вперед позиция первого мотора больше – он двигается быстрей – я его притормаживаю а второй ускоряю. Ну и так далее.

Возник вопрос – что делать если разница в позициях достигла предельно допустимой? 

Подтормозить до максимально допустимого торможения? Буть то хоть ноль, хоть отрицательная величина. Никаких дополнительных действий от алгоритма не требуется.

Share this post


Link to post
Share on other sites
1 hour ago, rkit said:

Подтормозить до максимально допустимого торможения? Буть то хоть ноль, хоть отрицательная величина. Никаких дополнительных действий от алгоритма не требуется.

хм. все гениальное просто? действительно, вариант.

хотя, когда я опускаю параметер pwm_min очень низко у меня система начинает плохо работать. сильные девиации появляются.

я даже для их сглаживания ввел бланкинг

pos_skip_times++;  
if (pos_skip_times >= pos_blank)
{
    pos_skip_times = 0;
                        
    PosDiffAdjust (mot_idx1, mot_idx2, pos_dif, group);
}

но отказался от этой затеи, работает хреново.

Edited by jenya7

Share this post


Link to post
Share on other sites
8 minutes ago, jenya7 said:

хотя, когда я опускаю параметер pwm_min очень низко у меня система начинает плохо работать. сильные девиации появляются.

 

Потому что нужно контролировать RPM при управлении посредством PWM.  Чисто программно такие задачи не решаются. Мне например в похожем случае пришлось применить аппаратный стабилизатор скорости. Так вот на низких скоростях двигатель превращался в шаговик. Нельзя просто снижать PWM - мощность тоже уменьшается.

Двигатели небось и нагрузку какую-то тянут? И что, она всегда одинаковая?

Share this post


Link to post
Share on other sites

то есть в этот момент

if (motor_rt_params[mot_idx1].position > motor_rt_params[mot_idx2].position)

нужно бы включить какой нибудь PID по хорошему - регулировать дельту скорости в зависимости от дельты позиции а не тупо

 motor_rt_params[mot_idx1].speed -= pos_dif_control[group].pwm_delta;
 motor_rt_params[mot_idx2].speed += pos_dif_control[group].pwm_delta;

 

1 hour ago, Gorby said:

Потому что нужно контролировать RPM при управлении посредством PWM.  Чисто программно такие задачи не решаются. Мне например в похожем случае пришлось применить аппаратный стабилизатор скорости. Так вот на низких скоростях двигатель превращался в шаговик. Нельзя просто снижать PWM - мощность тоже уменьшается.

Двигатели небось и нагрузку какую-то тянут? И что, она всегда одинаковая?

снизиться  PWM - снизиться мощность - снизиться скорость - снизиться разность в позициях.

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

Edited by jenya7

Share this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now