feat!: WIP convert all motor controls to stack based task pool
This commit close this branch with WIP implementation of a stack based task pool system to handle long running action (motor move) of the kissStepper lib (that need a long `while`). The goal was to allow handling of incomming serrial messages and motion cancellation during stepper control loop.
This commit is contained in:
parent
cbb17e279f
commit
403f9181e3
30
serial_stream.cpp
Normal file
30
serial_stream.cpp
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
#include "serial_stream.h"
|
||||
|
||||
SerialStream &SerialStream::connect(HardwareSerial &serial) {
|
||||
_serial = &serial;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::disconnect() {
|
||||
_serial = nullptr;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::bind(Turret &turret) {
|
||||
_turret = &turret;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::unbind() {
|
||||
_turret = nullptr;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerialStream &SerialStream::loop() {
|
||||
if (_serial == nullptr) return *this;
|
||||
if (_serial->available() == 0) return *this;
|
||||
|
||||
_serial->read();
|
||||
|
||||
return *this;
|
||||
}
|
||||
28
serial_stream.h
Normal file
28
serial_stream.h
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
#ifndef SERIAL_STREAM_H
|
||||
#define SERIAL_STREAM_H
|
||||
|
||||
#include "HardwareSerial.h"
|
||||
#include "turret.h"
|
||||
|
||||
class SerialStream {
|
||||
public:
|
||||
SerialStream();
|
||||
~SerialStream();
|
||||
|
||||
SerialStream &connect(HardwareSerial &serial);
|
||||
SerialStream &disconnect();
|
||||
|
||||
SerialStream &bind(Turret &turret);
|
||||
SerialStream &unbind();
|
||||
|
||||
SerialStream &loop();
|
||||
|
||||
private:
|
||||
// TEMP to keep this in mind
|
||||
int _internal_buffer;
|
||||
|
||||
HardwareSerial *_serial = nullptr;
|
||||
Turret *_turret = nullptr;
|
||||
};
|
||||
|
||||
#endif
|
||||
216
turret.cpp
216
turret.cpp
|
|
@ -49,66 +49,111 @@ Turret &Turret::init() {
|
|||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::gotoHome() {
|
||||
long xStop = 0;
|
||||
long yStop = 0;
|
||||
bool Turret::_enqueue_tick(Turret::TickHandler handler) {
|
||||
size_t next = (_tick_queue_tail + 1) % Turret::TICK_QUEUE_LENGTH;
|
||||
if (next == _tick_queue_head) return false; // queue full
|
||||
_tick_queue[_tick_queue_tail] = handler;
|
||||
_tick_queue_tail = next;
|
||||
return true;
|
||||
}
|
||||
|
||||
_stepper.x.prepareMove(-1000000l);
|
||||
_stepper.y.prepareMove(-1000000l);
|
||||
bool Turret::_dequeue_tick() {
|
||||
if (_tick_queue_head == _tick_queue_tail) return false; // queue is empty
|
||||
_tick_queue_head = (_tick_queue_head + 1) % Turret::TICK_QUEUE_LENGTH;
|
||||
return true;
|
||||
}
|
||||
|
||||
while (true) {
|
||||
_stepper.x.move();
|
||||
_stepper.y.move();
|
||||
Turret::TickHandler Turret::_current_tick() {
|
||||
if (_tick_queue_head == _tick_queue_tail) return nullptr;
|
||||
return _tick_queue[_tick_queue_head];
|
||||
}
|
||||
|
||||
if (xStop == 2 && yStop == 2) {
|
||||
_home.x = _stepper.x.getPos();
|
||||
_home.y = _stepper.y.getPos();
|
||||
break;
|
||||
Turret &Turret::wait(long ms, bool blocking) {
|
||||
if (blocking) {
|
||||
while(_wait_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_wait_tick;
|
||||
}
|
||||
}
|
||||
|
||||
if (xStop < 2 && digitalRead(_pin.x.home)) {
|
||||
xStop++;
|
||||
_stepper.x.stop();
|
||||
}
|
||||
bool Turret::_wait_tick() {
|
||||
if (delay + start >= now) return false;
|
||||
return true
|
||||
}
|
||||
|
||||
if (yStop < 2 && digitalRead(_pin.y.home)) {
|
||||
yStop++;
|
||||
_stepper.y.stop();
|
||||
Turret &Turret::gotoHome(bool blocking) {
|
||||
if (blocking) {
|
||||
while(_goto_home_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_goto_home_tick;
|
||||
}
|
||||
}
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::gotoZero() {
|
||||
bool Turret::_goto_home_tick() {
|
||||
_stepper.x.prepareMove(-1000000l);
|
||||
_stepper.y.prepareMove(-1000000l);
|
||||
|
||||
bool xStop = _stepper.x.getState();
|
||||
bool yStop = _stepper.y.getState();
|
||||
|
||||
_stepper.x.move();
|
||||
_stepper.y.move();
|
||||
|
||||
if (xStop && yStop ) {
|
||||
_home.x = _stepper.x.getPos();
|
||||
_home.y = _stepper.y.getPos();
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!xStop && digitalRead(_pin.x.home)) {
|
||||
_stepper.x.stop();
|
||||
}
|
||||
|
||||
if (!yStop && digitalRead(_pin.y.home)) {
|
||||
_stepper.y.stop();
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Turret &Turret::gotoZero(bool blocking) {
|
||||
if (blocking) {
|
||||
while(_goto_zero_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_goto_zero_tick;
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Turret::_goto_zero_tick() {
|
||||
_stepper.x.prepareMove(_home.x + _zero_offset.x);
|
||||
_stepper.y.prepareMove(_home.y + _zero_offset.y);
|
||||
|
||||
bool xStop = false;
|
||||
bool yStop = false;
|
||||
bool xStop = _stepper.x.getState();
|
||||
bool yStop = _stepper.y.getState();
|
||||
|
||||
if (xStop && yStop) {
|
||||
_zero.x = _stepper.x.getPos();
|
||||
_zero.y = _stepper.y.getPos();
|
||||
return false;
|
||||
}
|
||||
|
||||
while (true) {
|
||||
if (xStop && yStop)
|
||||
break;
|
||||
if (!xStop)
|
||||
xStop = _stepper.y.move() == STATE_STOPPED;
|
||||
if (!yStop)
|
||||
yStop = _stepper.x.move() == STATE_STOPPED;
|
||||
}
|
||||
|
||||
_zero.x = _stepper.x.getPos();
|
||||
_zero.y = _stepper.y.getPos();
|
||||
return true;
|
||||
}
|
||||
|
||||
Turret &Turret::calibrate(bool blocking) {
|
||||
gotoHome(blocking);
|
||||
gotoZero(blocking);
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::calibrate() {
|
||||
gotoHome();
|
||||
gotoZero();
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
|
||||
Turret &Turret::moveTo(double x, double y, double z, Unit unit, bool blocking) {
|
||||
vec2<long> step;
|
||||
|
||||
if (unit == Unit::MM) {
|
||||
|
|
@ -150,36 +195,62 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
|
|||
long stepY = constrain(step.y % maxDeltaStepY, -(maxDeltaStepY >> 2),
|
||||
(maxDeltaStepY >> 2));
|
||||
|
||||
_stepper.x.prepareMove(_zero.x + stepX);
|
||||
_stepper.y.prepareMove(_zero.y + stepY);
|
||||
|
||||
bool xStop = false;
|
||||
bool yStop = false;
|
||||
|
||||
while (true) {
|
||||
if (xStop && yStop)
|
||||
break;
|
||||
if (!xStop)
|
||||
xStop = _stepper.x.move() == STATE_STOPPED;
|
||||
if (!yStop)
|
||||
yStop = _stepper.y.move() == STATE_STOPPED;
|
||||
if (blocking) {
|
||||
while(_move_to_tick());
|
||||
} else {
|
||||
_next_tick = &Turret::_move_to_tick;
|
||||
}
|
||||
// _stepper.x.prepareMove(_zero.x + stepX);
|
||||
// _stepper.y.prepareMove(_zero.y + stepY);
|
||||
|
||||
_current.x = x;
|
||||
_current.y = y;
|
||||
_current.z = z;
|
||||
// bool xStop = false;
|
||||
// bool yStop = false;
|
||||
|
||||
// while (true) {
|
||||
// if (xStop && yStop)
|
||||
// break;
|
||||
// if (!xStop)
|
||||
// xStop = _stepper.x.move() == STATE_STOPPED;
|
||||
// if (!yStop)
|
||||
// yStop = _stepper.y.move() == STATE_STOPPED;
|
||||
// }
|
||||
|
||||
// _current.x = x;
|
||||
// _current.y = y;
|
||||
// _current.z = z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::moveBy(double x, double y, double z, Unit unit) {
|
||||
bool Turret::_move_to_tick() {
|
||||
stepX;
|
||||
stepY;
|
||||
|
||||
_stepper.x.prepareMove(_zero.x + stepX);
|
||||
_stepper.y.prepareMove(_zero.y + stepY);
|
||||
|
||||
bool xStop = _stepper.x.getState() == STATE_STOPPED;
|
||||
bool yStop = _stepper.y.getState() == STATE_STOPPED;
|
||||
|
||||
if (xStop && yStop) {
|
||||
_current.x = x;
|
||||
_current.y = y;
|
||||
_current.z = z;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!xStop) xStop = _stepper.x.move() == STATE_STOPPED;
|
||||
if (!yStop) yStop = _stepper.y.move() == STATE_STOPPED;
|
||||
}
|
||||
|
||||
Turret &Turret::moveBy(double x, double y, double z, Unit unit, bool blocking) {
|
||||
long zeroXStored = _zero.x;
|
||||
long zeroYStored = _zero.y;
|
||||
|
||||
_zero.x += _stepper.x.getPos();
|
||||
_zero.y += _stepper.y.getPos();
|
||||
|
||||
moveTo(x, y, z, unit);
|
||||
moveTo(x, y, z, unit, blocking);
|
||||
|
||||
_zero.x = zeroXStored;
|
||||
_zero.y = zeroYStored;
|
||||
|
|
@ -191,23 +262,40 @@ Turret &Turret::moveBy(double x, double y, double z, Unit unit) {
|
|||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::moveToX(double x, Unit unit) {
|
||||
return moveTo(x, _current.y, _current.z, unit);
|
||||
Turret &Turret::moveToX(double x, Unit unit, bool blocking) {
|
||||
return moveTo(x, _current.y, _current.z, unit, blocking);
|
||||
}
|
||||
|
||||
Turret &Turret::moveToY(double y, Unit unit) {
|
||||
return moveTo(_current.x, y, _current.z, unit);
|
||||
Turret &Turret::moveToY(double y, Unit unit, bool blocking) {
|
||||
return moveTo(_current.x, y, _current.z, unit, blocking);
|
||||
}
|
||||
|
||||
Turret &Turret::moveToZ(double z, Unit unit) {
|
||||
return moveTo(_current.x, _current.y, z, unit);
|
||||
Turret &Turret::moveToZ(double z, Unit unit, bool blocking) {
|
||||
return moveTo(_current.x, _current.y, z, unit, blocking);
|
||||
}
|
||||
|
||||
Turret &Turret::moveByX(double x, Unit unit) { return moveBy(x, 0, 0, unit); }
|
||||
Turret &Turret::moveByX(double x, Unit unit, bool blocking) { return moveBy(x, 0, 0, unit, blocking); }
|
||||
|
||||
Turret &Turret::moveByY(double y, Unit unit) { return moveTo(0, y, 0, unit); }
|
||||
Turret &Turret::moveByY(double y, Unit unit, bool blocking) { return moveTo(0, y, 0, unit, blocking); }
|
||||
|
||||
Turret &Turret::moveByZ(double z, Unit unit) { return moveTo(0, 0, z, unit); }
|
||||
Turret &Turret::moveByZ(double z, Unit unit, bool blocking) { return moveTo(0, 0, z, unit, blocking); }
|
||||
|
||||
Turret &Turret::nextTick() {
|
||||
TickHandler tick = _current_tick();
|
||||
if (tick == nullptr) return *this;
|
||||
if (!(this->*tick)()) _dequeue_tick();
|
||||
return *this;
|
||||
}
|
||||
|
||||
Turret &Turret::flushTick() {
|
||||
while (_dequeue_tick());
|
||||
return *this;
|
||||
}
|
||||
|
||||
bool Turret::hasTick() {
|
||||
TickHandler tick = _current_tick();
|
||||
return tick != nullptr;
|
||||
}
|
||||
|
||||
Turret &Turret::getPosition(double &x, double &y, double &z, Unit unit) {
|
||||
if (unit == Unit::MM) {
|
||||
|
|
|
|||
44
turret.h
44
turret.h
|
|
@ -45,29 +45,33 @@ public:
|
|||
~Turret();
|
||||
|
||||
Turret &init();
|
||||
Turret &gotoHome();
|
||||
Turret &gotoZero();
|
||||
Turret &gotoHome(bool blocking = false);
|
||||
Turret &gotoZero(bool blocking = false);
|
||||
|
||||
Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveTo(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveBy(double x, double y, double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
|
||||
Turret &moveToX(double x, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveToY(double y, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveToZ(double z, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveToX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveToY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveToZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
|
||||
Turret &moveByX(double x, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveByY(double y, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveByZ(double z, Unit unit = Turret::Unit::CM);
|
||||
Turret &moveByX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveByY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
Turret &moveByZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
|
||||
|
||||
Turret &getPosition(double &x, double &y, double &z, Unit unit);
|
||||
Turret &getHome(double &x, double &y, Unit unit);
|
||||
Turret &getZero(double &x, double &y, Unit unit);
|
||||
|
||||
Turret &calibrate();
|
||||
Turret &calibrate(bool blocking = false);
|
||||
|
||||
Turret &laserOn();
|
||||
Turret &laserOff();
|
||||
|
||||
Turret &nextTick();
|
||||
Turret &flushTick();
|
||||
bool hasTick();
|
||||
|
||||
private:
|
||||
vec2<kissStepper> _stepper;
|
||||
vec2<long> _home;
|
||||
|
|
@ -77,6 +81,24 @@ private:
|
|||
StepRatio _step_ratio;
|
||||
Offset _offset;
|
||||
ZeroOffset _zero_offset;
|
||||
|
||||
// tick queue related
|
||||
static constexpr size_t TICK_QUEUE_LENGTH = 32;
|
||||
using TickHandler = bool (Turret::*)();
|
||||
|
||||
TickHandler _tick_queue[TICK_QUEUE_LENGTH];
|
||||
size_t _tick_queue_head = 0;
|
||||
size_t _tick_queue_tail = 0;
|
||||
|
||||
bool _enqueue_tick(TickHandler fn);
|
||||
bool _dequeue_tick();
|
||||
TickHandler _current_tick();
|
||||
|
||||
bool (Turret::*_next_tick)() = nullptr;
|
||||
|
||||
// tick handlers
|
||||
bool _goto_zero_tick();
|
||||
bool _goto_home_tick();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in a new issue