diff --git a/serial_stream.cpp b/serial_stream.cpp new file mode 100644 index 0000000..0f20b63 --- /dev/null +++ b/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; +} diff --git a/serial_stream.h b/serial_stream.h new file mode 100644 index 0000000..f16e052 --- /dev/null +++ b/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 diff --git a/turret.cpp b/turret.cpp index 0c6881c..be11092 100644 --- a/turret.cpp +++ b/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; +} +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; +} + +Turret::TickHandler Turret::_current_tick() { + if (_tick_queue_head == _tick_queue_tail) return nullptr; + 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; +} + +bool Turret::_goto_home_tick() { _stepper.x.prepareMove(-1000000l); _stepper.y.prepareMove(-1000000l); + + bool xStop = _stepper.x.getState(); + bool yStop = _stepper.y.getState(); - while (true) { - _stepper.x.move(); - _stepper.y.move(); + _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)) { - xStop++; - _stepper.x.stop(); - } - - if (yStop < 2 && digitalRead(_pin.y.home)) { - yStop++; - _stepper.y.stop(); - } + 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; } -Turret &Turret::gotoZero() { +bool Turret::_goto_zero_tick() { _stepper.x.prepareMove(_home.x + _zero_offset.x); _stepper.y.prepareMove(_home.y + _zero_offset.y); + + bool xStop = _stepper.x.getState(); + bool yStop = _stepper.y.getState(); - bool xStop = false; - bool yStop = false; - - while (true) { - if (xStop && yStop) - break; - if (!xStop) - xStop = _stepper.y.move() == STATE_STOPPED; - if (!yStop) - yStop = _stepper.x.move() == STATE_STOPPED; + if (xStop && yStop) { + _zero.x = _stepper.x.getPos(); + _zero.y = _stepper.y.getPos(); + return false; } - _zero.x = _stepper.x.getPos(); - _zero.y = _stepper.y.getPos(); + if (!xStop) + xStop = _stepper.y.move() == STATE_STOPPED; + if (!yStop) + yStop = _stepper.x.move() == STATE_STOPPED; + 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 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) { diff --git a/turret.h b/turret.h index c73489e..9e81a69 100644 --- a/turret.h +++ b/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 _stepper; vec2 _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