initial rotated cartesian solution implementation

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 365e81a..8127656 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -59,6 +59,9 @@ void Robot::on_config_reload(void* argument){
                // place holder for now
                this->arm_solution = new RostockSolution(this->kernel->config);
 
+    }else if(solution_checksum == rotatable_cartesian_checksum) {
+        this->arm_solution = new RotatableCartesianSolution(this->kernel->config);
+
        }else if(solution_checksum == cartesian_checksum) {
                this->arm_solution = new CartesianSolution(this->kernel->config);
 

diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h
index 06a4bd6..086da9b 100644 (file)
--- a/src/modules/robot/Robot.h
+++ b/src/modules/robot/Robot.h
@@ -29,6 +29,7 @@ using std::string;
 #define z_axis_max_speed_checksum              CHECKSUM("z_axis_max_speed")
 #define arm_solution_checksum                  CHECKSUM("arm_solution")
 #define cartesian_checksum                     CHECKSUM("cartesian")
+#define rotatable_cartesian_checksum           CHECKSUM("rotatable_cartesian")
 #define rostock_checksum                       CHECKSUM("rostock")
 #define delta_checksum                         CHECKSUM("delta")
 

diff --git a/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp b/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp
new file mode 100644 (file)
index 0000000..3bc24d7
--- /dev/null
+++ b/src/modules/robot/arm_solutions/RotatableCartesianSolution.cpp
@@ -0,0 +1,53 @@
+#include "RotatableCartesianSolution.h"
+#include <math.h>
+
+#define PIOVER180       0.01745329251994329576923690768489
+
+RotatableCartesianSolution::RotatableCartesianSolution(Config* passed_config) : config(passed_config){
+    this->alpha_steps_per_mm = this->config->value(alpha_steps_per_mm_checksum)->as_number();
+    this->beta_steps_per_mm  = this->config->value( beta_steps_per_mm_checksum)->as_number();
+    this->gamma_steps_per_mm = this->config->value(gamma_steps_per_mm_checksum)->as_number();
+
+    double alpha_angle  = PIOVER180 * this->config->value(alpha_angle_checksum)->by_default(0.0)->as_number();
+    this->sin_alpha     = sin(alpha_angle);
+    this->cos_alpha     = cos(alpha_angle);
+}
+
+void RotatableCartesianSolution::millimeters_to_steps( double millimeters[], int steps[] ){
+    double rotated[3];
+    rotate( millimeters, rotated, this->sin_alpha, this->cos_alpha );
+
+    steps[ALPHA_STEPPER] = lround( rotated[X_AXIS] * this->alpha_steps_per_mm );
+    steps[BETA_STEPPER ] = lround( rotated[Y_AXIS] * this->beta_steps_per_mm );
+    steps[GAMMA_STEPPER] = lround( rotated[Z_AXIS] * this->gamma_steps_per_mm );
+}
+
+void RotatableCartesianSolution::steps_to_millimeters( int steps[], double millimeters[] ){
+    double rotated[3];
+
+    rotated[ALPHA_STEPPER] = steps[X_AXIS] / this->alpha_steps_per_mm;
+    rotated[BETA_STEPPER ] = steps[Y_AXIS] / this->beta_steps_per_mm;
+    rotated[GAMMA_STEPPER] = steps[Z_AXIS] / this->gamma_steps_per_mm;
+
+    rotate( rotated, millimeters, - this->sin_alpha, this->cos_alpha );
+}
+
+void RotatableCartesianSolution::rotate(double in[], double out[], double sin, double cos ){
+    out[X_AXIS] = cos * in[X_AXIS] - sin * in[Y_AXIS];
+    out[Y_AXIS] = sin * in[X_AXIS] + cos * in[Y_AXIS];
+    out[Z_AXIS] = in[Z_AXIS];
+}
+
+void RotatableCartesianSolution::set_steps_per_millimeter( double steps[] )
+{
+    this->alpha_steps_per_mm = steps[0];
+    this->beta_steps_per_mm  = steps[1];
+    this->gamma_steps_per_mm = steps[2];
+}
+
+void RotatableCartesianSolution::get_steps_per_millimeter( double steps[] )
+{
+    steps[0] = this->alpha_steps_per_mm;
+    steps[1] = this->beta_steps_per_mm;
+    steps[2] = this->gamma_steps_per_mm;
+}

diff --git a/src/modules/robot/arm_solutions/RotatableCartesianSolution.h b/src/modules/robot/arm_solutions/RotatableCartesianSolution.h
new file mode 100644 (file)
index 0000000..9ef8f2f
--- /dev/null
+++ b/src/modules/robot/arm_solutions/RotatableCartesianSolution.h
@@ -0,0 +1,38 @@
+#ifndef ROTATABLECARTESIANSOLUTION_H
+#define ROTATABLECARTESIANSOLUTION_H
+#include "libs/Module.h"
+#include "libs/Kernel.h"
+#include "BaseSolution.h"
+#include "libs/nuts_bolts.h"
+
+#include "libs/Config.h"
+
+#define alpha_steps_per_mm_checksum CHECKSUM("alpha_steps_per_mm")
+#define beta_steps_per_mm_checksum  CHECKSUM("beta_steps_per_mm")
+#define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
+
+#define alpha_angle_checksum        CHECKSUM("alpha_angle")
+
+class RotatableCartesianSolution : public BaseSolution {
+    public:
+        RotatableCartesianSolution(Config* passed_config);
+        void millimeters_to_steps( double millimeters[], int steps[] );
+        void steps_to_millimeters( int steps[], double millimeters[] );
+
+        void set_steps_per_millimeter( double steps[] );
+        void get_steps_per_millimeter( double steps[] );
+
+        void rotate( double in[], double out[], double sin, double cos );
+
+        Config* config;
+        double alpha_steps_per_mm;
+        double beta_steps_per_mm;
+        double gamma_steps_per_mm;
+
+        double sin_alpha;
+        double cos_alpha;
+};
+
+
+#endif // ROTATABLECARTESIANSOLUTION_H
+