From bf2e38b61f89f3696932fcd217e7c61035c54a03 Mon Sep 17 00:00:00 2001 From: Andrey Demenev Date: Sun, 25 Feb 2024 14:37:24 +0400 Subject: [PATCH 1/3] MP SCARA kinematic --- uCNC/src/hal/kinematics/README.md | 30 +++++++++++++++++++++++ uCNC/src/hal/kinematics/kinematic_scara.c | 12 ++++++--- 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/uCNC/src/hal/kinematics/README.md b/uCNC/src/hal/kinematics/README.md index 59928ac90..a2fdfe0f6 100644 --- a/uCNC/src/hal/kinematics/README.md +++ b/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, $101, $110, $111, $120 and $121 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. diff --git a/uCNC/src/hal/kinematics/kinematic_scara.c b/uCNC/src/hal/kinematics/kinematic_scara.c index 5ae29b415..813f6e05b 100644 --- a/uCNC/src/hal/kinematics/kinematic_scara.c +++ b/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; @@ -173,7 +179,7 @@ void kinematics_apply_reverse_transform(float *axis) bool kinematics_check_boundaries(float *axis) { - if (!g_settings.soft_limits_enabled || cnc_get_exec_state(EXEC_HOMING)) + if (/*!g_settings.soft_limits_enabled || */cnc_get_exec_state(EXEC_HOMING)) { return true; } From f9873b14fb796846136e03828b2614dfb698174e Mon Sep 17 00:00:00 2001 From: Andrey Demenev Date: Sun, 25 Feb 2024 14:40:39 +0400 Subject: [PATCH 2/3] remove debug statement --- uCNC/src/hal/kinematics/kinematic_scara.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uCNC/src/hal/kinematics/kinematic_scara.c b/uCNC/src/hal/kinematics/kinematic_scara.c index 813f6e05b..87662327b 100644 --- a/uCNC/src/hal/kinematics/kinematic_scara.c +++ b/uCNC/src/hal/kinematics/kinematic_scara.c @@ -179,7 +179,7 @@ void kinematics_apply_reverse_transform(float *axis) bool kinematics_check_boundaries(float *axis) { - if (/*!g_settings.soft_limits_enabled || */cnc_get_exec_state(EXEC_HOMING)) + if (!g_settings.soft_limits_enabled || cnc_get_exec_state(EXEC_HOMING)) { return true; } From ca912b28a7cbaee0cf75280329f9dad4a38fdb69 Mon Sep 17 00:00:00 2001 From: Andrey Demenev Date: Sun, 25 Feb 2024 14:44:06 +0400 Subject: [PATCH 3/3] clarify settings --- uCNC/src/hal/kinematics/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uCNC/src/hal/kinematics/README.md b/uCNC/src/hal/kinematics/README.md index a2fdfe0f6..b48c0bc27 100644 --- a/uCNC/src/hal/kinematics/README.md +++ b/uCNC/src/hal/kinematics/README.md @@ -78,7 +78,7 @@ In addition to the standard configurations you need to set the following extra s | $106 | SCARA arm length, mm | $107 | SCARA forearm length, mm -Settings $100, $101, $110, $111, $120 and $121 have a different meaning, they +Settings $100 and $101 have a different meaning, they use revolution instead of mm.