added configable beta and gamma angle offsets to the rostock solution

diff --git a/ConfigSamples/Smoothieboard.Rostock/config b/ConfigSamples/Smoothieboard.Rostock/config
index 9ee9061..901c70b 100644 (file)
--- a/ConfigSamples/Smoothieboard.Rostock/config
+++ b/ConfigSamples/Smoothieboard.Rostock/config
@@ -11,6 +11,9 @@ alpha_steps_per_mm                           80               # Steps per mm for
 beta_steps_per_mm                            80               # Steps per mm for beta stepper
 gamma_steps_per_mm                           80               # Steps per mm for gamma stepper
 
+beta_angle                                   120.0            # this is the angle in degrees from the alpha tower to the beta tower
+gamma_angle                                  240.0            # this is the angle in degrees from the alpha tower to the gamma tower
+
 arm_length                                   430.0            # this is the length of an arm from hinge to hinge
 arm_radius                                   220.675          # this is the horiontal distance from hinge to hinge when the effector is centered
 

diff --git a/src/modules/robot/arm_solutions/RostockSolution.cpp b/src/modules/robot/arm_solutions/RostockSolution.cpp
index 0f786fd..3481d97 100644 (file)
--- a/src/modules/robot/arm_solutions/RostockSolution.cpp
+++ b/src/modules/robot/arm_solutions/RostockSolution.cpp
@@ -1,15 +1,24 @@
 #include "RostockSolution.h"
 #include <math.h>
 
+#define PIOVER180       0.01745329251994329576923690768489
+
 RostockSolution::RostockSolution(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 beta_angle   = PIOVER180 * this->config->value( beta_angle_checksum)->by_default(120.0)->as_number();
+    this->sin_beta      = sin(beta_angle);
+    this->cos_beta      = cos(beta_angle);
+    double gamma_angle  = PIOVER180 * this->config->value(gamma_angle_checksum)->by_default(240.0)->as_number();
+    this->sin_gamma     = sin(gamma_angle);
+    this->cos_gamma     = cos(gamma_angle);
+
     // arm_length is the length of the arm from hinge to hinge
-    this->arm_length         = this->config->value(rostock_arm_length_checksum)->by_default(430.0)->as_number();
+    this->arm_length         = this->config->value(arm_length_checksum)->by_default(430.0)->as_number();
     // arm_radius is the horizontal distance from hinge to hinge when the effector is centered
-    this->arm_radius         = this->config->value(rostock_arm_radius_checksum)->by_default(220.675)->as_number();
+    this->arm_radius         = this->config->value(arm_radius_checksum)->by_default(220.675)->as_number();
 
     this->arm_length_squared = pow(this->arm_length, 2);
 }
@@ -18,10 +27,10 @@ void RostockSolution::millimeters_to_steps( double millimeters[], int steps[] ){
     double rotated[3];
     steps[ALPHA_STEPPER] = lround( solve_arm( millimeters ) * this->alpha_steps_per_mm );
 
-    rotate_120(millimeters, rotated);
+    rotate_beta(millimeters, rotated);
     steps[BETA_STEPPER ] = lround( solve_arm( rotated ) * this->beta_steps_per_mm );
 
-    rotate_240(millimeters, rotated);
+    rotate_gamma(millimeters, rotated);
     steps[GAMMA_STEPPER] = lround( solve_arm( rotated ) * this->gamma_steps_per_mm );
 }
 
@@ -42,15 +51,15 @@ double RostockSolution::solve_arm( double millimeters[]) {
 #define SIN240 -SIN60
 #define COS240 -COS60
  
-void RostockSolution::rotate_120( double in[], double out[] ){
-    out[X_AXIS] = COS120 * in[X_AXIS] - SIN120 * in[Y_AXIS];
-    out[Y_AXIS] = SIN120 * in[X_AXIS] + COS120 * in[Y_AXIS];
+void RostockSolution::rotate_beta( double in[], double out[] ){
+    out[X_AXIS] = this->cos_beta * in[X_AXIS] - this->sin_beta * in[Y_AXIS];
+    out[Y_AXIS] = this->sin_beta * in[X_AXIS] + this->cos_beta * in[Y_AXIS];
     out[Z_AXIS] = in[Z_AXIS];
 }
 
-void RostockSolution::rotate_240( double in[], double out[] ){
-    out[X_AXIS] = COS240 * in[X_AXIS] - SIN240 * in[Y_AXIS];
-    out[Y_AXIS] = SIN240 * in[X_AXIS] + COS240 * in[Y_AXIS];
+void RostockSolution::rotate_gamma( double in[], double out[] ){
+    out[X_AXIS] = this->cos_gamma * in[X_AXIS] - this->sin_gamma * in[Y_AXIS];
+    out[Y_AXIS] = this->sin_gamma * in[X_AXIS] + this->cos_gamma * in[Y_AXIS];
     out[Z_AXIS] = in[Z_AXIS];
 }
 

diff --git a/src/modules/robot/arm_solutions/RostockSolution.h b/src/modules/robot/arm_solutions/RostockSolution.h
index e9af21e..2db3cd5 100644 (file)
--- a/src/modules/robot/arm_solutions/RostockSolution.h
+++ b/src/modules/robot/arm_solutions/RostockSolution.h
@@ -11,8 +11,11 @@
 #define beta_steps_per_mm_checksum  CHECKSUM("beta_steps_per_mm")
 #define gamma_steps_per_mm_checksum CHECKSUM("gamma_steps_per_mm")
 
-#define rostock_arm_length_checksum         CHECKSUM("arm_length")
-#define rostock_arm_radius_checksum         CHECKSUM("arm_radius")
+#define arm_length_checksum         CHECKSUM("arm_length")
+#define arm_radius_checksum         CHECKSUM("arm_radius")
+
+#define beta_angle_checksum         CHECKSUM("beta_angle")
+#define gamma_angle_checksum        CHECKSUM("gamma_angle")
 
 class RostockSolution : public BaseSolution {
     public:
@@ -24,8 +27,8 @@ class RostockSolution : public BaseSolution {
         void get_steps_per_millimeter( double steps[] );
 
         double solve_arm( double millimeters[] );
-        void rotate_120( double coords[], double out[] );
-        void rotate_240( double coords[], double out[] );
+        void rotate_beta( double coords[], double out[] );
+        void rotate_gamma( double coords[], double out[] );
 
         Config* config;
         double alpha_steps_per_mm;
@@ -35,6 +38,11 @@ class RostockSolution : public BaseSolution {
         double arm_length;
         double arm_radius;
         double arm_length_squared;
+
+        double sin_beta;
+        double cos_beta;
+        double sin_gamma;
+        double cos_gamma;
 };