X-Git-Url: https://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/900601b54fc6ad04053d978272496a3bd10303b8..fc444c1e125a6bb96d5bd76e7b54c86930e0f461:/src/modules/robot/arm_solutions/MorganSCARASolution.cpp

diff --git a/src/modules/robot/arm_solutions/MorganSCARASolution.cpp b/src/modules/robot/arm_solutions/MorganSCARASolution.cpp
index 23e59fa3..ac6b729b 100644
--- a/src/modules/robot/arm_solutions/MorganSCARASolution.cpp
+++ b/src/modules/robot/arm_solutions/MorganSCARASolution.cpp
@@ -14,12 +14,15 @@
 
 #include "libs/Config.h"
 
-#define arm1_length_checksum         CHECKSUM("arm1_length")
-#define arm2_length_checksum         CHECKSUM("arm2_length")
-#define morgan_offset_x_checksum     CHECKSUM("morgan_offset_x")
-#define morgan_offset_y_checksum     CHECKSUM("morgan_offset_y")
-#define axis_scaling_x_checksum      CHECKSUM("axis_scaling_x")
-#define axis_scaling_y_checksum      CHECKSUM("axis_scaling_y")
+#define arm1_length_checksum          CHECKSUM("arm1_length")
+#define arm2_length_checksum          CHECKSUM("arm2_length")
+#define morgan_offset_x_checksum      CHECKSUM("morgan_offset_x")
+#define morgan_offset_y_checksum      CHECKSUM("morgan_offset_y")
+#define axis_scaling_x_checksum       CHECKSUM("axis_scaling_x")
+#define axis_scaling_y_checksum       CHECKSUM("axis_scaling_y")
+#define morgan_homing_checksum        CHECKSUM("morgan_homing")
+#define morgan_undefined_min_checksum CHECKSUM("morgan_undefined_min")
+#define morgan_undefined_max_checksum CHECKSUM("morgan_undefined_max")
 
 #define SQ(x) powf(x, 2)
 #define ROUND(x, y) (roundf(x * 1e ## y) / 1e ## y)
@@ -33,7 +36,13 @@ MorganSCARASolution::MorganSCARASolution(Config* config)
     // morgan_offset_x is the x offset of bed zero position towards the SCARA tower center
     morgan_offset_x     = config->value(morgan_offset_x_checksum)->by_default(100.0f)->as_number();
     // morgan_offset_y is the y offset of bed zero position towards the SCARA tower center
-    morgan_offset_y     = config->value(morgan_offset_y_checksum)->by_default(-65.0f)->as_number();
+    morgan_offset_y     = config->value(morgan_offset_y_checksum)->by_default(-60.0f)->as_number();
+    // morgan_undefined is the ratio at which the SCARA position is undefined.
+    // required to prevent the arm moving through singularity points
+    // min: head close to tower
+    morgan_undefined_min  = config->value(morgan_undefined_min_checksum)->by_default(0.95f)->as_number();
+    // max: head on maximum reach
+    morgan_undefined_max  = config->value(morgan_undefined_max_checksum)->by_default(0.95f)->as_number();
 
     init();
 }
@@ -56,10 +65,10 @@ void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float act
           SCARA_K2,
           SCARA_theta,
           SCARA_psi;
-  
+
     SCARA_pos[X_AXIS] = cartesian_mm[X_AXIS] - this->morgan_offset_x;  //Translate cartesian to tower centric SCARA X Y
     SCARA_pos[Y_AXIS] = cartesian_mm[Y_AXIS] - this->morgan_offset_y;  // morgan_offset not to be confused with home offset. Makes the SCARA math work.
- 
+
     if (this->arm1_length == this->arm2_length)
         SCARA_C2 = (SQ(SCARA_pos[X_AXIS])+SQ(SCARA_pos[Y_AXIS])-2.0f*SQ(this->arm1_length)) / (2.0f * SQ(this->arm1_length));
     else
@@ -68,21 +77,21 @@ void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float act
     // SCARA position is undefined if abs(SCARA_C2) >=1
     // In reality abs(SCARA_C2) >0.95 is problematic.
 
-    if (SCARA_C2 > 0.95f)
-        SCARA_C2 = 0.95f;
-    else if (SCARA_C2 < -0.95f)
-        SCARA_C2 = -0.95f;
+    if (SCARA_C2 > this->morgan_undefined_max)
+        SCARA_C2 = this->morgan_undefined_max;
+    else if (SCARA_C2 < -this->morgan_undefined_min)
+        SCARA_C2 = -this->morgan_undefined_min;
+
 
-     
     SCARA_S2 = sqrtf(1.0f-SQ(SCARA_C2));
 
     SCARA_K1 = this->arm1_length+this->arm2_length*SCARA_C2;
     SCARA_K2 = this->arm2_length*SCARA_S2;
-  
+
     SCARA_theta = (atan2f(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2f(SCARA_K1, SCARA_K2))*-1.0f;    // Morgan Thomas turns Theta in oposite direction
     SCARA_psi   = atan2f(SCARA_S2,SCARA_C2);
-  
-  
+
+
     actuator_mm[ALPHA_STEPPER] = to_degrees(SCARA_theta);             // Multiply by 180/Pi  -  theta is support arm angle
     actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_theta + SCARA_psi); // Morgan kinematics (dual arm)
     //actuator_mm[BETA_STEPPER ] = to_degrees(SCARA_psi);             // real scara
@@ -92,7 +101,7 @@ void MorganSCARASolution::cartesian_to_actuator( float cartesian_mm[], float act
 
 void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) {
     // Perform forward kinematics, and place results in cartesian_mm[]
-  
+
     float y1, y2,
            actuator_rad[2];
 
@@ -101,7 +110,7 @@ void MorganSCARASolution::actuator_to_cartesian( float actuator_mm[], float cart
 
     y1 = sinf(actuator_rad[X_AXIS])*this->arm1_length;
     y2 = sinf(actuator_rad[Y_AXIS])*this->arm2_length + y1;
-  
+
     cartesian_mm[X_AXIS] = cosf(actuator_rad[X_AXIS])*this->arm1_length + cosf(actuator_rad[Y_AXIS])*this->arm2_length + this->morgan_offset_x;
     cartesian_mm[Y_AXIS] = y2 + this->morgan_offset_y;
     cartesian_mm[Z_AXIS] = actuator_mm[Z_AXIS];
@@ -132,7 +141,7 @@ bool MorganSCARASolution::set_optional(const arm_options_t& options) {
     if(i != options.end()) {
         morgan_offset_y= i->second;
     }
-    
+
     init();
     return true;
 }