diff --git a/GyverStepper/GyverStepper.h b/GyverStepper/GyverStepper.h index 945f18a..ec575ba 100644 --- a/GyverStepper/GyverStepper.h +++ b/GyverStepper/GyverStepper.h @@ -29,6 +29,7 @@ v1.10 - повышена точность v1.11 - повышена точность задания скорости v1.12 - пофикшена плавная работа в KEEP_SPEED. Добавлена поддержка "внешних" драйверов. Убран аргумент SMOOTH из setSpeed + v1.13 - исправлены мелкие баги, оптимизация Алгоритм из AccelStepper: https://www.airspayce.com/mikem/arduino/AccelStepper/ AlexGyver, 2020 @@ -89,6 +90,7 @@ float getTargetDeg(); // Установка максимальной скорости (по модулю) в шагах/секунду и градусах/секунду (для режима FOLLOW_POS) // по умолч. 300 +// минимум - 1 шаг в час void setMaxSpeed(float speed); void setMaxSpeedDeg(float speed); @@ -114,6 +116,7 @@ void reset(); // Установка целевой скорости в шагах/секунду и градусах/секунду (для режима KEEP_SPEED) // при ненулевом setAcceleration будет выполнен плавный разгон/торможение к нужной скорости +// минимальная скорость - 1 шаг в час void setSpeed(float speed); void setSpeedDeg(float speed); @@ -137,33 +140,34 @@ uint32_t getMinPeriod(); // Текущий период "тика" для отладки и всего такого uint32_t stepTime; +// подключить внешний обработчик для шага и переключения питания +void attachStep(handler) +void attachPower(handler) + */ // Раскомментируй для использования более плавного, но медленного алгоритма // Также дефайн можно прописать в скетче до подключения библиотеки!!! См. пример smoothAlgorithm //#define SMOOTH_ALGORITHM -#define _MIN_STEPPER_SPEED 10 // мин. скорость для FOLLOW_POS -#define _MAX_STEP_PERIOD (1000000L/_MIN_STEPPER_SPEED) -#define MIN_STEPPER_SPEED (1.0f/3600) // 1 шаг в час - -#ifndef DRIVER_STEP_TIME -#define DRIVER_STEP_TIME 4 -#endif - #include -#ifdef __AVR__ -#include -#endif - -// макросы +// =========== МАКРОСЫ =========== #define degPerMinute(x) ((x)/60.0f) #define degPerHour(x) ((x)/3600.0f) #define _sign(x) ((x) >= 0 ? 1 : -1) // знак числа #define maxMacro(a,b) ((a)>(b)?(a):(b)) // привет esp #define _PINS_AMOUNT ( (_TYPE == STEPPER_PINS) ? (_DRV == 0 ? 2 : 4) : (0) ) +// =========== КОНСТАНТЫ =========== +#ifndef DRIVER_STEP_TIME +#define DRIVER_STEP_TIME 4 +#endif + +#define _MIN_SPEED_FP 5 // мин. скорость для движения в FOLLOW_POS с ускорением +#define _MAX_PERIOD_FP (1000000L/_MIN_SPEED_FP) +#define _MIN_STEP_SPEED (1.0f/3600) // мин. скорость 1 шаг в час + enum GS_driverType { STEPPER2WIRE, STEPPER4WIRE, @@ -187,7 +191,7 @@ enum GS_smoothType { SMOOTH, }; - +// =========== КЛАСС =========== template class GStepper { public: @@ -220,73 +224,79 @@ class GStepper { // возвращает true, если мотор всё ещё движется к цели bool tick() { - #ifndef SMOOTH_ALGORITHM - // в активном режиме движения к цели с ненулевым ускорением - // планировщик скорости быстрый - if (_workState && !_curMode && _accel != 0 && _maxSpeed > _MIN_STEPPER_SPEED) planner(); - #endif - // при плавном разгоне в KEEP_SPEED - if (_smoothStart && _curMode) smoothSpeedPlanner(); - - if (_workState && micros() - _prevTime >= stepTime) { - //_prevTime = micros(); - _prevTime += stepTime; - // FOLLOW_POS - if (!_curMode && _target == _current) { - brake(); - return false; - } - #ifdef SMOOTH_ALGORITHM - // в активном режиме движения к цели с ненулевым ускорением - // планировщик скорости плавный - // выходим если приехал - if (!_curMode && _accel != 0 && _maxSpeed > _MIN_STEPPER_SPEED) - if (!plannerSmooth()) { - brake(); - return false; - } + if (_workState) { + #ifndef SMOOTH_ALGORITHM + if (!_curMode && _accel != 0 && _maxSpeed >= _MIN_SPEED_FP) planner(); // планировщик скорости FOLLOW_POS быстрый #endif - // двигаем мотор - _current += _dir; + if (_smoothStart && _curMode) smoothSpeedPlanner(); // планировщик скорости KEEP_SPEED - if (_DRV == STEPPER2WIRE) stepDir(); - else step(); - } + if (micros() - _prevTime >= stepTime) { // основной таймер степпера + _prevTime += stepTime; + + #ifdef SMOOTH_ALGORITHM + // плавный планировщик вызывается каждый шаг. Проверка остановки + if (!_curMode && _accel != 0 && _maxSpeed >= _MIN_SPEED_FP && !plannerSmooth()) { + brake(); + return false; + } + #endif + + // проверка остановки для быстрого планировщика, а также работы без ускорения + if (!_curMode && _target == _current) { + brake(); + return false; + } + + // смещаем координату + _current += _dir; + + // двигаем мотор + if (_DRV == STEPPER2WIRE) stepDir(); + else { + thisStep += (_globDir ? _dir : -_dir); + step(); + } + } + } return _workState; } // инвертировать направление мотора - void reverse(bool dir) {_globDir = dir;} + void reverse(bool dir) { _globDir = dir; } // инвертировать поведение EN пина - void invertEn(bool dir) {_enDir = dir;} + void invertEn(bool dir) { _enDir = dir; } // установка и чтение текущей позиции в шагах и градусах - void setCurrent(long pos) {_current = pos; _accelSpeed = 0;} - void setCurrentDeg(float pos) {setCurrent((float)pos * _stepsPerDeg);} - long getCurrent() {return _current;} - float getCurrentDeg() {return ((float)_current / _stepsPerDeg);} + void setCurrent(long pos) { _current = pos; _accelSpeed = 0; } + void setCurrentDeg(float pos) { setCurrent((float)pos * _stepsPerDeg); } + long getCurrent() { return _current; } + float getCurrentDeg() { return ((float)_current / _stepsPerDeg); } // установка и получение целевой позиции в шагах и градусах void setTarget(long pos, GS_posType type = ABSOLUTE) { _target = type ? (_current + pos) : pos; if (_target != _current) { - recalculateSpeed(); - _workState = true; - if (!_powerState) enable(); + if (_accel == 0 || _maxSpeed < _MIN_SPEED_FP) { + stepTime = 1000000.0 / _maxSpeed; + _dir = (_target > _current) ? 1 : -1; + } + enable(); } } - - void setTargetDeg(float pos, GS_posType type = ABSOLUTE) {setTarget((float)pos * _stepsPerDeg, type);} - long getTarget() {return _target;} - float getTargetDeg() {return ((float)_target / _stepsPerDeg);} + + void setTargetDeg(float pos, GS_posType type = ABSOLUTE) { setTarget((float)pos * _stepsPerDeg, type); } + long getTarget() { return _target; } + float getTargetDeg() { return ((float)_target / _stepsPerDeg); } // установка максимальной скорости в шагах/секунду и градусах/секунду void setMaxSpeed(float speed) { - _maxSpeed = maxMacro(speed, MIN_STEPPER_SPEED); // 1 шаг в час минимум - recalculateSpeed(); + speed = abs(speed); + _maxSpeed = maxMacro(speed, _MIN_STEP_SPEED); // 1 шаг в час минимум + // считаем stepTime для низких скоростей или отключенного ускорения + if (_accel == 0 || _maxSpeed < _MIN_SPEED_FP) stepTime = 1000000.0 / _maxSpeed; #ifdef SMOOTH_ALGORITHM _cmin = 1000000.0 / _maxSpeed; @@ -301,24 +311,23 @@ class GStepper { #endif } - void setMaxSpeedDeg(float speed){setMaxSpeed((float)speed * _stepsPerDeg);} + void setMaxSpeedDeg(float speed) { setMaxSpeed((float)speed * _stepsPerDeg); } // установка ускорения шагах и градусах в секунду - void setAcceleration(int accel) { + void setAcceleration(int accel) { _accel = abs(accel); _accelInv = 0.5f / accel; _accelTime = accel / 1000000.0f; #ifdef SMOOTH_ALGORITHM _n = _n * (_accel / accel); - _c0 = 0.676 * sqrt(2.0 / _accel) * 1000000.0; // Equation 15 + _c0 = 0.676 * sqrt(2.0 / _accel) * 1000000.0; plannerSmooth(); #endif } - void setAccelerationDeg(float accel) {setAcceleration(accel * _stepsPerDeg);} + void setAccelerationDeg(float accel) { setAcceleration(accel * _stepsPerDeg); } - void autoPower(bool mode) {_autoPower = mode;} + void autoPower(bool mode) { _autoPower = mode; } - // плавная остановка с ускорением void stop() { if (_workState) { if (_curMode == FOLLOW_POS) { @@ -329,88 +338,99 @@ class GStepper { _n = (float)_accelSpeed * _accelSpeed * _accelInv; #endif } else { - setSpeed(0, true); + setSpeed(0); } } } - // жёсткая остановка - void brake() { - if (_workState) { - _workState = false; - if (_autoPower) disable(); - _accelSpeed = 0; - //stepTime = _MAX_STEP_PERIOD; - #ifdef SMOOTH_ALGORITHM - _n = 0; - #endif - } + void brake() { + disable(); + _accelSpeed = 0; + #ifdef SMOOTH_ALGORITHM + _n = 0; + #endif } - void reset() {brake(); setCurrent(0);} + void reset() { + brake(); + setCurrent(0); + } // установка и получение целевой скорости в шагах/секунду и градусах/секунду void setSpeed(float speed, bool smooth = false) { // smooth убран! // 1 шаг в час минимум _speed = speed; - if (abs(_speed) < MIN_STEPPER_SPEED) _speed = MIN_STEPPER_SPEED * _sign(_speed); + if (abs(_speed) < _MIN_STEP_SPEED) _speed = _MIN_STEP_SPEED * _sign(_speed); if (_accel != 0) { // плавный старт - if (_accelSpeed == _speed) return; // скорости совпадают? Выходим - _smoothStart = true; - #ifdef __AVR__ - _smoothPlannerPrd = map(max(abs((int)_speed), abs((int)_accelSpeed)), 1000, 20000, 15000, 1000); - #else - // горячий привет тупому компилятору ESP8266 и индусам, которые его настраивали - int speed1 = abs(_speed); - int speed2 = abs((int)_accelSpeed); - int maxSpeed = maxMacro(speed1, speed2); - _smoothPlannerPrd = map(maxSpeed, 1000, 20000, 15000, 1000); - #endif - _smoothPlannerPrd = constrain(_smoothPlannerPrd, 15000, 1000); - } else { // резкий старт - if (speed == 0) {brake(); return;} // скорость 0? Отключаемся и выходим + if (_accelSpeed != _speed) { + _smoothStart = true; + #ifdef __AVR__ + _smoothPlannerPrd = map(max(abs((int)_speed), abs((int)_accelSpeed)), 1000, 20000, 15000, 1000); + #else + // горячий привет тупому компилятору ESP8266 и индусам, которые его настраивали + int speed1 = abs(_speed); + int speed2 = abs((int)_accelSpeed); + int maxSpeed = maxMacro(speed1, speed2); + _smoothPlannerPrd = map(maxSpeed, 1000, 20000, 15000, 1000); + #endif + _smoothPlannerPrd = constrain(_smoothPlannerPrd, 15000, 1000); + } + } else { // резкий старт + if (speed == 0) { // скорость 0? Отключаемся и выходим + brake(); + return; + } _accelSpeed = _speed; stepTime = round(1000000.0 / abs(_speed)); _dir = (_speed > 0) ? 1 : -1; - } - _workState = true; - if (!_powerState) enable(); + } + enable(); } - void setSpeedDeg(float speed, bool smooth = false) {setSpeed(_stepsPerDeg * speed, smooth);} - float getSpeed() {return (1000000.0 / stepTime * _dir);} - float getSpeedDeg() {return ((float)getSpeed() / _stepsPerDeg);} + void setSpeedDeg(float speed, bool smooth = false) {setSpeed(_stepsPerDeg * speed); } + float getSpeed() { return (1000000.0 / stepTime * _dir); } + float getSpeedDeg() { return ((float)getSpeed() / _stepsPerDeg); } // установка режима работы void setRunMode(GS_runMode mode){ _curMode = mode; - if (mode == KEEP_SPEED) recalculateSpeed(); - else _smoothStart = false; + if (mode == FOLLOW_POS) _smoothStart = false; } - bool getState() {return _workState;} + bool getState() { return _workState; } void enable() { - _powerState = true; - if (_TYPE == STEPPER_PINS) { - // подадим прошлый сигнал на мотор, чтобы вал зафиксировался - if (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF) step(); - if (_enPin != -1) digitalWrite(_enPin, _enDir); - } else if (*_power) _power(1); + _workState = true; + if (!_powerState) { + _powerState = true; + _smoothPlannerTime = _plannerTime = _prevTime = micros(); // сбросить все таймеры + if (_autoPower) { + if (_TYPE == STEPPER_PINS) { + // подадим прошлый сигнал на мотор, чтобы вал зафиксировался + if (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF) step(); + if (_enPin != -1) digitalWrite(_enPin, _enDir); + } else if (*_power) _power(1); + } + } } void disable() { - _powerState = false; - if (_TYPE == STEPPER_PINS) { - if (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF) { - setPin(0, 0); - setPin(1, 0); - setPin(2, 0); - setPin(3, 0); + _workState = false; + if (_powerState) { + _powerState = false; + if (_autoPower) { + if (_TYPE == STEPPER_PINS) { + if (_DRV == STEPPER4WIRE || _DRV == STEPPER4WIRE_HALF) { + setPin(0, 0); + setPin(1, 0); + setPin(2, 0); + setPin(3, 0); + } + if (_enPin != -1) digitalWrite(_enPin, !_enDir); + } else if (*_power) _power(0); } - if (_enPin != -1) digitalWrite(_enPin, !_enDir); - } else if (*_power) _power(0); + } } uint32_t getMinPeriod() { @@ -420,8 +440,8 @@ class GStepper { uint32_t stepTime = 10000; - void attachStep(void (*handler)(uint8_t)) {_step = handler;} - void attachPower(void (*handler)(bool)) {_power = handler;} + void attachStep(void (*handler)(uint8_t)) { _step = handler; } + void attachPower(void (*handler)(bool)) { _power = handler; } private: // настройка пина @@ -449,8 +469,7 @@ class GStepper { // сделать шаг на базе thisStep void step() { - // ~7 us - thisStep += (_globDir ? _dir : -_dir); + // ~7 us if (_TYPE == STEPPER_PINS) { if (_DRV == STEPPER4WIRE) { // 0b11 берёт два бита, т.е. формирует 0 1 2 3 0 1.. @@ -509,23 +528,13 @@ class GStepper { } } - void recalculateSpeed() { - if (!_curMode && (_accel == 0 || _maxSpeed < _MIN_STEPPER_SPEED)) { - stepTime = 1000000.0 / _maxSpeed; - _dir = (_target > _current) ? 1 : -1; - } - } - #ifdef SMOOTH_ALGORITHM // планировщик скорости из AccelStepper bool plannerSmooth() { long err = _target - _current; long stepsToStop = (float)_accelSpeed * _accelSpeed * _accelInv; - if (err == 0 && stepsToStop <= 1) { - brake(); - return false; - } + if (err == 0 && stepsToStop <= 1) return false; if (err > 0) { if (_n > 0) { @@ -568,47 +577,45 @@ class GStepper { // планировщик скорости мой void planner() { if (micros() - _plannerTime >= _plannerPrd) { - _plannerTime = micros(); + _plannerTime += _plannerPrd; // ~110 us long err = _target - _current; // "ошибка" bool thisDir = ( _accelSpeed * _accelSpeed * _accelInv >= abs(err) ); // пора тормозить _accelSpeed += ( _accelTime * _plannerPrd * (thisDir ? -_sign(_accelSpeed) : _sign(err)) ); // разгон/торможение _accelSpeed = constrain(_accelSpeed, -_maxSpeed, _maxSpeed); // ограничение - if (abs(_accelSpeed) > _MIN_STEPPER_SPEED) stepTime = abs(1000000.0 / _accelSpeed); // ограничение на мин. скорость - else stepTime = _MAX_STEP_PERIOD; + if (abs(_accelSpeed) > _MIN_SPEED_FP) stepTime = abs(1000000.0 / _accelSpeed); // ограничение на мин. скорость + else stepTime = _MAX_PERIOD_FP; _dir = _sign(_accelSpeed); // направление для шагов } } - uint32_t _plannerTime = 0; int _plannerPrd = 15000; #endif float _accelTime = 0; int _smoothPlannerPrd = 15000; uint32_t _smoothPlannerTime = 0; + uint32_t _plannerTime = 0; // планировщик разгона для KEEP_SPEED void smoothSpeedPlanner() { if (micros() - _smoothPlannerTime >= _smoothPlannerPrd) { - _smoothPlannerTime = micros(); + _smoothPlannerTime += _smoothPlannerPrd; int8_t dir = _sign(_speed - _accelSpeed); // 1 - разгон, -1 - торможение - _accelSpeed += ( _accelTime * _smoothPlannerPrd * dir); + _accelSpeed += (_accelTime * _smoothPlannerPrd * dir); _dir = _sign(_accelSpeed); // прекращение работы планировщика if ((dir == 1 && _accelSpeed >= _speed) || (dir == -1 && _accelSpeed <= _speed)) { _accelSpeed = _speed; _smoothStart = false; - if (abs(_speed) <= MIN_STEPPER_SPEED) { // если нужно остановиться + if (abs(_speed) <= _MIN_STEP_SPEED) { // если нужно остановиться brake(); - return; // выходим + return; // выходим } } - - if (abs(_accelSpeed) > _MIN_STEPPER_SPEED) stepTime = abs(1000000.0 / _accelSpeed); // ограничение на мин. скорость - else stepTime = _MAX_STEP_PERIOD; + stepTime = abs(1000000.0 / _accelSpeed); } } diff --git a/GyverStepper/library.properties b/GyverStepper/library.properties index f53a90d..2b3e8c4 100644 --- a/GyverStepper/library.properties +++ b/GyverStepper/library.properties @@ -1,5 +1,5 @@ name=GyverStepper -version=1.12 +version=1.13 author=AlexGyver maintainer=AlexGyver sentence=Library for stepmotor control. diff --git a/README.md b/README.md index c01e702..669954c 100644 --- a/README.md +++ b/README.md @@ -1449,7 +1449,7 @@ int16_t _duty = 0; ![Logo](/logos/stepperLogo.png) -### GyverStepper v1.8 [СКАЧАТЬ](https://github.com/AlexGyver/GyverLibs/releases/download/GyverStepper/GyverStepper.zip), [ДОКУМЕНТАЦИЯ](https://alexgyver.ru/gyverstepper/) +### GyverStepper v1.13 [СКАЧАТЬ](https://github.com/AlexGyver/GyverLibs/releases/download/GyverStepper/GyverStepper.zip), [ДОКУМЕНТАЦИЯ](https://alexgyver.ru/gyverstepper/) GyverStepper - производительная библиотека для управления шаговыми моторами - Поддержка 4х пинового (шаг и полушаг) и STEP-DIR драйверов - Автоматическое отключение питания при достижении цели