config.h

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

maths.cpp

@@ -5,18 +5,12 @@ double degToRad(double deg) { return deg * M_PI / 180.0; }
 
 double radToDeg(double rad) { return rad * 180.0 / M_PI; }
 
-void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
+void angleToStep(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) {
@@ -28,19 +22,5 @@ void cartesianToPolar(Turret::StepRatio stepRatio, Turret::Offset offset,
 
   vec2<double> angle(atan2(dZ, dY) - M_PI / 2, atan2(dZ, dX) - M_PI / 2);
 
-  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;
+  angleToStep(stepRatio, angle, step);
 }

maths.h

@@ -5,14 +5,9 @@
 
 double degToRad(double deg);
 double radToDeg(double rad);
-void radToStep(Turret::StepRatio stepRatio, vec2<double> angle,
+void angleToStep(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,11 +1,10 @@
 #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)
+Turret::Turret(StepRatio step_ratio, Offset 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)),
@@ -17,7 +16,6 @@ Turret::Turret(StepRatio step_ratio, Offset offset, ZeroOffset zero_offset,
   _pin.y = pin_map_y;
   _step_ratio = step_ratio;
   _offset = offset;
-  _zero_offset = zero_offset;
 }
 
 Turret::~Turret() {
@@ -81,8 +79,9 @@ Turret &Turret::gotoHome() {
 }
 
 Turret &Turret::gotoZero() {
-  _stepper.x.prepareMove(_home.x + _zero_offset.x);
-  _stepper.y.prepareMove(_home.y + _zero_offset.y);
+  // TODO remove magic numbers
+  _stepper.x.prepareMove(_home.x + 0);
+  _stepper.y.prepareMove(_home.y + 2500);
 
   bool xStop = false;
   bool yStop = false;
@@ -132,26 +131,16 @@ Turret &Turret::moveTo(double x, double y, double z, Unit unit) {
   }
 
   if (unit == Unit::RAD) {
-    radToStep(_step_ratio, vec2<double>(x, y), step);
+    angleToStep(_step_ratio, vec2<double>(x, y), step);
   }
 
   if (unit == Unit::DEG) {
-    radToStep(_step_ratio, vec2<double>(degToRad(x), degToRad(y)), step);
+    angleToStep(_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));
-
-  _stepper.x.prepareMove(_zero.x + stepX);
-  _stepper.y.prepareMove(_zero.y + stepY);
+  // TODO min(valueI, -_homeI);
+  _stepper.x.prepareMove(_zero.x + step.x);
+  _stepper.y.prepareMove(_zero.y + step.y);
 
   bool xStop = false;
   bool yStop = false;
@@ -210,124 +199,17 @@ 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) {
-  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);
-  }
-
+  // TODO implement
   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);
-  }
-
+Turret &Turret::getHome(double &x, double &y, double &z, Unit unit) {
+  // TODO implement
   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);
-  }
-
+Turret &Turret::getZero(double &x, double &y, double &z, Unit unit) {
+  // TODO implement
   return *this;
 }
 

turret.h

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

turret_debug.ino

@@ -19,9 +19,8 @@ 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, zeroOffset, pinX, pinY);
+Turret turret(stepRatio, offset, pinX, pinY);
 
 void setup() {
   turret.init();