diff --git a/README.md b/README.md index 25aaadb..3d0876a 100644 --- a/README.md +++ b/README.md @@ -307,6 +307,7 @@ void loop() { - Во время движения к цели можно вызвать brake(), тогда мотор остановится, ready() не вернёт true - После остановки можно вызвать resume(), мотор продолжит движение к цели - Постоянное вращение задаётся setSpeed()/setSpeedDeg(). Остановиться можно резко - stop() или brake() +- Скорость и ускорение можно задать в любое время, но применяются они после остановки мотора!
РАЗВЕРНУТЬ @@ -429,6 +430,7 @@ void loop() { - Тормоз/плавная остановка/пауза на траектории планировщика - Оптимизировано для работы по прерыванию таймера - Быстрый контроль пинов шаговика для Arduino AVR +- Скорость и ускорение можно задать в любое время, но применяются они после остановки мотора! ### Логика работы Планировщик управляет любым количеством моторов, вращая их к указанной позиции. В данной версии @@ -565,6 +567,7 @@ void loop() { - Тормоз/плавная остановка/пауза на траектории планировщика - Оптимизировано для работы по прерыванию таймера - Быстрый контроль пинов шаговика для Arduino AVR +- Скорость и ускорение можно задать в любое время, но применяются они после остановки мотора! ### Логика работы Планировщик управляет любым количеством моторов, вращая их к указанной позиции. В данной версии @@ -731,6 +734,7 @@ void loop() { - v2.1.2 - совместимость Digispark - v2.1.3 - починил FOLLOW_POS в GStepper, починил RELATIVE в GPlanner2 и исправил багу с рывками - v2.1.4 - GPlanner2: исправил рывки, добавил адаптивное перестроение траектории без остановок, чутка оптимизировал вычисления +- v2.1.5 - возможность менять скорость и ускорение во время работы планировщика (GStepper2, GPlanner, GPlanner2) ## Баги и обратная связь diff --git a/library.properties b/library.properties index 240586e..ebcbaaf 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=GyverStepper -version=2.1.4 +version=2.1.5 author=AlexGyver maintainer=AlexGyver sentence=Fast library for stepmotor control and multi-axis planning diff --git a/src/GyverPlanner.h b/src/GyverPlanner.h index 916a256..57f7c1c 100644 --- a/src/GyverPlanner.h +++ b/src/GyverPlanner.h @@ -98,16 +98,25 @@ class GPlanner { } // установка максимальной скорости планировщика в шаг/сек - void setMaxSpeed(float nV) { - V = nV; - usMin = 1000000.0 / V; + void setMaxSpeed(float speed) { + nV = speed; + if (!status) { + V = nV; + usMin = 1000000.0 / V; + setAcceleration(na); + changeSett = 0; + } else changeSett = 1; } // установка ускорения планировщика в шаг/сек^2 - void setAcceleration(uint16_t nA) { - a = nA; - if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); - else us0 = usMin; + void setAcceleration(uint16_t acc) { + na = acc; + if (!status) { + a = na; + if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); + else us0 = usMin; + changeSett = 0; + } else changeSett = 1; } // пауза (доехать до заданной точки и ждать). ready() не вернёт true, пока ты на паузе @@ -166,6 +175,11 @@ class GPlanner { // установить цель в шагах и начать движение. ~100 us bool setTarget(int32_t target[], GS_posType type = ABSOLUTE) { tmr = micros(); + if (changeSett) { // применяем настройки + setMaxSpeed(nV); + changeSett = 0; + } + S = 0; // путь for (uint8_t i = 0; i < _AXLES; i++) { // для всех осей if (type == RELATIVE) target[i] += steppers[i]->pos; // если относительное смещение - прибавляем текущий pos @@ -179,7 +193,7 @@ class GPlanner { } if (S == 0) { // путь == 0, мы никуда не едем readyF = true; // готовы к следующей точке - status = 0; // стоп машина + brake(); // стоп машина return 0; } @@ -285,10 +299,7 @@ class GPlanner { us10 += 2ul * us10 / (4ul * stopStep + 1); // торможение us = (uint32_t)us10 >> 10; us = constrain(us, usMin, us0); - if (stopStep <= 0 || step >= S || us >= us0) { - us = 0; - status = 0; - } + if (stopStep <= 0 || step >= S || us >= us0) brake(); return status; } @@ -299,15 +310,14 @@ class GPlanner { us = (uint32_t)us10 >> 10; us = constrain(us, usMin, us0); } - else if (step < s2) us = usMin; // постоянная - else if (step < S) { // торможение + else if (step < s2) us = usMin; // постоянная + else if (step < S) { // торможение us10 += 2ul * us10 / (4ul * (S - step) + 1); us = (uint32_t)us10 >> 10; us = constrain(us, usMin, us0); - } else { // приехали - us = 0; + } else { // приехали if (status == 1) readyF = true; - status = 0; + brake(); } return status; } @@ -327,11 +337,12 @@ class GPlanner { int32_t tar[_AXLES], nd[_AXLES], dS[_AXLES]; int32_t step, S, s1, s2, so1; uint32_t tmr, us0, usMin, us10; - uint16_t a; + uint16_t a, na; int16_t stopStep; - float V; + float V, nV; uint8_t status = 0, speedAxis = 0, maxAx; bool readyF = true; + bool changeSett = 0; Stepper<_DRV>* steppers[_AXLES]; }; #endif \ No newline at end of file diff --git a/src/GyverPlanner2.h b/src/GyverPlanner2.h index 6e34e0d..2db0847 100644 --- a/src/GyverPlanner2.h +++ b/src/GyverPlanner2.h @@ -99,16 +99,25 @@ class GPlanner2 { // ============================= PLANNER ============================= // установка максимальной скорости планировщика в шаг/сек - void setMaxSpeed(float nV) { - V = nV; - usMin = 1000000.0 / V; + void setMaxSpeed(float speed) { + nV = speed; + if (!status) { + V = nV; + usMin = 1000000.0 / V; + setAcceleration(na); + changeSett = 0; + } else changeSett = 1; } // установка ускорения планировщика в шаг/сек^2 - void setAcceleration(uint16_t nA) { - a = nA; - if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); - else us0 = usMin; + void setAcceleration(uint16_t acc) { + na = acc; + if (!status) { + a = na; + if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); + else us0 = usMin; + changeSett = 0; + } else changeSett = 1; } // установить dt смены скорости, 0.0.. 1.0 по умолч. 0.3 @@ -261,7 +270,7 @@ class GPlanner2 { if (bufL.available() > 1) { if (--block < 0) block = 0; // в движении с ускорением пересчитываем путь, если остановились или есть на это время - if (a > 0 && (bufV.get(0) == 0 || (block == 0 && us > blockCalc))) calculateBlock(); + if (a > 0 && (bufV.get(0) == 0 || (block == 0 && !changeSett && us > blockCalc))) calculateBlock(); if (setTarget()) return 1; } } @@ -310,6 +319,16 @@ class GPlanner2 { private: // пересчитать скорости void calculateBlock() { + if (changeSett && bufV.get(0) == 0) { // параметры движения изменились, а мы стоим + noInterrupts(); + uint8_t stBuf = status; + status = 0; + setMaxSpeed(nV); + status = stBuf; + interrupts(); + changeSett = 0; + } + uint32_t calcTime = micros(); block = _BUF / 2; // через половину буфера попробуем сделать перерасчёт пути uint32_t nextS = calcS(0); @@ -431,15 +450,17 @@ class GPlanner2 { int32_t nd[_AXLES], dS[_AXLES]; int32_t step, S, s1, s2, so1, so2; uint32_t tmr, us0, usMin, us10; - uint16_t a; + uint16_t a, na; int16_t stopStep; - float V; + float V, nV; uint8_t status = 0, speedAxis = 0, maxAx; bool readyF = true; float dtA = 0.3; int8_t block = 100; uint32_t blockCalc = 20000; + bool changeSett = 0; + Stepper<_DRV>* steppers[_AXLES]; FIFO bufP[_AXLES]; diff --git a/src/GyverStepper.h b/src/GyverStepper.h index 28554e2..4976398 100644 --- a/src/GyverStepper.h +++ b/src/GyverStepper.h @@ -45,6 +45,7 @@ v2.1.2 - совместимость Digispark v2.1.3 - починил FOLLOW_POS в GStepper, починил RELATIVE в GPlanner2 и исправил багу с рывками v2.1.4 - GPlanner2: исправил рывки, добавил адаптивное перестроение траектории без остановок, чутка оптимизировал вычисления + v2.1.5 - возможность менять скорость и ускорение во время работы планировщика (GStepper2, GPlanner, GPlanner2) */ /* diff --git a/src/GyverStepper2.h b/src/GyverStepper2.h index fed7e69..2ea1aea 100644 --- a/src/GyverStepper2.h +++ b/src/GyverStepper2.h @@ -188,6 +188,15 @@ class GStepper2 : public Stepper<_DRV, _TYPE> { // установить цель и опционально режим void setTarget(int32_t ntar, GS_posType type = ABSOLUTE) { tmr = micros(); + if (changeSett) { // применяем настройки + usMin = usMinN; + #ifndef GS_NO_ACCEL + V = 1000000L / usMin; + setAcceleration(na); + #endif + changeSett = 0; + } + if (type == RELATIVE) tar = ntar + pos; else tar = ntar; if (tar == pos) { @@ -261,20 +270,30 @@ class GStepper2 : public Stepper<_DRV, _TYPE> { // ========================== POSITION SETTINGS ========================== // установка ускорения в шаг/сек^2 - void setAcceleration(uint16_t nA) { + void setAcceleration(uint16_t acc) { #ifndef GS_NO_ACCEL - a = nA; - if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); - else us0 = usMin; + na = acc; + if (!status) { // применяем, если мотор остановлен + a = na; + if (a != 0) us0 = 0.676 * 1000000 * sqrt(2.0 / a); + else us0 = usMin; + changeSett = 0; + } else changeSett = 1; // иначе флаг на изменение #endif } // установить скорость движения при следовании к позиции в шагах/сек void setMaxSpeed(int32_t speed) { - usMin = 1000000L / speed; - #ifndef GS_NO_ACCEL - V = (uint16_t)speed; // если < 1, отсечётся до 0 - #endif + if (speed == 0) return; + usMinN = 1000000L / speed; + if (!status) { // применяем, если мотор остановлен + usMin = usMinN; + #ifndef GS_NO_ACCEL + setAcceleration(a); + V = (uint16_t)speed; // если < 1, отсечётся до 0 + #endif + changeSett = 0; + } else changeSett = 1; // иначе флаг на изменение } #ifndef ESP8266 @@ -285,10 +304,16 @@ class GStepper2 : public Stepper<_DRV, _TYPE> { // установить скорость движения при следовании к позиции в шагах/сек, float void setMaxSpeed(float speed) { - usMin = 1000000.0 / speed; - #ifndef GS_NO_ACCEL - V = speed; // если < 1, отсечётся до 0 - #endif + if (speed == 0) return; + usMinN = 1000000.0 / speed; + if (!status) { // применяем, если мотор остановлен + usMin = usMinN; + #ifndef GS_NO_ACCEL + setAcceleration(a); + V = (uint16_t)speed; // если < 1, отсечётся до 0 + #endif + changeSett = 0; + } else changeSett = 1; // иначе флаг на изменение } // установить скорость движения при следовании к позиции в град/сек @@ -370,9 +395,12 @@ class GStepper2 : public Stepper<_DRV, _TYPE> { uint16_t stepsRev; uint8_t status = 0; bool readyF = 0; + bool changeSett = 0; + uint32_t usMinN; #ifndef GS_NO_ACCEL uint16_t a, V; + uint16_t na; int16_t stopStep; uint32_t S, us0, us10; int32_t s1, s2, steps;