Гость
Форумы / Программирование [игнор отключен] [закрыт для гостей] / Алгоритм закрытия позиционной разности моторов / 15 сообщений из 15, страница 1 из 1
06.06.2019, 12:09
    #39823358
jenya7
Гость
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
Есть два мотора. Я отслеживаю их позиции. Если при движении вперед позиция первого мотора больше – он двигается быстрей – я его притормаживаю а второй ускоряю. Ну и так далее.
Исходник
Код: plaintext
1.
2.
3.
4.
5.
6.
7.
8.
9.
10.
11.
12.
13.
14.
15.
16.
17.
18.
19.
20.
21.
22.
23.
24.
25.
26.
27.
28.
29.
30.
31.
32.
33.
34.
35.
36.
37.
38.
39.
40.
41.
42.
43.
44.
45.
46.
47.
48.
49.
50.
51.
52.
53.
54.
55.
56.
57.
58.
59.
60.
61.
62.
63.
64.
65.
66.
67.
68.
69.
70.
71.
72.
73.
74.
75.
76.
77.
78.
79.
80.
81.
82.
83.
84.
85.
86.
87.
88.
89.
90.
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;
}


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

Модератор: Просьба большие простыни убирать под спойлер. Поправил.
...
Рейтинг: 0 / 0
06.06.2019, 13:02
    #39823401
Dima T
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
Погугли алгоритмы работы автомобильных антиблокировочных систем. Там похожая задача решается.
...
Рейтинг: 0 / 0
06.06.2019, 13:06
    #39823404
jenya7
Гость
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
Dima TПогугли алгоритмы работы автомобильных антиблокировочных систем. Там похожая задача решается.
да. там система пытается скомпенсировать разность вращения колес.
...
Рейтинг: 0 / 0
06.06.2019, 13:54
    #39823443
mayton
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
Чето у тебя так много букв? Если заменить motor_rt_params[mot_idx1] и pos_dif_control[group] на переменные то
станет как-то компактнее.
...
Рейтинг: 0 / 0
06.06.2019, 14:01
    #39823448
982183
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
Явно не хватает параметра инертности разгона и торможения.
...
Рейтинг: 0 / 0
06.06.2019, 14:17
    #39823467
jenya7
Гость
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
982183Явно не хватает параметра инертности разгона и торможения.
это осталось вне алгоритма.

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


нужно бы включить какой нибудь PID по хорошему - регулировать дельту скорости в зависимости от дельты позиции а не тупо
Код: c#
1.
2.
 motor_rt_params[mot_idx1].speed -= pos_dif_control[group].pwm_delta;
 motor_rt_params[mot_idx2].speed += pos_dif_control[group].pwm_delta;
...
Рейтинг: 0 / 0
06.06.2019, 14:33
    #39823491
Akina
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
jenya7Есть какая то алгоритмика в этих вопросах?Есть такая штука как теория управления. Пропорциональные регуляторы, дифференциальные, интегральные, смешанных типов... почитайте, очень способствует. Потом запрограммировать это не так уж и сложно.
...
Рейтинг: 0 / 0
07.06.2019, 12:02
    #39823948
exp98
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
jenya7нужно бы включить какой нибудь PID по хорошему - регулировать дельту скорости в зависимости от дельты позиции а не тупо
Код: c#
1.
2.
 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.
Покопайся и порисуй траектории на плоскости - помогает думать.
...
Рейтинг: 0 / 0
07.06.2019, 12:04
    #39823950
exp98
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
jenya7, надеюсь понятно, что dx/dt скорость.
...
Рейтинг: 0 / 0
07.06.2019, 12:57
    #39823995
jenya7
Гость
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
exp98jenya7, надеюсь понятно, что dx/dt скорость.
Я должен пояснить, что регулировку скорости в моторе я делаю приложением PWM. То есть speed это PWM.
Пока я немного улучшил алгоритм, включив пропорциональную составляющую
Код: plaintext
1.
2.
3.
4.
pos_dif = abs(position1 - position2);

speed1 -= pos_dif * kp;
speed2 += pos_dif * kp;
...
Рейтинг: 0 / 0
07.06.2019, 13:55
    #39824030
exp98
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
jenya7, это не важно, я проглядел (давно не брал шашек в руки)
Твой вариант типа однородный дифур 2-й степени (слева будет ускорение)
d2Z/dt2= -A*dZ, где Z= y-x так? всё равно решение аналитическое через корни квадратного ур-ния, просто, но надо вспоминать.
И, вроде, всё равно Z будет подобие комплексной экспоненты.
Получается тогда, что ускорение "дёргаешь" пропорционально разнице коорд-т, а скорость экспоненциально. Если качественно я не соврал, то вот и алгоритмика получилась. Тебе решать, подходят ли такие рывки.
Но вроде, поскольку дёргаешь ускорение, не контролируя будущую скорость, то и возможна игра моторов "на перегонки".
ИМХО хорошо сначала определиться с концепцией "догонялок".

Кажется немного похожим, если в пробке газовать пропорционально расстояниюдо передней авто. Типа: если 10 метров, то педаль в пол, а текущая скорость не учтена. Как-то так ...
...
Рейтинг: 0 / 0
07.06.2019, 23:48
    #39824348
kealon(Ruslan)
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
jenya7,

21739904
...
Рейтинг: 0 / 0
17.08.2019, 13:22
    #39850668
АСУ ТПшник
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
Ох уже программисты.... Так вот моторы и убивают. Чего непонятного то?
...
Рейтинг: 0 / 0
17.08.2019, 13:27
    #39850670
АСУ ТПшник
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
Жмешь педалю, а по сопротивлению линий пропорционально раскидывается подаваемое давление (управление твоими моторами - больше газа на мотор который отстает).
ПИД регулятор, который тут посоветовали - ну очень интересно было бы посмотреть, как его в программе сделают а потом настраивать на нужную чувствительность будут.
...
Рейтинг: 0 / 0
17.08.2019, 13:28
    #39850671
АСУ ТПшник
Участник
Скрыть профиль Поместить в игнор-лист Сообщения автора в теме
Алгоритм закрытия позиционной разности моторов
А это как вы моторы могучие убиваете своими алгоритмами :D

...
Рейтинг: 0 / 0
Форумы / Программирование [игнор отключен] [закрыт для гостей] / Алгоритм закрытия позиционной разности моторов / 15 сообщений из 15, страница 1 из 1
Целевая тема:
Создать новую тему:
Автор:
Найденые пользователи ...
Разблокировать пользователей ...
Читали форум (0):
Пользователи онлайн (0):
x
x
Закрыть


Просмотр
0 / 0
Close
Debug Console [Select Text]