upd

GyverLibs · Oct 23, 2021 · e571e62 · e571e62
1 parent a61ab0f
commit e571e62
Show file tree

Hide file tree

Showing 8 changed files with 42 additions and 37 deletions.
diff --git 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 и исправил багу с рывками
 
 <a id="feedback"></a>
 ## Баги и обратная связь

diff --git a/library.properties b/library.properties
@@ -1,5 +1,5 @@
 name=GyverStepper
-version=2.1.2
+version=2.1.3
 author=AlexGyver <[email protected]>
 maintainer=AlexGyver <[email protected]>
 sentence=Fast library for stepmotor control and multi-axis planning

diff --git a/src/FIFO.h b/src/FIFO.h
@@ -2,10 +2,14 @@
 
 #ifndef _FIFO_h
 #define _FIFO_h
+#define FIFO_WIPE 1
 
 template <typename T, uint16_t SIZE >
 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
@@ -14,10 +14,6 @@
     AlexGyver, [email protected]
     https://alexgyver.ru/
     MIT License
-    
-    Версии:
-    v1.0
-    v1.1 - небольшие фиксы
 */
 
 /*

diff --git a/src/GyverPlanner2.h b/src/GyverPlanner2.h
@@ -9,14 +9,11 @@
     - Режим постоянного вращения для одной оси (для движения к концевику например)
     - Тормоз/плавная остановка/пауза на траектории планировщика
     - Оптимизировано для работы по прерыванию таймера
-    - Быстрый контроль пинов шаговика для Arduino AVR    
+    - Быстрый контроль пинов шаговика для Arduino AVR
     
     AlexGyver, [email protected]
     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
@@ -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
@@ -7,9 +7,10 @@
     - Вращение со скоростью (без плавной смены скорости)
     - Оптимизировано для работы по прерыванию таймера
     - Наследует класс Stepper из StepperCore
-
-    Версии:
-    v1.0
+    
+    AlexGyver, [email protected]
+    https://alexgyver.ru/
+    MIT License
 */
 
 /*

diff --git a/src/StepperCore.h b/src/StepperCore.h
@@ -4,9 +4,10 @@
     - Поддержка пина EN
     - Виртуальный драйвер
     - Быстрый алгоритм IO для AVR
-
-    Версии:
-    v1.0
+    
+    AlexGyver, [email protected]
+    https://alexgyver.ru/
+    MIT License
 */
 
 /*