Алгоритм плавного разгона и торможения шгового двигателя

[isx]

Приветствую всех.
Сижу и ломаю голову над одним вопросом - как правильно плавно разогнать и главное затормозить шаговик.
В арсенале есть следующие переменные:
Текущая скорость;
Максимальная допустимая скорость;
Минимальнаяидопустимая скорость;
Значение разгона (шагов в секунду);
Значение торможения (шагов в секунду);
Оставшееая количество шагов до полной остановки.

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

[BOB51]

Этот вопрос рассматривался в документации старых советских принтеров (9-игольчатых)...
Жаль той "фолианты" у меня не сохранилось...

[isx]

А по памяти ничего не подскажете? Блин, вроде задача простая, но чет никак не соображу...

[AndyBig]

Надо, наверное, копать в сторону формулы определения расстояния при равноускоренном движении:

где:
S - пройденное расстояние, м
v1 - начальная скорость при ускорении или конечная при замедлении, м/с
v2 - конечная скорость при ускорении или начальная при замедлении, м/с
a - ускорение, м/с^2

Только вместо метров и секунд подставить шаги двигателя и миллисекунды. То есть:
S - количество сделанных шагов
v1 - начальная скорость при ускорении или конечная при замедлении, шагов/миллисекунду
v2 - конечная скорость при ускорении или начальная при замедлении, шагов/миллисекунду
a - ускорение, шагов/мс^2

Тогда получается, что если, к примеру, нужно снизить скорость от 50 до 2 шага в миллисекунду с ускорением (значением торможения) 2 шага в миллисекунду, тогда находим количество шагов, которые двигатель сделает в процессе торможения:

Значит за 624 шага до того как скорость должна стать равной 1 начинаем снижать скорость по 2 шага в миллисекунду.

Я не уверен во всем этом, не пробовал, но мне кажется, что должно работать

UPD: Не, фигня это. Не получается почему-то так.

Добавлено after 1 hour 23 minutes 48 seconds:
Re: Алгоритм плавного разгона и торможения шгового двигателя
Тогда смотреть в сторону суммы членов арифметической прогрессии

S - количество сделанных шагов
v1 - начальная скорость при ускорении или конечная при замедлении, шагов/миллисекунду
v2 - конечная скорость при ускорении или начальная при замедлении, шагов/миллисекунду
a - ускорение, шагов/мс^2
t - это количество шагов уменьшения скорости до достижения нужной

Вот так вроде все сходится с подсчетом в лоб на калькуляторе

[Mishany]

делал на таймере, там все просто было вроде
видюху нашел даже управа с переменника

[AndyBig]

делал на таймере, там все просто было вроде

Это если не нужно вращать на точно заданное количество шагов.

[Mishany]

нет, там все точно было, реализован на 2-х таймерах STM32, смотрел исходник но что то с коментами они какие то левые
Принцип прост передаешь позицию мотор выходит на нее с ускорением и замедлением, причем можно на лету менять позицию
не знаю от куда там скорость в км/час, но это прерывание отвечает за ускорение/замедление

Спойлер

Код:

void TIM2_IRQHandler(void)
{
  /* USER CODE BEGIN TIM2_IRQn 0 */
//   HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_10);
//   HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_11);
   /*начало детектирования скорости ниже 2 км/час(на случай переполнения таймера захвата)*/
   if (flag_test != 0)
   {
      counter_tim2++;
      //если нащелкали более 1 сек
      if(counter_tim2 > 28)
         {
            flag_tim2 = 1;
            counter_tim2 = 0;
            //SPEED_value = 0;
         }
   }
   
   /*конец детектирования скорости ниже 2 км/час*/
   if(raznica < 40)
{
      if (timerse < 12)
      {
         timerse++;   
         //flag_inc = 10;
         htim4.Init.Period = timerse;
         HAL_TIM_Base_Init(&htim4);
      }
}
if(raznica > 40)
{   
      if (timerse > 1)
      {
         timerse--;
         //flag_inc = 0;
         htim4.Init.Period = timerse;
         HAL_TIM_Base_Init(&htim4);
      }   
}

  /* USER CODE END TIM2_IRQn 0 */
  HAL_TIM_IRQHandler(&htim2);
  /* USER CODE BEGIN TIM2_IRQn 1 */

  /* USER CODE END TIM2_IRQn 1 */
}

/**
* @brief This function handles TIM3 global interrupt.
*/
void TIM3_IRQHandler(void)
{
  /* USER CODE BEGIN TIM3_IRQn 0 */

  /* USER CODE END TIM3_IRQn 0 */
  HAL_TIM_IRQHandler(&htim3);
  /* USER CODE BEGIN TIM3_IRQn 1 */

  /* USER CODE END TIM3_IRQn 1 */
}

/**
* @brief This function handles TIM4 global interrupt.
*/
void TIM4_IRQHandler(void)
{
  /* USER CODE BEGIN TIM4_IRQn 0 */
   /*заходим после каждого полного шага, когда на ноге STEP лог. "0"*/
   if(flag_polarity == 0)
   {
      if(SPEED_value > Position)
      {
         //Вращение по часовой DIR
         HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_RESET);
         raznica = SPEED_value - Position;
         Position++;   //запоминаем новое положение стрелки
         full_position_up = 1;
      }
      else
      {
      full_position_up = 0;//флаг окончания выхода на позицию по часовой
      }
      if(SPEED_value < Position)
      {
         //Вращение против часовой DIR
         HAL_GPIO_WritePin(GPIOA, GPIO_PIN_7, GPIO_PIN_SET);
         raznica = Position - SPEED_value;
         Position--;   //запоминаем новое положение стрелки
         full_position_dn = 1;
      }
      else
      {
      full_position_dn = 0;//флаг окончания выхода на позицию против часовой
      }
   }
   //шагаем пока не выйдем на заданную позицию (каждый заход меняем состояние на ноге STEP):
   if((full_position_dn != 0)||(full_position_up != 0))
   {
      if(flag_polarity == 0)
      {
         HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_SET);
         flag_polarity = 1;
      }
      else
      {
         HAL_GPIO_WritePin(GPIOA, GPIO_PIN_6, GPIO_PIN_RESET);
         flag_polarity = 0;
      }
   }
   else
   {
      if(flag_test == 1)
      {
      htim4.Init.Period = 12;
      HAL_TIM_Base_Init(&htim4);
      }
   }

  /* USER CODE END TIM4_IRQn 0 */
  HAL_TIM_IRQHandler(&htim4);
  /* USER CODE BEGIN TIM4_IRQn 1 */

  /* USER CODE END TIM4_IRQn 1 */
}

[AndyBig]

Ну вообще можно посмотреть исходники того же Marlin (управление 3D-принтерами) или LinuxCNC. Там все это (и многое другое) есть.

[xoxa]

AndyBig писал(а):

Надо, наверное, копать в сторону формулы определения расстояния при равноускоренном движении:
...

...

От куда такие картинки ? Какой это софт ?

[Ivanoff-iv]

всё очень просто
разгон:
V=a*t;
if (V>Vmax) V=Vmax;
торможение:
смотри формулу пути до остановки при равнозамедленном движении (например высоты подброшенного вверх камня) из формулы выражай скорость, дальше как и при разгоне если скорость получилась больше, чем максимальня - используй максимальную, если меньше - используй полученную.
П.С. эти три условия (разгон, максимальная, торможение) собери в одну функцию т.к. при коротком пути торможение может потребоваться ещё до окончания разгона.

Добавлено after 1 minute 38 seconds:
Re: Алгоритм плавного разгона и торможения шгового двигателя
ох, ё... опять говно мамонта вырыли...

[xoxa]

Это я копатель

[Eddy_Em]

Делал в прерывании по переполнению таймера, генерирующего меандр: просто проверяется КА состояния, и если надо, в ARR добавляется или вычитается постоянная величина. На гитхабе у меня валяется код - взять тот же фотометр MMPP.

[xoxa]

Eddy_Em писал(а):

Делал в прерывании по переполнению таймера, генерирующего меандр: просто проверяется КА состояния, и если надо, в ARR добавляется или вычитается постоянная величина. На гитхабе у меня валяется код - взять тот же фотометр MMPP.

надо бы в одном таймере сразу несколькими двигателями управлять

[Eddy_Em]

Одним таймером невозможно: не получится разгонять и тормозить. Строго по таймеру на движок.

[xoxa]

Не надо так категорично. Marlin, grbl не тратят по таймеру на двигатель и работают. Даже с 6 двигателями одновременно.

[Eddy_Em]

Потому как написаны школьниками за еду! Через одно место...
Стопудово, там управление производится тупым ногодрыгом. И если один двигатель должен делать 2000 шагов в секунду, а второй - 0.03, ничего не выйдет...

[Ivanoff-iv]

шаговики (степ-дир) — можно собрать софт-таймер и подключить несколько штук на 1 таймер... макс. обороты будут меньше, чем при использовании индивидуального таймера (и джиттер больше, особенно на больших оборотах)..., но тоже вполне сносно... по крайней мере 200 и 0,03 разом без проблем...

[xoxa]

Eddy_Em писал(а):

Потому как написаны школьниками за еду! Через одно место...
Стопудово, там управление производится тупым ногодрыгом. И если один двигатель должен делать 2000 шагов в секунду, а второй - 0.03, ничего не выйдет...

вот он об этом не знал и получилось:
http://easyelectronics.ru/upravlenie-mn ... hinok.html

[bmv89]

Добрый день! Помогите понять принцип (алгоритм) работы линейного интерполятора контроллера двухосевого чпу. Пытался изучить исходники marlin и grbl (файлы stepper.c и stepper.h), но мало чего хорошего из этого получилось. В общем пришлось изобретать свой велосипед с нуля. В файле stepper.c в комментариях авторы приводят краткое пояснение принципа работы интерполятора (скрин во вложении), но этой информации мне не хватило для полного понимания.
Сделал свою простенькую реализацию, которая обрабатывает каждый кадр (каждую команду G0/G1) независимо от предшествующих и последующих команд перемещения (G0/G1). Получилось, что каждое линейное перемещение начинается с нулевой скорости по обеим осям и заканчивается нулевой скоростью. Форма графика зависимости скорости по отдельной оси от времени трапецеидальная (если времени достаточно для достижения заданной скорости в пределах одной команды G0/G1), либо равнобедренный треугольник (если времени не достаточно для достижения заданной скорости в пределах одной команды G0/G1). Время и количество шагов на ускорение и замедление для отдельной оси всегда одинаковое. Но при такой реализации очень неэффективно происходит обработка кривых траекторий, которые преобразуются генератором g-кода в большое количество мелких линейных перемещений.
Понятно, что нужно как-то учитывать последующую команду G0/G1 для определения конечной скорости текущего перемещения. И начинать каждое новое перемещение с нулевой скоростью не обязательно, если изменение направления траектории движения не существенное. Но ведь, получается, что при обсчёте одного кадра (вычисление начальной скорости, количество шагов на ускорение, количества шагов на замедление, конечной скорости) необходимо учитывать несколько кадров предшествующих и как минимум один последующий кадр. Ведь не факт, что за 1 или 2 линейных перемещения удастся достичь заданной скорости, а замедлятся потом (может так случится) придётся также в течении не одного кадра (например отрисовка окружности или дуги или даже простой прямой, но состоящей из отдельных коллинеарных отрезков малой длины).
В описании в файле stepper.c трапеция обозначается, как BLOCK. Что авторы подразумевают под BLOCK? Отдельный кадр (команду G0/G1), или набор нескольких кадров? Для организации конвейера явно используется буфер команд элементарных линейных перемещений, пока исполняются команды из одной его половины происходит заполнение второй половины (из COM порта, SD карты или ещё откуда...). И этот буфер, явно, вмещает не больше 100-500 команд. Что если количество команд перемещения в BLOCK будет (в каких-то случаях) превышать ёмкость буфера команд? Или (ещё более реальная ситуация) начало BLOCKа попадёт на одну половину буфера, а конец на другую половину, тогда у контроллера вообще не будет единовременного доступа ко всем кадрам данного BLOCKа. В общем не могу понять саму идею реализации линейного интерполятора на микроконтроллерах.

Вложения

img.jpg

(119.41 KiB) Скачиваний: 83