diff options
author | David Crocker <dcrocker@eschertech.com> | 2021-02-16 11:59:54 +0300 |
---|---|---|
committer | David Crocker <dcrocker@eschertech.com> | 2021-02-16 11:59:54 +0300 |
commit | 41a0add99c952dadf215faf267dadcd7d653e682 (patch) | |
tree | 962d8cc3cde466727fe262a9876552b9cdea647a /src/Movement | |
parent | 22960e269c0a2a5c3b24bf300f9979d538d729a5 (diff) |
Corrected rotary delta forward kinematics
Diffstat (limited to 'src/Movement')
-rw-r--r-- | src/Movement/Kinematics/RotaryDeltaKinematics.cpp | 4 |
1 files changed, 2 insertions, 2 deletions
diff --git a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp index 584529a9..beb902e1 100644 --- a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp +++ b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp @@ -472,7 +472,7 @@ void RotaryDeltaKinematics::ForwardTransform(float Ha, float Hb, float Hc, float const float Db2 = fsquare(posBX) + fsquare(posBY) + fsquare(posBZ); const float Dc2 = fsquare(posCX) + fsquare(posCY) + fsquare(posCZ); - // Calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P. + // Calculate PQRST such that x = (Qz + S)/P, y = -(Rz + T)/P. const float P = (posBX * posCY - posAX * posCY - posCX * posBY + posAX * posBY + posCX * posAY - posBX * posAY) * 2; const float Q = ((posBY - posAY) * posCZ + (posAY - posCY) * posBZ + (posCY - posBY) * posAZ) * 2; const float R = ((posBX - posAX) * posCZ + (posAX - posCX) * posBZ + (posCX - posBX) * posAZ) * 2; @@ -495,7 +495,7 @@ void RotaryDeltaKinematics::ForwardTransform(float Ha, float Hb, float Hc, float // Substitute back for X and Y machinePos[X_AXIS] = (Q * z + S)/P; - machinePos[Y_AXIS] = (R * z + T)/P; + machinePos[Y_AXIS] = -(R * z + T)/P; machinePos[Z_AXIS] = z; if (reprap.Debug(moduleMove)) |