Skip to content

Commit

Permalink
upd
Browse files Browse the repository at this point in the history
  • Loading branch information
@GyverLibs
GyverLibs committed Oct 26, 2021
1 parent 5aa6021 commit 13dd8ec
Show file tree
Hide file tree
Showing 6 changed files with 107 additions and 42 deletions.
4 changes: 4 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -307,6 +307,7 @@ void loop() {
- Во время движения к цели можно вызвать brake(), тогда мотор остановится, ready() не вернёт true
- После остановки можно вызвать resume(), мотор продолжит движение к цели
- Постоянное вращение задаётся setSpeed()/setSpeedDeg(). Остановиться можно резко - stop() или brake()
- Скорость и ускорение можно задать в любое время, но применяются они после остановки мотора!

<details>
<summary>РАЗВЕРНУТЬ</summary>
Expand Down Expand Up @@ -429,6 +430,7 @@ void loop() {
- Тормоз/плавная остановка/пауза на траектории планировщика
- Оптимизировано для работы по прерыванию таймера
- Быстрый контроль пинов шаговика для Arduino AVR
- Скорость и ускорение можно задать в любое время, но применяются они после остановки мотора!
### Логика работы
Планировщик управляет любым количеством моторов, вращая их к указанной позиции. В данной версии
Expand Down Expand Up @@ -565,6 +567,7 @@ void loop() {
- Тормоз/плавная остановка/пауза на траектории планировщика
- Оптимизировано для работы по прерыванию таймера
- Быстрый контроль пинов шаговика для Arduino AVR
- Скорость и ускорение можно задать в любое время, но применяются они после остановки мотора!

### Логика работы
Планировщик управляет любым количеством моторов, вращая их к указанной позиции. В данной версии
Expand Down Expand Up @@ -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)

<a id="feedback"></a>
## Баги и обратная связь
Expand Down
2 changes: 1 addition & 1 deletion library.properties
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
name=GyverStepper
version=2.1.4
version=2.1.5
author=AlexGyver <alex@alexgyver.ru>
maintainer=AlexGyver <alex@alexgyver.ru>
sentence=Fast library for stepmotor control and multi-axis planning
Expand Down
49 changes: 30 additions & 19 deletions src/GyverPlanner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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, пока ты на паузе
Expand Down Expand Up @@ -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
Expand All @@ -179,7 +193,7 @@ class GPlanner {
}
if (S == 0) { // путь == 0, мы никуда не едем
readyF = true; // готовы к следующей точке
status = 0; // стоп машина
brake(); // стоп машина
return 0;
}

Expand Down Expand Up @@ -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;
}

Expand All @@ -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;
}
Expand All @@ -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
41 changes: 31 additions & 10 deletions src/GyverPlanner2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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;
}
}
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<int32_t, _BUF> bufP[_AXLES];
Expand Down
1 change: 1 addition & 0 deletions src/GyverStepper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
*/

/*
Expand Down
52 changes: 40 additions & 12 deletions src/GyverStepper2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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
Expand All @@ -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; // иначе флаг на изменение
}

// установить скорость движения при следовании к позиции в град/сек
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 13dd8ec

Please sign in to comment.