From 6832f350da60f82fd8e498d6a80c7f98ca92ed44 Mon Sep 17 00:00:00 2001
From: Julien Oculi <xdrz.jot.sr@gmail.com>
Date: Thu, 5 Jun 2025 10:46:12 +0200
Subject: [PATCH] feat: implement `Turret`

---
 turret.cpp | 215 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 215 insertions(+)
 create mode 100644 turret.cpp

diff --git a/turret.cpp b/turret.cpp
new file mode 100644
index 0000000..011645d
--- /dev/null
+++ b/turret.cpp
@@ -0,0 +1,215 @@
+#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;
+}