Julien Oculi
403f9181e3
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.
433 lines
10 KiB
C++
433 lines
10 KiB
C++
#include "turret.h"
|
|
#include "Arduino.h"
|
|
#include "math.h"
|
|
#include "maths.h"
|
|
#include <kissStepper.h>
|
|
|
|
Turret::Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
|
|
PinMap pin_map_x, PinMap pin_map_y)
|
|
: _stepper(kissStepper(static_cast<uint8_t>(pin_map_x.direction),
|
|
static_cast<uint8_t>(pin_map_x.pulse),
|
|
static_cast<uint8_t>(pin_map_x.enable)),
|
|
kissStepper(static_cast<uint8_t>(pin_map_y.direction),
|
|
static_cast<uint8_t>(pin_map_y.pulse),
|
|
static_cast<uint8_t>(pin_map_y.enable))) {
|
|
|
|
_pin.x = pin_map_x;
|
|
_pin.y = pin_map_y;
|
|
_step_ratio = step_ratio;
|
|
_offset = offset;
|
|
_zero_offset = zero_offset;
|
|
}
|
|
|
|
Turret::~Turret() {
|
|
_stepper.x.stop();
|
|
_stepper.x.disable();
|
|
_stepper.y.stop();
|
|
_stepper.y.disable();
|
|
digitalWrite(_pin.x.laser, LOW);
|
|
digitalWrite(_pin.y.laser, LOW);
|
|
}
|
|
|
|
Turret &Turret::init() {
|
|
pinMode(_pin.x.laser, OUTPUT);
|
|
pinMode(_pin.y.laser, OUTPUT);
|
|
|
|
pinMode(_pin.x.direction, OUTPUT);
|
|
pinMode(_pin.x.pulse, OUTPUT);
|
|
pinMode(_pin.x.enable, OUTPUT);
|
|
pinMode(_pin.x.home, INPUT_PULLUP);
|
|
|
|
pinMode(_pin.y.direction, OUTPUT);
|
|
pinMode(_pin.y.pulse, OUTPUT);
|
|
pinMode(_pin.y.enable, OUTPUT);
|
|
pinMode(_pin.y.home, INPUT_PULLUP);
|
|
|
|
_stepper.x.begin();
|
|
_stepper.y.begin();
|
|
|
|
return *this;
|
|
}
|
|
|
|
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();
|
|
|
|
_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 = _stepper.x.getState();
|
|
bool yStop = _stepper.y.getState();
|
|
|
|
if (xStop && yStop) {
|
|
_zero.x = _stepper.x.getPos();
|
|
_zero.y = _stepper.y.getPos();
|
|
return false;
|
|
}
|
|
|
|
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::moveTo(double x, double y, double z, Unit unit, bool blocking) {
|
|
vec2<long> step;
|
|
|
|
if (unit == Unit::MM) {
|
|
vec3<double> position(x / 10.0, y / 10.0, z / 10.0);
|
|
cartesianToPolar(_step_ratio, _offset, _zero, position, step);
|
|
}
|
|
|
|
if (unit == Unit::CM) {
|
|
vec3<double> position(x, y, z);
|
|
cartesianToPolar(_step_ratio, _offset, _zero, position, step);
|
|
}
|
|
|
|
if (unit == Unit::M) {
|
|
vec3<double> position(x * 100.0, y * 100.0, z * 100.0);
|
|
cartesianToPolar(_step_ratio, _offset, _zero, position, step);
|
|
}
|
|
|
|
if (unit == Unit::STEP) {
|
|
step.x = x;
|
|
step.y = y;
|
|
}
|
|
|
|
if (unit == Unit::RAD) {
|
|
radToStep(_step_ratio, vec2<double>(x, y), step);
|
|
}
|
|
|
|
if (unit == Unit::DEG) {
|
|
radToStep(_step_ratio, vec2<double>(degToRad(x), degToRad(y)), step);
|
|
}
|
|
|
|
long maxDeltaStepX = _step_ratio.x * M_PI_2;
|
|
long maxDeltaStepY = _step_ratio.y * M_PI_2;
|
|
|
|
// force move from -90° to 90° avoiding backside,
|
|
// if move > 180° skip the full turn a start at -90°,
|
|
// same in the other direction
|
|
long stepX = constrain(step.x % maxDeltaStepX, -(maxDeltaStepX >> 2),
|
|
(maxDeltaStepX >> 2));
|
|
long stepY = constrain(step.y % maxDeltaStepY, -(maxDeltaStepY >> 2),
|
|
(maxDeltaStepY >> 2));
|
|
|
|
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);
|
|
|
|
// 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;
|
|
}
|
|
|
|
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, blocking);
|
|
|
|
_zero.x = zeroXStored;
|
|
_zero.y = zeroYStored;
|
|
|
|
_current.x = x;
|
|
_current.y = y;
|
|
_current.z = z;
|
|
|
|
return *this;
|
|
}
|
|
|
|
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, bool blocking) {
|
|
return moveTo(_current.x, y, _current.z, unit, blocking);
|
|
}
|
|
|
|
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, bool blocking) { return moveBy(x, 0, 0, unit, blocking); }
|
|
|
|
Turret &Turret::moveByY(double y, Unit unit, bool blocking) { return moveTo(0, y, 0, unit, blocking); }
|
|
|
|
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) {
|
|
vec3<double> position;
|
|
vec2<long> xy(_current.x, _current.y);
|
|
polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
|
|
x = position.x / 10;
|
|
y = position.y / 10;
|
|
z = position.z / 10;
|
|
}
|
|
|
|
if (unit == Unit::CM) {
|
|
vec3<double> position;
|
|
vec2<long> xy(_current.x, _current.y);
|
|
polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
|
|
x = position.x;
|
|
y = position.y;
|
|
z = position.z;
|
|
}
|
|
|
|
if (unit == Unit::M) {
|
|
vec3<double> position;
|
|
vec2<long> xy(_current.x, _current.y);
|
|
polarToCartesian(_step_ratio, _offset, _current, _zero, xy, position);
|
|
x = position.x * 100;
|
|
y = position.y * 100;
|
|
z = position.z * 100;
|
|
}
|
|
|
|
if (unit == Unit::STEP) {
|
|
x = _home.x;
|
|
y = _home.y;
|
|
}
|
|
|
|
if (unit == Unit::RAD) {
|
|
vec2<double> angle;
|
|
stepToRad(_step_ratio, _home, angle);
|
|
x = angle.x;
|
|
y = angle.y;
|
|
}
|
|
|
|
if (unit == Unit::DEG) {
|
|
vec2<double> angle;
|
|
stepToRad(_step_ratio, _home, angle);
|
|
x = radToDeg(angle.x);
|
|
y = radToDeg(angle.y);
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
Turret &Turret::getHome(double &x, double &y, Unit unit) {
|
|
if (unit == Unit::MM) {
|
|
vec3<double> position;
|
|
polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
|
|
x = position.x / 10;
|
|
y = position.y / 10;
|
|
}
|
|
|
|
if (unit == Unit::CM) {
|
|
vec3<double> position;
|
|
polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
|
|
x = position.x;
|
|
y = position.y;
|
|
}
|
|
|
|
if (unit == Unit::M) {
|
|
vec3<double> position;
|
|
polarToCartesian(_step_ratio, _offset, _current, _zero, _home, position);
|
|
x = position.x * 100;
|
|
y = position.y * 100;
|
|
}
|
|
|
|
if (unit == Unit::STEP) {
|
|
x = _home.x;
|
|
y = _home.y;
|
|
}
|
|
|
|
if (unit == Unit::RAD) {
|
|
vec2<double> angle;
|
|
stepToRad(_step_ratio, _home, angle);
|
|
x = angle.x;
|
|
y = angle.y;
|
|
}
|
|
|
|
if (unit == Unit::DEG) {
|
|
vec2<double> angle;
|
|
stepToRad(_step_ratio, _home, angle);
|
|
x = radToDeg(angle.x);
|
|
y = radToDeg(angle.y);
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
Turret &Turret::getZero(double &x, double &y, Unit unit) {
|
|
if ((unit == Unit::MM) || (unit == Unit::CM) || (unit == Unit::MM)) {
|
|
x = 0;
|
|
y = 0;
|
|
}
|
|
|
|
if (unit == Unit::STEP) {
|
|
x = _zero.x;
|
|
y = _zero.y;
|
|
}
|
|
|
|
if (unit == Unit::RAD) {
|
|
vec2<double> angle;
|
|
stepToRad(_step_ratio, _zero, angle);
|
|
x = angle.x;
|
|
y = angle.y;
|
|
}
|
|
|
|
if (unit == Unit::DEG) {
|
|
vec2<double> angle;
|
|
stepToRad(_step_ratio, _zero, angle);
|
|
x = radToDeg(angle.x);
|
|
y = radToDeg(angle.y);
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
Turret &Turret::laserOn() {
|
|
digitalWrite(_pin.x.laser, HIGH);
|
|
digitalWrite(_pin.y.laser, HIGH);
|
|
return *this;
|
|
}
|
|
|
|
Turret &Turret::laserOff() {
|
|
digitalWrite(_pin.x.laser, LOW);
|
|
digitalWrite(_pin.y.laser, LOW);
|
|
return *this;
|
|
}
|