#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)
// 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();
}
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
// 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
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];
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];
if(i != options.end()) {
morgan_offset_y= i->second;
}
-
+
init();
return true;
}