feat: implement getter for turret home zero and position

maths.cpp

@@ -11,6 +11,12 @@ void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
   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) {
@@ -24,3 +30,17 @@ void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
 
   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

@@ -7,7 +7,12 @@ double degToRad(double deg);
 double radToDeg(double rad);
 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

@@ -210,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) {
+Turret &Turret::getHome(double &x, double &y, Unit unit) {
-  // TODO implement
+  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) {
+Turret &Turret::getZero(double &x, double &y, Unit unit) {
-  // TODO implement
+  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

@@ -60,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 &getHome(double &x, double &y, Unit unit);
-  Turret &getZero(double &x, double &y, double &z, Unit unit);
+  Turret &getZero(double &x, double &y, Unit unit);
 
   Turret &calibrate();