From b6b988c7092074f6abed8c1cd57867f186581b5e Mon Sep 17 00:00:00 2001 From: Vincent Date: Mon, 1 Dec 2014 21:48:37 +0100 Subject: [PATCH] Fixed actuator_to_cartesian See http://forum.conceptforge.org/viewtopic.php?f=6&t=174&p=2045#p2045 for the math --- src/modules/robot/arm_solutions/SimpsonSolution.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/modules/robot/arm_solutions/SimpsonSolution.cpp b/src/modules/robot/arm_solutions/SimpsonSolution.cpp index 1a40ad2360..638bc15c2f 100644 --- a/src/modules/robot/arm_solutions/SimpsonSolution.cpp +++ b/src/modules/robot/arm_solutions/SimpsonSolution.cpp @@ -73,13 +73,10 @@ void SimpsonSolution::cartesian_to_actuator( float cartesian_mm[], float actuato void SimpsonSolution::actuator_to_cartesian( float actuator_mm[], float cartesian_mm[] ) { - float d = SIMPSON_ARM2_X * 2.0; - float i = SIMPSON_ARM2_X; - float j = -SIMPSON_ARM2_Y * 3.0; - - float x = (SQ(actuator_mm[0]) - SQ(actuator_mm[1]) + SQ(d)) / (2.0 * d); - float y = (SQ(actuator_mm[0]) - SQ(actuator_mm[2]) - SQ(x) + SQ(x-i) + SQ(j)) / (2.0 * j); - float z = sqrtf(SQ(actuator_mm[0]) - SQ(x) - SQ(y)); + //Trilateration with the (0, 0) point in the center of the bed, for more see http://forum.conceptforge.org/viewtopic.php?f=6&t=174&p=2045#p2045 + float x = (SQ(actuator_mm[0]) - SQ(actuator_mm[1])) / (4*SIMPSON_ARM2_X); + float y = (2*x*SIMPSON_ARM2_X + SQ(actuator_mm[1]) - SQ(actuator_mm[2])) / (-2*SIMPSON_ARM2_Y + 2*SIMPSON_ARM3_Y); + float z = sqrtf(SQ(actuator_mm[2]) - SQ(x) - SQ(y - SIMPSON_ARM3_Y)); cartesian_mm[0] = ROUND(x, 4); cartesian_mm[1] = ROUND(y, 4);