technoshop.laser_turret_debug/turret.cpp

#include <kissStepper.h>
#include "config.h"
#include "turret.h"
#include "maths.h"

Turret::Turret(
  int pin_x_direction, int pin_x_pulse, int pin_x_enable, int pin_x_home,
  int pin_y_direction, int pin_y_pulse, int pin_y_enable, int pin_y_home,
  int pin_laser) : _stepperX(pin_x_direction, pin_x_pulse, pin_x_enable), _stepperY(pin_y_direction, pin_y_pulse, pin_y_enable) {

  _pin_x_direction = pin_x_direction;
  _pin_x_pulse = pin_x_pulse;
  _pin_x_enable = pin_x_enable;

  _pin_y_direction = pin_y_direction;
  _pin_y_pulse = pin_y_pulse;
  _pin_y_enable = pin_y_enable;

  _pin_x_home = pin_x_home;
  _pin_y_home = pin_y_home;
  _pin_laser = pin_laser;
}

Turret& Turret::init() {
  pinMode(_pin_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);

  _stepperX.begin();
  _stepperY.begin();

  return *this;
}

Turret& Turret::gotoHome() {
  int xStop = 0;
  int yStop = 0;

  _stepperX.prepareMove(-1000000l);
  _stepperY.prepareMove(-1000000l);

  while (true) {
    _stepperX.move();
    _stepperY.move();

    if (xStop == 2 && yStop == 2) {
      _homeX = _stepperX.getPos();
      _homeY = _stepperY.getPos();
      break;
    }

    if (xStop < 2 && digitalRead(_pin_x_home)) {
      xStop++;
      _stepperX.stop();
    }

    if (yStop < 2 && digitalRead(_pin_y_home)) {
      yStop++;
      _stepperY.stop();
    }
  }

  return *this;
}

Turret& Turret::gotoZero() {
  //TODO remove magic numbers
  _stepperX.prepareMove(_homeX + 9300);
  _stepperY.prepareMove(_homeY + 3500);

  bool xStop = false;
  bool yStop = false;

  while (true) {
    if (xStop && yStop) break;
    if (!xStop) xStop = _stepperY.move() == STATE_STOPPED;
    if (!yStop) yStop = _stepperX.move() == STATE_STOPPED;
  }

  _zeroX = _stepperX.getPos();
  _zeroY = _stepperY.getPos();

  return *this;
}

Turret& Turret::calibrate() {
  gotoHome();
  gotoZero();
  return *this;
}

Turret& Turret::moveTo(double x, double y, double z, Unit unit) {
  int stepX;
  int stepY;

  if (unit == Unit::MM) {
    cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x / 10.0, y / 10.0, z / 10.0);
  }

  if (unit == Unit::CM) {
    cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x, y, z);
  }

  if (unit == Unit::M) {
    cartesianToPolar(stepX, stepY, _zeroX, _zeroY, x * 100.0, y * 100.0, z * 100.0);
  }

  if (unit == Unit::STEP) {
    stepX = x;
    stepY = y;
  }

  if (unit == Unit::RAD) {
    angleToStep(stepX, stepY, x, y);
  }

  if (unit == Unit::DEG) {
    angleToStep(stepX, stepY, degToRad(x), degToRad(y));
  }

  //TODO min(valueI, -_homeI);
  _stepperX.prepareMove(_zeroX + stepX);
  _stepperY.prepareMove(_zeroY + stepY);

  bool xStop = false;
  bool yStop = false;

  while (true) {
    if (xStop && yStop) break;
    if (!xStop) xStop = _stepperX.move() == STATE_STOPPED;
    if (!yStop) yStop = _stepperY.move() == STATE_STOPPED;
  }
  
  _currentX = x;
  _currentY = y;
  _currentZ = z;

  return *this;
}

Turret& Turret::moveBy(double x, double y, double z, Unit unit) {
  int zeroXStored = _zeroX;
  int zeroYStored = _zeroY;

  _zeroX += _stepperX.getPos();
  _zeroY += _stepperY.getPos();

  moveTo(x, y, z, unit);

  _zeroX = zeroXStored;
  _zeroY = zeroYStored;

  _currentX = x;
  _currentY = y;
  _currentZ = z;

  return *this;
}

Turret& Turret::moveToX(double x, Unit unit) {
  return moveTo(x, _currentY, _currentZ, unit);
}

Turret& Turret::moveToY(double y, Unit unit) {
  return moveTo(_currentX, y, _currentZ, unit);
}

Turret& Turret::moveToZ(double z, Unit unit) {
  return moveTo(_currentX, _currentY, z, unit);
}

Turret& Turret::moveByX(double x, Unit unit) {
  return moveBy(x, 0, 0, unit);
}

Turret& Turret::moveByY(double y, Unit unit) {
  return moveTo(0, y, 0, unit);
}

Turret& Turret::moveByZ(double z, Unit unit) {
  return moveTo(0, 0, z, unit);
}

Turret& Turret::getPosition(double &x, double &y, double &z, Unit unit) {
  //TODO implement
  return *this;
}

Turret& Turret::getHome(double &x, double &y, double &z, Unit unit) {
  //TODO implement
  return *this;
}

Turret& Turret::getZero(double &x, double &y, double &z, Unit unit) {
  //TODO implement
  return *this;
}

Turret& Turret::laserOn() {
  digitalWrite(_pin_laser, HIGH);
  return *this;
}

Turret& Turret::laserOff() {
  digitalWrite(_pin_laser, LOW);
  return *this;
}