diff --git a/README.md b/README.md
index 6e5c4dc..a528223 100644
--- a/README.md
+++ b/README.md
@@ -729,6 +729,7 @@ void loop() {
- v2.1 - добавил GyverStepper2, упрощённая и оптимизированная версия GyverStepper
- v2.1.1 - исправлена бага в GyverStepper
- v2.1.2 - совместимость Digispark
+- v2.1.3 - починил FOLLOW_POS в GStepper, починил RELATIVE в GPlanner2 и исправил багу с рывками
## Баги и обратная связь
diff --git a/library.properties b/library.properties
index 9de3654..8d85d6b 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
name=GyverStepper
-version=2.1.2
+version=2.1.3
author=AlexGyver
maintainer=AlexGyver
sentence=Fast library for stepmotor control and multi-axis planning
diff --git a/src/FIFO.h b/src/FIFO.h
index 0405167..46bcc95 100644
--- a/src/FIFO.h
+++ b/src/FIFO.h
@@ -2,10 +2,14 @@
#ifndef _FIFO_h
#define _FIFO_h
+#define FIFO_WIPE 1
template
class FIFO {
public:
+ FIFO() {
+ for (int i = 0; i < SIZE; i++) buf[i] = 0;
+ }
// очистить
void clear() {
head = tail = 0;
@@ -20,6 +24,11 @@ class FIFO {
T get(int16_t i = 0) {
return buf[(SIZE + tail + i) % SIZE];
}
+
+ // прочитать крайний элемент буфера
+ T getLast() {
+ return buf[(SIZE + head - 1) % SIZE];
+ }
// добавить элемент с конца
void add(T data) {
@@ -36,8 +45,8 @@ class FIFO {
}
// сдвинуть начало на 1
- void next() {
- buf[tail] = 0;
+ void next(bool wipe = 0) {
+ if (wipe) buf[tail] = 0;
tail = (tail + 1) % SIZE;
}
diff --git a/src/GyverPlanner.h b/src/GyverPlanner.h
index 4e9db13..916a256 100644
--- a/src/GyverPlanner.h
+++ b/src/GyverPlanner.h
@@ -14,10 +14,6 @@
AlexGyver, alex@alexgyver.ru
https://alexgyver.ru/
MIT License
-
- Версии:
- v1.0
- v1.1 - небольшие фиксы
*/
/*
diff --git a/src/GyverPlanner2.h b/src/GyverPlanner2.h
index e9b0254..529f725 100644
--- a/src/GyverPlanner2.h
+++ b/src/GyverPlanner2.h
@@ -9,14 +9,11 @@
- Режим постоянного вращения для одной оси (для движения к концевику например)
- Тормоз/плавная остановка/пауза на траектории планировщика
- Оптимизировано для работы по прерыванию таймера
- - Быстрый контроль пинов шаговика для Arduino AVR
+ - Быстрый контроль пинов шаговика для Arduino AVR
AlexGyver, alex@alexgyver.ru
https://alexgyver.ru/
MIT License
-
- Версии:
- v1.0
*/
/*
@@ -281,8 +278,8 @@ class GPlanner2 {
// добавить новую точку. Массив координат, флаг окончания и абсолютный/относительный
void addTarget(int32_t tar[], uint8_t l, GS_posType type = ABSOLUTE) {
- if (type = ABSOLUTE) for (int i = 0; i < _AXLES; i++) bufP[i].add(tar[i]);
- else for (int i = 0; i < _AXLES; i++) bufP[i].add(tar[i] + bufP[i].get(-1));
+ if (type == ABSOLUTE) for (int i = 0; i < _AXLES; i++) bufP[i].add(tar[i]);
+ else for (int i = 0; i < _AXLES; i++) bufP[i].add(tar[i] + bufP[i].getLast());
bufL.add(l);
bufV.add(0);
bufS.add(0);
@@ -342,10 +339,10 @@ class GPlanner2 {
bufV.set(i + 1, maxV);
} else if (v1 < v0 && maxV > v1) {
int16_t count = 0;
- while (1) {
+ while (true) {
uint32_t minV = sqrt(2ul * a * bufS.get(i + count) + (uint32_t)bufV.get(i + count + 1) * bufV.get(i + count + 1));
if (minV >= bufV.get(i + count)) break;
- else bufV.set(i + count, minV);
+ else bufV.set(i + count, minV);
count--;
}
}
@@ -367,7 +364,7 @@ class GPlanner2 {
void next() {
for (int i = 0; i < _AXLES; i++) bufP[i].next();
bufL.next();
- bufV.next();
+ bufV.next(FIFO_WIPE); // обнуляем использованную ячейку
bufS.next();
}
@@ -388,18 +385,18 @@ class GPlanner2 {
for (int i = 0; i < _AXLES; i++) nd[i] = S / 2u; // записываем половину
if (a > 0) {
- int16_t v1 = bufV.get(0); // скорость начала отрезка
- int16_t v2 = bufV.get(1);
+ int32_t v1 = bufV.get(0); // скорость начала отрезка
+ int32_t v2 = bufV.get(1); // скорость конца отрезка
if (2L * V * V - v1 * v1 - v2 * v2 > 2L * a * S) { // треугольник
- s1 = (2L * a * S + (int32_t)v2 * v2 - (int32_t)v1 * v1) / (4L * a);
+ s1 = (2L * a * S + v2 * v2 - v1 * v1) / (4L * a);
s2 = 0;
} else { // трапеция
- s1 = ((int32_t)V * V - (int32_t)v1 * v1) / (2L * a);
- s2 = S - ((int32_t)V * V - (int32_t)v2 * v2) / (2L * a);
+ s1 = ((int32_t)V * V - v1 * v1) / (2L * a);
+ s2 = S - ((int32_t)V * V - v2 * v2) / (2L * a);
}
- so1 = (int32_t)v1 * v1 / (2 * a);
- so2 = (int32_t)v2 * v2 / (2 * a);
+ so1 = v1 * v1 / (2 * a);
+ so2 = v2 * v2 / (2 * a);
if (status != 4) {
if (v1 == 0) us = us0;
else us = 1000000ul / v1;
diff --git a/src/GyverStepper.h b/src/GyverStepper.h
index 03cacb7..6eeb7d8 100644
--- a/src/GyverStepper.h
+++ b/src/GyverStepper.h
@@ -43,6 +43,7 @@
v2.1 - добавил GyverStepper2, упрощённая и оптимизированная версия GyverStepper
v2.1.1 - исправлена бага в GyverStepper
v2.1.2 - совместимость Digispark
+ v2.1.3 - починил FOLLOW_POS в GStepper, починил RELATIVE в GPlanner2 и исправил багу с рывками
*/
/*
@@ -182,7 +183,6 @@ enum GS_runMode {
KEEP_SPEED,
};
-
enum GS_smoothType {
NO_SMOOTH,
SMOOTH,
@@ -243,8 +243,8 @@ class GStepper : public Stepper<_DRV, _TYPE> {
}
// установка текущей позиции в градусах
- void setCurrentDeg(float pos) {
- setCurrent((float)pos * _stepsPerDeg);
+ void setCurrentDeg(float npos) {
+ setCurrent((float)npos * _stepsPerDeg);
}
// чтение текущей позиции в шагах
@@ -258,8 +258,8 @@ class GStepper : public Stepper<_DRV, _TYPE> {
}
// установка целевой позиции в шагах
- void setTarget(long pos, GS_posType type = ABSOLUTE) {
- _target = type ? (pos + pos) : pos;
+ void setTarget(long npos, GS_posType type = ABSOLUTE) {
+ _target = type ? (npos + pos) : npos;
if (_target != pos) {
if (_accel == 0 || _maxSpeed < _MIN_SPEED_FP) {
stepTime = 1000000.0 / _maxSpeed;
@@ -270,8 +270,8 @@ class GStepper : public Stepper<_DRV, _TYPE> {
}
// установка целевой позиции в градусах
- void setTargetDeg(float pos, GS_posType type = ABSOLUTE) {
- setTarget((float)pos * _stepsPerDeg, type);
+ void setTargetDeg(float npos, GS_posType type = ABSOLUTE) {
+ setTarget((float)npos * _stepsPerDeg, type);
}
// получение целевой позиции в шагах
@@ -516,7 +516,7 @@ class GStepper : public Stepper<_DRV, _TYPE> {
if (tickUs - _plannerTime >= _plannerPrd) {
_plannerTime += _plannerPrd;
// ~110 us
- long err = _target - pos; // "ошибка"
+ long err = _target - pos; // "ошибка"
bool thisDir = ( _accelSpeed * _accelSpeed * _accelInv >= abs(err) ); // пора тормозить
_accelSpeed += ( _accelTime * _plannerPrd * (thisDir ? -_sign(_accelSpeed) : _sign(err)) ); // разгон/торможение
if (_stopSpeed == 0) _accelSpeed = constrain(_accelSpeed, -_maxSpeed, _maxSpeed); // ограничение
diff --git a/src/GyverStepper2.h b/src/GyverStepper2.h
index dbdd70d..fed7e69 100644
--- a/src/GyverStepper2.h
+++ b/src/GyverStepper2.h
@@ -7,9 +7,10 @@
- Вращение со скоростью (без плавной смены скорости)
- Оптимизировано для работы по прерыванию таймера
- Наследует класс Stepper из StepperCore
-
- Версии:
- v1.0
+
+ AlexGyver, alex@alexgyver.ru
+ https://alexgyver.ru/
+ MIT License
*/
/*
diff --git a/src/StepperCore.h b/src/StepperCore.h
index a0f4190..773313a 100644
--- a/src/StepperCore.h
+++ b/src/StepperCore.h
@@ -4,9 +4,10 @@
- Поддержка пина EN
- Виртуальный драйвер
- Быстрый алгоритм IO для AVR
-
- Версии:
- v1.0
+
+ AlexGyver, alex@alexgyver.ru
+ https://alexgyver.ru/
+ MIT License
*/
/*