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.

serial_stream.cpp

@@ -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;
+}

serial_stream.h

@@ -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

turret.cpp

@@ -49,66 +49,111 @@ Turret &Turret::init() {
   return *this;
 }
 
-Turret &Turret::gotoHome() {
+bool Turret::_enqueue_tick(Turret::TickHandler handler) {
-  long xStop = 0;
+  size_t next = (_tick_queue_tail + 1) % Turret::TICK_QUEUE_LENGTH;
-  long yStop = 0;
+  if (next == _tick_queue_head) return false;  // queue full
-
+  _tick_queue[_tick_queue_tail] = handler;
-  _stepper.x.prepareMove(-1000000l);
+  _tick_queue_tail = next;
-  _stepper.y.prepareMove(-1000000l);
+  return true;
-
-  while (true) {
-    _stepper.x.move();
-    _stepper.y.move();
-
-    if (xStop == 2 && yStop == 2) {
-      _home.x = _stepper.x.getPos();
-      _home.y = _stepper.y.getPos();
-      break;
 }
 
-    if (xStop < 2 && digitalRead(_pin.x.home)) {
+bool Turret::_dequeue_tick() {
-      xStop++;
+  if (_tick_queue_head == _tick_queue_tail) return false;  // queue is empty
-      _stepper.x.stop();
+  _tick_queue_head = (_tick_queue_head + 1) % Turret::TICK_QUEUE_LENGTH;
+  return true;
 }
 
-    if (yStop < 2 && digitalRead(_pin.y.home)) {
+Turret::TickHandler Turret::_current_tick() {
-      yStop++;
+  if (_tick_queue_head == _tick_queue_tail) return nullptr;
-      _stepper.y.stop();
+  return _tick_queue[_tick_queue_head];
+}
+
+Turret &Turret::wait(long ms, bool blocking) {
+  if (blocking) {
+    while(_wait_tick());
+  } else {
+    _next_tick = &Turret::_wait_tick;
   }
 }
 
+bool Turret::_wait_tick() {
+  if (delay + start >= now) return false;
+  return true
+}
+
+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 xStop = _stepper.x.getState();
-  bool yStop = false;
+  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;
+
+  return true; 
 }
 
-  _zero.x = _stepper.x.getPos();
+Turret &Turret::calibrate(bool blocking) {
-  _zero.y = _stepper.y.getPos();
+  gotoHome(blocking);
-
+  gotoZero(blocking);
   return *this;
 }
 
-Turret &Turret::calibrate() {
+Turret &Turret::moveTo(double x, double y, double z, Unit unit, bool blocking) {
-  gotoHome();
-  gotoZero();
-  return *this;
-}
-
-Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
   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);
+  if (blocking) {
-  _stepper.y.prepareMove(_zero.y + stepY);
+    while(_move_to_tick());
-
+  } else {
-  bool xStop = false;
+    _next_tick = &Turret::_move_to_tick;
-  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;
   }
+  // _stepper.x.prepareMove(_zero.x + stepX);
+  // _stepper.y.prepareMove(_zero.y + stepY);
 
-  _current.x = x;
+  // bool xStop = false;
-  _current.y = y;
+  // bool yStop = false;
-  _current.z = z;
+
+  // 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) {
+Turret &Turret::moveToX(double x, Unit unit, bool blocking) {
-  return moveTo(x, _current.y, _current.z, unit);
+  return moveTo(x, _current.y, _current.z, unit, blocking);
 }
 
-Turret &Turret::moveToY(double y, Unit unit) {
+Turret &Turret::moveToY(double y, Unit unit, bool blocking) {
-  return moveTo(_current.x, y, _current.z, unit);
+  return moveTo(_current.x, y, _current.z, unit, blocking);
 }
 
-Turret &Turret::moveToZ(double z, Unit unit) {
+Turret &Turret::moveToZ(double z, Unit unit, bool blocking) {
-  return moveTo(_current.x, _current.y, z, unit);
+  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) {

turret.h

@@ -45,29 +45,33 @@ public:
   ~Turret();
 
   Turret &init();
-  Turret &gotoHome();
+  Turret &gotoHome(bool blocking = false);
-  Turret &gotoZero();
+  Turret &gotoZero(bool blocking = false);
 
-  Turret &moveTo(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);
+  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 &moveToX(double x, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveToY(double y, Unit unit = Turret::Unit::CM);
+  Turret &moveToY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveToZ(double z, Unit unit = Turret::Unit::CM);
+  Turret &moveToZ(double z, Unit unit = Turret::Unit::CM, bool blocking = false);
 
-  Turret &moveByX(double x, 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);
+  Turret &moveByY(double y, Unit unit = Turret::Unit::CM, bool blocking = false);
-  Turret &moveByZ(double z, Unit unit = Turret::Unit::CM);
+  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