Добро пожаловать в форум, Guest  >>   Войти | Регистрация | Поиск | Правила | В избранное | Подписаться
Все форумы / Программирование Новый топик    Ответить
 Алгоритм закрытия позиционной разности моторов  [new]
jenya7
Member

Откуда:
Сообщений: 1159
Есть два мотора. Я отслеживаю их позиции. Если при движении вперед позиция первого мотора больше – он двигается быстрей – я его притормаживаю а второй ускоряю. Ну и так далее.
+ Исходник
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 group)
{
    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;
    uint32_t skip = 0;
    
    if (pos_dif >= pos_dif_control[group].pos_dif)
    {
        sys_status |= MOT_POS_DIFF;
        MASTER_StopAll();
        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; 
            }
        }
    }
     
     return pos_dif;
}

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

Модератор: Просьба большие простыни убирать под спойлер. Поправил.
6 июн 19, 12:09    [21903348]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
Dima T
Member

Откуда:
Сообщений: 13915
Погугли алгоритмы работы автомобильных антиблокировочных систем. Там похожая задача решается.
6 июн 19, 13:02    [21903412]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
jenya7
Member

Откуда:
Сообщений: 1159
Dima T
Погугли алгоритмы работы автомобильных антиблокировочных систем. Там похожая задача решается.

да. там система пытается скомпенсировать разность вращения колес.
6 июн 19, 13:06    [21903415]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
mayton
Member

Откуда: loopback
Сообщений: 41808
Чето у тебя так много букв? Если заменить motor_rt_params[mot_idx1] и pos_dif_control[group] на переменные то
станет как-то компактнее.
6 июн 19, 13:54    [21903490]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
982183
Member

Откуда: VL
Сообщений: 3104
Явно не хватает параметра инертности разгона и торможения.
6 июн 19, 14:01    [21903500]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
jenya7
Member

Откуда:
Сообщений: 1159
982183
Явно не хватает параметра инертности разгона и торможения.

это осталось вне алгоритма.

то есть в этот момент
if (motor_rt_params[mot_idx1].position &#65279;> motor_rt_params&#65279;[mot_idx2].position&#65279;)

нужно бы включить какой нибудь 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;
6 июн 19, 14:17    [21903523]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
Akina
Member

Откуда: Зеленоград, Москва, Россия
Сообщений: 19271
jenya7
Есть какая то алгоритмика в этих вопросах?
Есть такая штука как теория управления. Пропорциональные регуляторы, дифференциальные, интегральные, смешанных типов... почитайте, очень способствует. Потом запрограммировать это не так уж и сложно.
6 июн 19, 14:33    [21903554]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
exp98
Member

Откуда:
Сообщений: 1674
jenya7
нужно бы включить какой нибудь 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;
Для примера постейшая система диф.уравнений с постоянными коэфф-ми типа
dy/dt= -a*x(t)+ c1
dx/dt= -b*y(t) +c2
+ физические ограничения.
Параметры такие, чтобы решения были устойчивыми к малым колебаниям в параметрах и в начальных условиях y0 x0.
Покопайся и порисуй траектории на плоскости - помогает думать.
7 июн 19, 12:02    [21904376]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
exp98
Member

Откуда:
Сообщений: 1674
jenya7, надеюсь понятно, что dx/dt скорость.
7 июн 19, 12:04    [21904378]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
jenya7
Member

Откуда:
Сообщений: 1159
exp98
jenya7, надеюсь понятно, что dx/dt скорость.

Я должен пояснить, что регулировку скорости в моторе я делаю приложением PWM. То есть speed это PWM.
Пока я немного улучшил алгоритм, включив пропорциональную составляющую
pos_dif = abs(position1 - position2);

speed1 -= pos_dif * kp;
speed2 += pos_dif * kp;
7 июн 19, 12:57    [21904438]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
exp98
Member

Откуда:
Сообщений: 1674
jenya7, это не важно, я проглядел (давно не брал шашек в руки)
Твой вариант типа однородный дифур 2-й степени (слева будет ускорение)
d2Z/dt2= -A*dZ, где Z= y-x так? всё равно решение аналитическое через корни квадратного ур-ния, просто, но надо вспоминать.
И, вроде, всё равно Z будет подобие комплексной экспоненты.
Получается тогда, что ускорение "дёргаешь" пропорционально разнице коорд-т, а скорость экспоненциально. Если качественно я не соврал, то вот и алгоритмика получилась. Тебе решать, подходят ли такие рывки.
Но вроде, поскольку дёргаешь ускорение, не контролируя будущую скорость, то и возможна игра моторов "на перегонки".
ИМХО хорошо сначала определиться с концепцией "догонялок".

Кажется немного похожим, если в пробке газовать пропорционально расстояниюдо передней авто. Типа: если 10 метров, то педаль в пол, а текущая скорость не учтена. Как-то так ...
7 июн 19, 13:55    [21904503]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
kealon(Ruslan)
Member

Откуда: Нижневартовск
Сообщений: 5107
jenya7,

21739904
7 июн 19, 23:48    [21904993]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
АСУ ТПшник
Member

Откуда:
Сообщений: 782
Ох уже программисты.... Так вот моторы и убивают. Чего непонятного то?
Картинка с другого сайта.
17 авг 19, 13:22    [21951701]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
АСУ ТПшник
Member

Откуда:
Сообщений: 782
Жмешь педалю, а по сопротивлению линий пропорционально раскидывается подаваемое давление (управление твоими моторами - больше газа на мотор который отстает).
ПИД регулятор, который тут посоветовали - ну очень интересно было бы посмотреть, как его в программе сделают а потом настраивать на нужную чувствительность будут.
17 авг 19, 13:27    [21951704]     Ответить | Цитировать Сообщить модератору
 Re: Алгоритм закрытия позиционной разности моторов  [new]
АСУ ТПшник
Member

Откуда:
Сообщений: 782
А это как вы моторы могучие убиваете своими алгоритмами :D

+
Картинка с другого сайта.
17 авг 19, 13:28    [21951706]     Ответить | Цитировать Сообщить модератору
Все форумы / Программирование Ответить