Merge pull request #645 from ademenev/master

MP SCARA kinematic

uCNC/src/hal/kinematics/README.md

@@ -52,6 +52,36 @@ In addition to the standard configurations you need to set 5 extra settings:
 
 For more information please head to the [µCNC wiki page](https://github.com/Paciente8159/uCNC/wiki)
 
+### SCARA
+
+SCARA kinematic is supported. µCNC supports 2 implementations of SCARA kinematic:
+* standard serial SCARA. The shoulder motor is fixed to the base and rotates
+  the arm. The elbow motor is fixed to the arm and rotates the forearm. When the
+	shoulder motor rotates and the elbow motor is stopped, the angle between the
+	arm and the forearm is fixed, and the angle of the arm changes in relation
+	to the base.
+* serial SCARA implementation with fixed motors, known as MP SCARA (mostly
+  printed SCARA). Both shoulder and  elbow motors are fixed to the base. A
+	system of pulleys and closed belts is used to transfer motion from the elbow
+	motor to elbow joint. When the shoulder motor rotates and the elbow motor is
+	stopped, the angle of the arm changes in relation	to the base, and the angle
+	between the forearm and the base is fixed.
+
+To use MP SCARA kinematic, define MP_SCARA.
+
+In addition to the standard configurations you need to set the following extra settings:
+
+| Setting | Description |
+| --- | --- |
+| $28        | SCARA arm homing angle, degrees
+| $29        | SCARA forearm homing angle, degrees
+| $106       | SCARA arm length, mm
+| $107       | SCARA forearm length, mm
+
+Settings $100 and $101 have a different meaning, they
+use revolution instead of mm.
+
+
 ## The kinematics HAL
 This HAL is manages the way the linear actuators and the 3D Cartesian space axis relate to each other. 
    * Converts linear actuators (steppers) in to X,Y,Z,A,B,C coordinates and back.

uCNC/src/hal/kinematics/kinematic_scara.c

@@ -51,10 +51,14 @@ void kinematics_apply_inverse(float *axis, int32_t *steps)
 
 	float distance = (axis[AXIS_X] * axis[AXIS_X] + axis[AXIS_Y] * axis[AXIS_Y] - arm * arm - forearm * forearm) / (2.0f * arm * forearm);
 	float angle2 = acosf(distance);
-
-	steps[1] = (int32_t)roundf(angle2 * DOUBLE_PI_INV * g_settings.step_per_mm[1]);
 	float angle1 = atan2f(axis[AXIS_Y], axis[AXIS_X]) - atan2f(forearm * sin(angle2), (arm + forearm * distance));
+
+#ifdef MP_SCARA
+	angle2 += angle1;
+#endif
+
 	steps[0] = (int32_t)roundf(angle1 * DOUBLE_PI_INV * g_settings.step_per_mm[0]);
+	steps[1] = (int32_t)roundf(angle2 * DOUBLE_PI_INV * g_settings.step_per_mm[1]);
 
 #if AXIS_COUNT > 2
 	for (uint8_t i = 2; i < AXIS_COUNT; i++)
@@ -72,7 +76,9 @@ void kinematics_apply_forward(int32_t *steps, float *axis)
 	float joint1 = steps[0] * scara_arm_angle_fact[0];
 	float joint2 = steps[1] * scara_arm_angle_fact[1];
 
+#ifndef MP_SCARA
 	joint2 += joint1;
+#endif
 
 	float arm = g_settings.scara_arm_length;
 	float forearm = g_settings.scara_forearm_length;