[Emc-commit] TRUNK emc2/src/emc/kinematics rotatekins.c,1.5,1.6

[Ben L. <cv...@cv...>]

Modified file emc2/src/emc/kinematics/rotatekins.c

Full file:
<http://cvs.linuxcnc.org/cvs/emc2/src/emc/kinematics/rotatekins.c?rev=1.6>

Difference:
<http://cvs.linuxcnc.org/cvs/emc2/src/emc/kinematics/rotatekins.c.diff?r1=1.5;r2=1.6>

Branch: TRUNK

Log:
revision 1.6
date: 2007/12/15 17:34:51;  author: fenn;  state: Exp;  lines: +7 -9
allow arbitrary rotational offset - this might as well be useful as well as informative

--- rotatekins.c	2007/07/14 21:43:22	1.5
+++ rotatekins.c	2007/12/15 17:34:51	1.6
@@ -1,7 +1,6 @@
 /********************************************************************
 * Description: rotatekins.c
-*   Simple example kinematics for coord system rotated 45 degrees
-*   in XY
+*   Simple example kinematics for a rotary table in software
 *
 *   Derived from a work by Fred Proctor & Will Shackleford
 *
@@ -14,9 +13,6 @@
 ********************************************************************/
 
 #include "rtapi_math.h"
-#ifndef M_PI_4l
-#define M_PI_4l (M_PIl/4.0)
-#endif
 #include "kinematics.h"		/* these decls */
 
 int kinematicsForward(const double *joints,
@@ -24,8 +20,9 @@
 		      const KINEMATICS_FORWARD_FLAGS * fflags,
 		      KINEMATICS_INVERSE_FLAGS * iflags)
 {
-    pos->tran.x = joints[0] * cos(-M_PI_4l) - joints[1] * sin(-M_PI_4l);
-    pos->tran.y = joints[0] * sin(-M_PI_4l) + joints[1] * cos(-M_PI_4l);
+    double c_rad = joints[5]*M_PI/180;
+    pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad);
+    pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad);
     pos->tran.z = joints[2];
     pos->a = joints[3];
     pos->b = joints[4];
@@ -42,8 +39,9 @@
 		      const KINEMATICS_INVERSE_FLAGS * iflags,
 		      KINEMATICS_FORWARD_FLAGS * fflags)
 {
-    joints[0] = pos->tran.x * cos(M_PI_4l) - pos->tran.y * sin(M_PI_4l);
-    joints[1] = pos->tran.x * sin(M_PI_4l) + pos->tran.y * cos(M_PI_4l);
+    double c_rad = pos->c*M_PI/180;
+    joints[0] = pos->tran.x * cos(c_rad) - pos->tran.y * sin(c_rad);
+    joints[1] = pos->tran.x * sin(c_rad) + pos->tran.y * cos(c_rad);
     joints[2] = pos->tran.z;
     joints[3] = pos->a;
     joints[4] = pos->b;