feat: implement getter for turret home zero and position

config.h

@@ -1,6 +1,8 @@
 #ifndef CONFIG_H
 #define CONFIG_H
 
+#include "math.h"
+
 #define BAUDRATE (9600)
 
 // Pins
@@ -26,11 +28,15 @@
 
 #define PIN_LASER (PIN_PD2)
 
-// Magic numbers
-#define STEP_RATIO_X (7020)
-#define STEP_RATIO_Y (7020)
+// Stepper ratio [step/rad]
+#define STEP_RATIO_X (6432 / M_PI)
+#define STEP_RATIO_Y (6432 / M_PI)
 
-// Offset [cm]
+// Offset between home and zero [step]
+#define ZERO_OFFSET_X (0)
+#define ZERO_OFFSET_Y (2500)
+
+// Offset between turret and lidar [cm]
 #define OFFSET_X (0)
 #define OFFSET_Y (21)
 #define OFFSET_Z (166)

maths.cpp

@@ -5,12 +5,18 @@ double degToRad(double deg) { return deg * M_PI / 180.0; }
 
 double radToDeg(double rad) { return rad * 180.0 / M_PI; }
 
-void angleToStep(Turret::StepRatio stepRatio, vec2<double> angle,
-                 vec2<long> &step) {
+void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
+               vec2<long> &step) {
   step.x = angle.x * stepRatio.x;
   step.y = angle.y * stepRatio.y;
 }
 
+void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
+               vec2<double> &angle) {
+  angle.x = step.x / stepRatio.x;
+  angle.y = step.y / stepRatio.y;
+}
+
 void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
                       vec2<long> zero, vec3<double> position,
                       vec2<long> &step) {
@@ -22,5 +28,19 @@ void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
 
   vec2<double> angle(atan2(dZ, dY) - M_PI / 2, atan2(dZ, dX) - M_PI / 2);
 
-  angleToStep(stepRatio, angle, step);
+  radToStep(stepRatio, angle, step);
+}
+
+void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
+                      vec3<long> current, vec2<long> zero, vec2<long> step,
+                      vec3<double> &position) {
+
+  vec2<double> angle;
+  stepToRad(stepRatio, step, angle);
+
+  vec2<double> dXYZ(tan(angle.x) * current.z, tan(angle.y) * current.z);
+
+  position.x = dXYZ.x - offset.x;
+  position.y = -(dXYZ.y - offset.y); // natural axis direction
+  position.z = current.z - offset.z;
 }

maths.h

@@ -5,9 +5,14 @@
 
 double degToRad(double deg);
 double radToDeg(double rad);
-void angleToStep(Turret::StepRatio stepRatio, vec2<double> angle,
-                 vec2<long> &step);
+void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
+               vec2<long> &step);
+void stepToRad(Turret::StepRatio stepRatio, vec2<long> step,
+               vec2<double> &angle);
 void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
                       vec2<long> zero, vec3<double> position, vec2<long> &step);
+void polarToCartesian(Turret::StepRatio stepRatio, Turret::Offset offset,
+                      vec3<long> current, vec2<long> zero, vec2<long> step,
+                      vec3<double> &position);
 
 #endif

turret.cpp

@@ -1,10 +1,11 @@
 #include "turret.h"
 #include "Arduino.h"
+#include "math.h"
 #include "maths.h"
 #include <kissStepper.h>
 
-Turret::Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x,
-               PinMap pin_map_y)
+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)),
@@ -16,6 +17,7 @@ Turret::Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x,
   _pin.y = pin_map_y;
   _step_ratio = step_ratio;
   _offset = offset;
+  _zero_offset = zero_offset;
 }
 
 Turret::~Turret() {
@@ -79,9 +81,8 @@ Turret &Turret::gotoHome() {
 }
 
 Turret &Turret::gotoZero() {
-  // TODO remove magic numbers
-  _stepper.x.prepareMove(_home.x + 0);
-  _stepper.y.prepareMove(_home.y + 2500);
+  _stepper.x.prepareMove(_home.x + _zero_offset.x);
+  _stepper.y.prepareMove(_home.y + _zero_offset.y);
 
   bool xStop = false;
   bool yStop = false;
@@ -131,16 +132,26 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
   }
 
   if (unit == Unit::RAD) {
-    angleToStep(_step_ratio, vec2<double>(x, y), step);
+    radToStep(_step_ratio, vec2<double>(x, y), step);
   }
 
   if (unit == Unit::DEG) {
-    angleToStep(_step_ratio, vec2<double>(degToRad(x), degToRad(y)), step);
+    radToStep(_step_ratio, vec2<double>(degToRad(x), degToRad(y)), step);
   }
 
-  // TODO min(valueI, -_homeI);
-  _stepper.x.prepareMove(_zero.x + step.x);
-  _stepper.y.prepareMove(_zero.y + step.y);
+  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));
+
+  _stepper.x.prepareMove(_zero.x + stepX);
+  _stepper.y.prepareMove(_zero.y + stepY);
 
   bool xStop = false;
   bool yStop = false;
@@ -199,17 +210,124 @@ 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
+  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, double &z, Unit unit) {
-  // TODO implement
+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, double &z, Unit unit) {
-  // TODO implement
+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.h

@@ -30,6 +30,8 @@ public:
   // y -> laser to stand
   // z -> unused
 
+  using ZeroOffset = vec2<long>;
+
   struct PinMap {
     int home;
     int direction;
@@ -38,8 +40,8 @@ public:
     int laser;
   };
 
-  Turret(StepRatio step_ratio, Offset offset, PinMap pin_map_x,
-         PinMap pin_map_y);
+  Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
+         PinMap pin_map_x, PinMap pin_map_y);
   ~Turret();
 
   Turret &init();
@@ -58,8 +60,8 @@ public:
   Turret &moveByZ(double z, Unit unit = Turret::Unit::CM);
 
   Turret &getPosition(double &x, double &y, double &z, Unit unit);
-  Turret &getHome(double &x, double &y, double &z, Unit unit);
-  Turret &getZero(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();
 
@@ -74,6 +76,7 @@ private:
   vec2<PinMap> _pin;
   StepRatio _step_ratio;
   Offset _offset;
+  ZeroOffset _zero_offset;
 };
 
 #endif

turret_debug.ino

@@ -19,8 +19,9 @@ Turret::PinMap pinY = {
 
 Turret::StepRatio stepRatio(STEP_RATIO_X, STEP_RATIO_Y);
 Turret::Offset offset(OFFSET_X, OFFSET_Y, OFFSET_Z);
+Turret::ZeroOffset zeroOffset(ZERO_OFFSET_X, ZERO_OFFSET_Y);
 
-Turret turret(stepRatio, offset, pinX, pinY);
+Turret turret(stepRatio, offset, zeroOffset, pinX, pinY);
 
 void setup() {
   turret.init();