diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index 29760d6d795..c63a5e211c1 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -175,20 +175,20 @@ the adjustment values should be added to the old values instead of replacing the """; see_also "kins(9)"; -pin out bit dummy=1; // halcompile requires at least one pin +pin out bool dummy=1; // halcompile requires at least one pin license "GPL"; ;; static struct haldata { - hal_float_t C_xx; - hal_float_t C_xy; - hal_float_t C_xz; - hal_float_t C_yx; - hal_float_t C_yy; - hal_float_t C_yz; - hal_float_t C_zx; - hal_float_t C_zy; - hal_float_t C_zz; + hal_real_t C_xx; + hal_real_t C_xy; + hal_real_t C_xz; + hal_real_t C_yx; + hal_real_t C_yy; + hal_real_t C_yz; + hal_real_t C_zx; + hal_real_t C_zy; + hal_real_t C_zz; } *haldata; static int matrixkins_setup(void) { @@ -203,20 +203,15 @@ static int matrixkins_setup(void) { haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; - res |= hal_param_float_new("matrixkins.C_xx", HAL_RW, &haldata->C_xx, comp_id); - res |= hal_param_float_new("matrixkins.C_xy", HAL_RW, &haldata->C_xy, comp_id); - res |= hal_param_float_new("matrixkins.C_xz", HAL_RW, &haldata->C_xz, comp_id); - res |= hal_param_float_new("matrixkins.C_yx", HAL_RW, &haldata->C_yx, comp_id); - res |= hal_param_float_new("matrixkins.C_yy", HAL_RW, &haldata->C_yy, comp_id); - res |= hal_param_float_new("matrixkins.C_yz", HAL_RW, &haldata->C_yz, comp_id); - res |= hal_param_float_new("matrixkins.C_zx", HAL_RW, &haldata->C_zx, comp_id); - res |= hal_param_float_new("matrixkins.C_zy", HAL_RW, &haldata->C_zy, comp_id); - res |= hal_param_float_new("matrixkins.C_zz", HAL_RW, &haldata->C_zz, comp_id); - - haldata->C_xx = haldata->C_yy = haldata->C_zz = 1.0; - haldata->C_xy = haldata->C_xz = 0.0; - haldata->C_yx = haldata->C_yz = 0.0; - haldata->C_zx = haldata->C_zy = 0.0; + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xx, 1.0, "matrixkins.C_xx"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xy, 0.0, "matrixkins.C_xy"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xz, 0.0, "matrixkins.C_xz"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yx, 0.0, "matrixkins.C_yx"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yy, 1.0, "matrixkins.C_yy"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yz, 0.0, "matrixkins.C_yz"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zx, 0.0, "matrixkins.C_zx"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zy, 0.0, "matrixkins.C_zy"); + res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zz, 1.0, "matrixkins.C_zz"); if (res) goto error; @@ -259,20 +254,20 @@ int kinematicsForward(const double *j, // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices - double a = haldata->C_xx; - double b = haldata->C_xy; - double c = haldata->C_xz; - double d = haldata->C_yx; - double e = haldata->C_yy; - double f = haldata->C_yz; - double g = haldata->C_zx; - double h = haldata->C_zy; - double i = haldata->C_zz; - - double det = a * (e * i - f * h) + rtapi_real a = hal_get_real(haldata->C_xx); + rtapi_real b = hal_get_real(haldata->C_xy); + rtapi_real c = hal_get_real(haldata->C_xz); + rtapi_real d = hal_get_real(haldata->C_yx); + rtapi_real e = hal_get_real(haldata->C_yy); + rtapi_real f = hal_get_real(haldata->C_yz); + rtapi_real g = hal_get_real(haldata->C_zx); + rtapi_real h = hal_get_real(haldata->C_zy); + rtapi_real i = hal_get_real(haldata->C_zz); + + rtapi_real det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g); - double invdet = 1.0 / det; + rtapi_real invdet = 1.0 / det; // Apply inverse matrix transform to the 3 cartesian coordinates pos->tran.x = invdet * ( (e * i - f * h) * j[0] @@ -305,15 +300,15 @@ int kinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - double a = haldata->C_xx; - double b = haldata->C_xy; - double c = haldata->C_xz; - double d = haldata->C_yx; - double e = haldata->C_yy; - double f = haldata->C_yz; - double g = haldata->C_zx; - double h = haldata->C_zy; - double i = haldata->C_zz; + rtapi_real a = hal_get_real(haldata->C_xx); + rtapi_real b = hal_get_real(haldata->C_xy); + rtapi_real c = hal_get_real(haldata->C_xz); + rtapi_real d = hal_get_real(haldata->C_yx); + rtapi_real e = hal_get_real(haldata->C_yy); + rtapi_real f = hal_get_real(haldata->C_yz); + rtapi_real g = hal_get_real(haldata->C_zx); + rtapi_real h = hal_get_real(haldata->C_zy); + rtapi_real i = hal_get_real(haldata->C_zz); // Apply matrix transform to the 3 cartesian coordinates j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c; diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 2a9ae56df59..1350d219c61 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -28,7 +28,7 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. // Use the *_setup() function for pins and params used by kinematics. -pin out s32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; +pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; function fdemo; license "GPL"; @@ -40,15 +40,15 @@ author "David Mueller"; static struct haldata { // Example pin pointers: - hal_u32_t *in; - hal_u32_t *out; + hal_uint_t in; + hal_uint_t out; // Example parameters: - hal_float_t param_rw; - hal_float_t param_ro; + //hal_real_t param_rw; + //hal_real_t param_ro; //Declare hal pin pointers used for switchable kinematics - hal_bit_t *kinstype_is_0; - hal_bit_t *kinstype_is_1; + hal_bool_t kinstype_is_0; + hal_bool_t kinstype_is_1; } *haldata; FUNCTION(fdemo) { @@ -57,7 +57,7 @@ FUNCTION(fdemo) { if (fpin == 0) { rtapi_print("fdemo function added to thread\n"); } - fpin++; + fpin_set(fpin + 1); } static int millturn_setup(void) { @@ -69,23 +69,21 @@ static int millturn_setup(void) { // set unready to allow creation of pins if (hal_set_unready(comp_id)) goto error; - haldata = hal_malloc(sizeof(struct haldata)); + haldata = hal_malloc(sizeof(*haldata)); if (!haldata) goto error; // hal pin examples: - res += hal_pin_u32_newf(HAL_IN ,&(haldata->in) ,comp_id,"%s.in" ,HAL_PREFIX); - res += hal_pin_u32_newf(HAL_OUT,&(haldata->out),comp_id,"%s.out",HAL_PREFIX); + res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->in, 0, "%s.in", HAL_PREFIX); + res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->out, 0, "%s.out", HAL_PREFIX); // hal parameter examples: - res += hal_param_float_newf(HAL_RW, &haldata->param_rw,comp_id,"%s.param-rw",HAL_PREFIX); - res += hal_param_float_newf(HAL_RO, &haldata->param_ro,comp_id,"%s.param-ro",HAL_PREFIX); + //res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); + //res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); // hal pins required for switchable kinematics: - res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(haldata->kinstype_is_0), comp_id); - res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(haldata->kinstype_is_1), comp_id); - - // define default kinematics at startup for switchable kinematics - *haldata->kinstype_is_0 = 1; //default at startup -> mill configuration - *haldata->kinstype_is_1 = 0; //-> turn configuration + //default at startup -> mill configuration + //-> turn configuration + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); if (res) goto error; hal_ready(comp_id); @@ -103,7 +101,7 @@ EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); -static hal_u32_t switchkins_type; +static rtapi_u32 switchkins_type; int kinematicsSwitchable() {return 1;} @@ -115,19 +113,19 @@ int kinematicsSwitch(int new_switchkins_type) switch (switchkins_type) { case 0: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE0\n"); - *haldata->kinstype_is_0 = 1; - *haldata->kinstype_is_1 = 0; + hal_set_bool(haldata->kinstype_is_0, 1); + hal_set_bool(haldata->kinstype_is_1, 0); break; case 1: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n"); - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_1 = 1; + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_1, 1); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "kinematicsSwitch:BAD VALUE <%d>\n", switchkins_type); - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_0 = 0; + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_0, 0); return -1; // FAIL } return 0; // ok @@ -175,7 +173,7 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (*haldata->in && !is_ready && !gave_msg) { + if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { rtapi_print_msg(RTAPI_MSG_ERR, "%s the 'in' pin not echoed until Inverse called\n", __FILE__); @@ -213,8 +211,8 @@ int kinematicsInverse(const EmcPose * pos, } //example hal pin update (homing reqd before kinematicsInverse) - *haldata->out = *haldata->in; //dereference - //read from param example: *haldata->out = haldata->param_rw; + hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference + //read from param example: *haldata->out = hal_get_real(haldata->param_rw); return 0; } // kinematicsInverse() diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index 427e98fdb34..85e8800c02a 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -59,7 +59,7 @@ change all instances of `userkins` to `mykins`. """; // The fpin pin is not accessible in kinematics functions. // Use the *_setup() function for pins and params used by kinematics. -pin out s32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; +pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; function fdemo; license "GPL"; @@ -71,17 +71,17 @@ author "Dewey Garrett"; static struct haldata { // Example pin pointers - hal_u32_t *in; - hal_u32_t *out; + hal_uint_t in; + hal_uint_t out; // Example parameters - hal_float_t param_rw; - hal_float_t param_ro; + hal_real_t param_rw; + hal_real_t param_ro; } *haldata; // hal pin/param types: -// hal_bit_t bit -// hal_u32_t unsigned 32bit integer -// hal_s32_t signed 32bit integer -// hal_float_t float (double precision) +// hal_bool_t boolean bit +// hal_uint_t unsigned integer +// hal_sint_t signed integer +// hal_real_t floating point (double precision) FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -89,7 +89,7 @@ FUNCTION(fdemo) { if (fpin == 0) { rtapi_print("fdemo function added to thread\n"); } - fpin++; + fpin_set(fpin + 1); } static int userkins_setup(void) { @@ -105,12 +105,12 @@ static int userkins_setup(void) { if (!haldata) goto error; // hal pin examples: - res += hal_pin_u32_newf(HAL_IN ,&(haldata->in) ,comp_id,"%s.in" ,HAL_PREFIX); - res += hal_pin_u32_newf(HAL_OUT,&(haldata->out),comp_id,"%s.out",HAL_PREFIX); + res += hal_pin_new_ui32(comp_id, HAL_IN , &(haldata->in) , 0, "%s.in" , HAL_PREFIX); + res += hal_pin_new_ui32(comp_id, HAL_OUT, &(haldata->out), 0, "%s.out", HAL_PREFIX); // hal parameter examples: - res += hal_param_float_newf(HAL_RW, &haldata->param_rw,comp_id,"%s.param-rw",HAL_PREFIX); - res += hal_param_float_newf(HAL_RO, &haldata->param_ro,comp_id,"%s.param-ro",HAL_PREFIX); + res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); + res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); if (res) goto error; hal_ready(comp_id); @@ -160,7 +160,7 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (*haldata->in && !is_ready && !gave_msg) { + if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { rtapi_print_msg(RTAPI_MSG_ERR, "%s The 'in' pin not echoed until Inverse called\n", __FILE__); @@ -190,8 +190,8 @@ int kinematicsInverse(const EmcPose * pos, j[2] = pos->tran.z; // joint 2 //example hal pin update (homing reqd before kinematicsInverse) - *haldata->out = *haldata->in; //dereference - //read from param example: *haldata->out = haldata->param_rw; + hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference + //read from param example: hal_set_ui32(haldata->out, hal_get_real(haldata->param_rw)); return 0; } // kinematicsInverse() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 210606c95c9..ea9e39839a0 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -30,7 +30,7 @@ chapter (docs/src/motion/switchkins.txt) """; -pin out s32 dummy=0"one pin needed to satisfy halcompile requirement"; +pin out si32 dummy=0"one pin needed to satisfy halcompile requirement"; license "GPL"; author "David Mueller"; @@ -42,16 +42,16 @@ author "David Mueller"; static struct haldata { // Declare hal pin pointers used for xyzab_tdr kinematics: - hal_float_t *tool_offset_z; - hal_float_t *x_offset; - hal_float_t *z_offset; - hal_float_t *x_rot_point; - hal_float_t *y_rot_point; - hal_float_t *z_rot_point; + hal_real_t tool_offset_z; + hal_real_t x_offset; + hal_real_t z_offset; + hal_real_t x_rot_point; + hal_real_t y_rot_point; + hal_real_t z_rot_point; //Declare hal pin pointers used for switchable kinematics - hal_bit_t *kinstype_is_0; - hal_bit_t *kinstype_is_1; + hal_bool_t kinstype_is_0; + hal_bool_t kinstype_is_1; } *haldata; static int xyzab_tdr_setup(void) { @@ -62,24 +62,22 @@ static int xyzab_tdr_setup(void) { // set unready to allow creation of pins if (hal_set_unready(comp_id)) goto error; - haldata = hal_malloc(sizeof(struct haldata)); + haldata = hal_malloc(sizeof(*haldata)); if (!haldata) goto error; // hal pins required for xyzab_tdr kinematics: - res += hal_pin_float_newf(HAL_IN ,&(haldata->tool_offset_z) ,comp_id,"%s.tool-offset-z" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->x_offset) ,comp_id,"%s.x-offset" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->z_offset) ,comp_id,"%s.z-offset" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&haldata->x_rot_point ,comp_id,"%s.x-rot-point" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&haldata->y_rot_point ,comp_id,"%s.y-rot-point" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&haldata->z_rot_point ,comp_id,"%s.z-rot-point" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_offset, 0.0, "%s.z-offset", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_point, 0.0, "%s.x-rot-point", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_point, 0.0, "%s.y-rot-point", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_point, 0.0, "%s.z-rot-point", HAL_PREFIX); // hal pins required for switchable kinematics: - res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(haldata->kinstype_is_0), comp_id); - res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(haldata->kinstype_is_1), comp_id); - - // define default kinematics at startup for switchable kinematics - *haldata->kinstype_is_0 = 1; //default at startup -> identity kinematics - *haldata->kinstype_is_1 = 0; //-> XYZAB TCP + //default at startup -> identity kinematics + //-> XYZAB TCP + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); if (res) goto error; hal_ready(comp_id); @@ -97,7 +95,7 @@ EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); -static hal_u32_t switchkins_type; +static rtapi_u32 switchkins_type; int kinematicsSwitchable() {return 1;} @@ -109,19 +107,19 @@ int kinematicsSwitch(int new_switchkins_type) switch (switchkins_type) { case 0: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE0\n"); - *haldata->kinstype_is_0 = 1; - *haldata->kinstype_is_1 = 0; + hal_set_bool(haldata->kinstype_is_0, 1); + hal_set_bool(haldata->kinstype_is_1, 0); break; case 1: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n"); - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_1 = 1; + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_1, 1); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "kinematicsSwitch:BAD VALUE <%d>\n", switchkins_type); - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_0 = 0; + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_0, 0); return -1; // FAIL } return 0; // ok @@ -145,12 +143,12 @@ int kinematicsForward(const double *j, { (void)fflags; (void)iflags; - double x_rot_point = *(haldata->x_rot_point); - double y_rot_point = *(haldata->y_rot_point); - double z_rot_point = *(haldata->z_rot_point); + double x_rot_point = hal_get_real(haldata->x_rot_point); + double y_rot_point = hal_get_real(haldata->y_rot_point); + double z_rot_point = hal_get_real(haldata->z_rot_point); - double dz = *(haldata->z_offset); - double dt = *(haldata->tool_offset_z); + double dz = hal_get_real(haldata->z_offset); + double dt = hal_get_real(haldata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -209,13 +207,13 @@ int kinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - double x_rot_point = *(haldata->x_rot_point); - double y_rot_point = *(haldata->y_rot_point); - double z_rot_point = *(haldata->z_rot_point); + double x_rot_point = hal_get_real(haldata->x_rot_point); + double y_rot_point = hal_get_real(haldata->y_rot_point); + double z_rot_point = hal_get_real(haldata->z_rot_point); - double dx = *(haldata->x_offset); - double dz = *(haldata->z_offset); - double dt = *(haldata->tool_offset_z); + double dx = hal_get_real(haldata->x_offset); + double dz = hal_get_real(haldata->z_offset); + double dt = hal_get_real(haldata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 3f0ed80da70..dfbe4466ace 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -5,11 +5,8 @@ description FIXME """; -// The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. -pin out s32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; +pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; -function fdemo; license "GPL"; author "David Mueller"; @@ -20,53 +17,36 @@ author "David Mueller"; static struct haldata { - // Example pin pointers: - hal_u32_t *in; - hal_u32_t *out; // these should be parameters really but we want to be able to // change them for demonstration purposes - hal_float_t *y_pivot; - hal_float_t *z_pivot; - hal_float_t *x_offset; - hal_float_t *y_offset; - hal_float_t *y_rot_axis; - hal_float_t *z_rot_axis; - hal_float_t *pre_rot; - hal_float_t *nut_angle; - hal_float_t *prim_angle; - hal_float_t *sec_angle; - - - // Example parameters: - // hal_float_t param_rw; - // hal_float_t param_ro; + hal_real_t y_pivot; + hal_real_t z_pivot; + hal_real_t x_offset; + hal_real_t y_offset; + hal_real_t y_rot_axis; + hal_real_t z_rot_axis; + hal_real_t pre_rot; + hal_real_t nut_angle; + hal_real_t prim_angle; + hal_real_t sec_angle; // Parameters used for xyzacb_trsrn kinematics: // Declare hal pin pointers used for xyzacb_trsrn kinematics: - hal_float_t *tool_offset_z; + hal_real_t tool_offset_z; //Declare hal pin pointers used for switchable kinematics - hal_bit_t *kinstype_is_0; - hal_bit_t *kinstype_is_1; - hal_bit_t *kinstype_is_2; + hal_bool_t kinstype_is_0; + hal_bool_t kinstype_is_1; + hal_bool_t kinstype_is_2; } *haldata; -FUNCTION(fdemo) { - // This function can be added to a thread (addf) for - // purposes not related to the kinematics functions. - if (fpin == 0) { - rtapi_print("fdemo function added to thread\n"); - } - fpin++; -} - static int xyzacb_trsrn_setup(void) { #define HAL_PREFIX "xyzacb_trsrn_kins" int res=0; - // inbherit comp_id from rtapi_main() + // inherit comp_id from rtapi_main() if (comp_id < 0) goto error; // set unready to allow creation of pins if (hal_set_unready(comp_id)) goto error; @@ -74,37 +54,26 @@ static int xyzacb_trsrn_setup(void) { haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; - // hal pin examples: - res += hal_pin_u32_newf(HAL_IN ,&(haldata->in) ,comp_id,"%s.in" ,HAL_PREFIX); - res += hal_pin_u32_newf(HAL_OUT,&(haldata->out),comp_id,"%s.out",HAL_PREFIX); - - // hal parameter examples: - // res += hal_param_float_newf(HAL_RW, &haldata->param_rw ,comp_id,"%s.param-rw",HAL_PREFIX); - // res += hal_param_float_newf(HAL_RO, &haldata->param_ro ,comp_id,"%s.param-ro",HAL_PREFIX); - // hal pins required for xyzacb_trsrn kinematics: - res += hal_pin_float_newf(HAL_IN ,&(haldata->tool_offset_z) - ,comp_id,"%s.tool-offset-z" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->y_pivot) ,comp_id,"%s.y-pivot" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->z_pivot) ,comp_id,"%s.z-pivot" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->x_offset) ,comp_id,"%s.x-offset" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->y_offset) ,comp_id,"%s.y-offset" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->y_rot_axis) ,comp_id,"%s.y-rot-axis" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->z_rot_axis) ,comp_id,"%s.z-rot-axis" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->pre_rot) ,comp_id,"%s.pre-rot" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->nut_angle) ,comp_id,"%s.nut-angle" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->prim_angle) ,comp_id,"%s.primary-angle" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->sec_angle) ,comp_id,"%s.secondary-angle" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,HAL_PREFIX); // hal pins required for switchable kinematics: - res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(haldata->kinstype_is_0), comp_id); - res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(haldata->kinstype_is_1), comp_id); - res += hal_pin_bit_new("kinstype.is-2", HAL_OUT, &(haldata->kinstype_is_2), comp_id); - - // define default kinematics at startup for switchable kinematics - *haldata->kinstype_is_0 = 1; //default at startup -> identity kinematics - *haldata->kinstype_is_1 = 0; //-> xyzabc TCP - *haldata->kinstype_is_2 = 0; //-> xyzabc TOOL + //default at startup -> identity kinematics + //-> xyzabc TCP + //-> xyzabc TOOL + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); if (res) goto error; hal_ready(comp_id); @@ -122,7 +91,7 @@ EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); -static hal_u32_t switchkins_type; +static rtapi_u32 switchkins_type; int kinematicsSwitchable() {return 1;} @@ -136,28 +105,28 @@ int kinematicsSwitch(int new_switchkins_type) switch (switchkins_type) { case 0: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE0\n"); - *haldata->kinstype_is_0 = 1; - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_2 = 0; + hal_set_bool(haldata->kinstype_is_0, 1); + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_2, 0); break; case 1: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n"); - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_1 = 1; - *haldata->kinstype_is_2 = 0; + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_1, 1); + hal_set_bool(haldata->kinstype_is_2, 0); break; case 2: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n"); - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_2 = 1; + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_2, 1); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "kinematicsSwitch:BAD VALUE <%d>\n", switchkins_type); - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_2 = 0; + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_2, 0); return -1; // FAIL } return 0; // ok @@ -174,9 +143,6 @@ static bool is_setup=0; } // kinematicsType() - -static bool is_ready=0; - int kinematicsForward(const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -184,24 +150,23 @@ int kinematicsForward(const double *j, { (void)fflags; (void)iflags; - static bool gave_msg; // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file - double Ly = *haldata->y_pivot; - double Lz = *haldata->z_pivot; - double Dx = *haldata->x_offset; - double Dy = *haldata->y_offset; - double Dray = *haldata->y_rot_axis - (Dy + Ly); - double Draz = *haldata->z_rot_axis - Lz; - double tc = *haldata->pre_rot; - double nu = *haldata->nut_angle; // degrees - double theta_1 = *haldata->prim_angle; // degrees - double theta_2 = *haldata->sec_angle; // degrees + double Ly = hal_get_real(haldata->y_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = *(haldata->tool_offset_z); + double Dt = hal_get_real(haldata->tool_offset_z); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -335,14 +300,7 @@ int kinematicsForward(const double *j, pos->u = 0; pos->v = 0; pos->w = 0; - // pass the current position to the outside world - if (*haldata->in && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s in pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; } // kinematicsForward() @@ -353,24 +311,23 @@ int kinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file - double Ly = *haldata->y_pivot; - double Lz = *haldata->z_pivot; - double Dx = *haldata->x_offset; - double Dy = *haldata->y_offset; - double Dray = *haldata->y_rot_axis - (Dy + Ly); - double Draz = *haldata->z_rot_axis - Lz; - double tc = *haldata->pre_rot; - double nu = *haldata->nut_angle; // degrees - double theta_1 = *haldata->prim_angle; // degrees - double theta_2 = *haldata->sec_angle; // degrees + double Ly = hal_get_real(haldata->y_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = *(haldata->tool_offset_z); + double Dt = hal_get_real(haldata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -503,9 +460,5 @@ int kinematicsInverse(const EmcPose * pos, break; } - //example hal pin update (homing reqd before kinematicsInverse) - *haldata->out = *haldata->in; //dereference - //read from param example: *haldata->out = haldata->param_rw; - return 0; } // kinematicsInverse() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 39bcd54e8d7..b8f451c17f9 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -5,11 +5,8 @@ description FIXME """; -// The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. -pin out s32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; +pin out si32 dummy=0 "dummy pin to satisfy halcompile"; option period no; -function fdemo; license "GPL"; author "David Mueller"; @@ -20,48 +17,32 @@ author "David Mueller"; static struct haldata { - // Example pin pointers: - hal_u32_t *in; - hal_u32_t *out; // these should be parameters really but we want to be able to // change them for demonstration purposes - hal_float_t *x_pivot; - hal_float_t *z_pivot; - hal_float_t *x_offset; - hal_float_t *y_offset; - hal_float_t *x_rot_axis; - hal_float_t *z_rot_axis; - hal_float_t *pre_rot; - hal_float_t *nut_angle; - hal_float_t *prim_angle; - hal_float_t *sec_angle; - - // Example parameters: - // hal_float_t param_rw; - // hal_float_t param_ro; + hal_real_t x_pivot; + hal_real_t z_pivot; + hal_real_t x_offset; + hal_real_t y_offset; + hal_real_t x_rot_axis; + hal_real_t z_rot_axis; + hal_real_t pre_rot; + hal_real_t nut_angle; + hal_real_t prim_angle; + hal_real_t sec_angle; // Parameters used for xyzbca_trsrn kinematics: // Declare hal pin pointers used for xyzbca_trsrn kinematics: - hal_float_t *tool_offset_z; + hal_real_t tool_offset_z; //Declare hal pin pointers used for switchable kinematics - hal_bit_t *kinstype_is_0; - hal_bit_t *kinstype_is_1; - hal_bit_t *kinstype_is_2; + hal_bool_t kinstype_is_0; + hal_bool_t kinstype_is_1; + hal_bool_t kinstype_is_2; } *haldata; -FUNCTION(fdemo) { - // This function can be added to a thread (addf) for - // purposes not related to the kinematics functions. - if (fpin == 0) { - rtapi_print("fdemo function added to thread\n"); - } - fpin++; -} - static int xyzbca_trsrn_setup(void) { #define HAL_PREFIX "xyzbca_trsrn_kins" int res=0; @@ -73,37 +54,26 @@ static int xyzbca_trsrn_setup(void) { haldata = hal_malloc(sizeof(struct haldata)); if (!haldata) goto error; - // hal pin examples: - res += hal_pin_u32_newf(HAL_IN ,&(haldata->in) ,comp_id,"%s.in" ,HAL_PREFIX); - res += hal_pin_u32_newf(HAL_OUT,&(haldata->out),comp_id,"%s.out",HAL_PREFIX); - - // hal parameter examples: - // res += hal_param_float_newf(HAL_RW, &haldata->param_rw ,comp_id,"%s.param-rw",HAL_PREFIX); - // res += hal_param_float_newf(HAL_RO, &haldata->param_ro ,comp_id,"%s.param-ro",HAL_PREFIX); - // hal pins required for xyzbca_trsrn kinematics: - res += hal_pin_float_newf(HAL_IN ,&(haldata->tool_offset_z) - ,comp_id,"%s.tool-offset-z" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->x_pivot) ,comp_id,"%s.x-pivot" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->z_pivot) ,comp_id,"%s.z-pivot" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->x_offset) ,comp_id,"%s.x-offset" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->y_offset) ,comp_id,"%s.y-offset" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->x_rot_axis) ,comp_id,"%s.x-rot-axis" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->z_rot_axis) ,comp_id,"%s.z-rot-axis" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->pre_rot) ,comp_id,"%s.pre-rot" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->nut_angle) ,comp_id,"%s.nut-angle" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->prim_angle) ,comp_id,"%s.primary-angle" ,HAL_PREFIX); - res += hal_pin_float_newf(HAL_IN ,&(haldata->sec_angle) ,comp_id,"%s.secondary-angle" ,HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", HAL_PREFIX); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", HAL_PREFIX); // hal pins required for switchable kinematics: - res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(haldata->kinstype_is_0), comp_id); - res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(haldata->kinstype_is_1), comp_id); - res += hal_pin_bit_new("kinstype.is-2", HAL_OUT, &(haldata->kinstype_is_2), comp_id); - - // define default kinematics at startup for switchable kinematics - *haldata->kinstype_is_0 = 1; //default at startup -> identity kinematics - *haldata->kinstype_is_1 = 0; //-> xyzabc TCP - *haldata->kinstype_is_2 = 0; //-> xyzabc TOOL + //default at startup -> identity kinematics + //-> xyzabc TCP + //-> xyzabc TOOL + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); + res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); if (res) goto error; hal_ready(comp_id); @@ -121,7 +91,7 @@ EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); -static hal_u32_t switchkins_type; +static rtapi_u32 switchkins_type; int kinematicsSwitchable() {return 1;} @@ -135,28 +105,28 @@ int kinematicsSwitch(int new_switchkins_type) switch (switchkins_type) { case 0: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE0\n"); - *haldata->kinstype_is_0 = 1; - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_2 = 0; + hal_set_bool(haldata->kinstype_is_0, 1); + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_2, 0); break; case 1: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n"); - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_1 = 1; - *haldata->kinstype_is_2 = 0; + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_1, 1); + hal_set_bool(haldata->kinstype_is_2, 0); break; case 2: rtapi_print_msg(RTAPI_MSG_INFO, "kinematicsSwitch:TYPE1\n"); - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_2 = 1; + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_2, 1); break; default: rtapi_print_msg(RTAPI_MSG_ERR, "kinematicsSwitch:BAD VALUE <%d>\n", switchkins_type); - *haldata->kinstype_is_1 = 0; - *haldata->kinstype_is_0 = 0; - *haldata->kinstype_is_2 = 0; + hal_set_bool(haldata->kinstype_is_1, 0); + hal_set_bool(haldata->kinstype_is_0, 0); + hal_set_bool(haldata->kinstype_is_2, 0); return -1; // FAIL } return 0; // ok @@ -173,9 +143,6 @@ static bool is_setup=0; } // kinematicsType() - -static bool is_ready=0; - int kinematicsForward(const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -183,24 +150,23 @@ int kinematicsForward(const double *j, { (void)fflags; (void)iflags; - static bool gave_msg; // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file - double Lx = *haldata->x_pivot; - double Lz = *haldata->z_pivot; - double Dx = *haldata->x_offset; - double Dy = *haldata->y_offset; - double Drax = *haldata->x_rot_axis - Lx- Dx; - double Draz = *haldata->z_rot_axis - Lz; - double tc = *haldata->pre_rot; - double nu = *haldata->nut_angle; // degrees - double theta_1 = *haldata->prim_angle; // degrees - double theta_2 = *haldata->sec_angle; // degrees + double Lx = hal_get_real(haldata->x_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Drax = hal_get_real(haldata->x_rot_axis) - Lx- Dx; + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = *(haldata->tool_offset_z); + double Dt = hal_get_real(haldata->tool_offset_z); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -339,14 +305,7 @@ int kinematicsForward(const double *j, pos->u = 0; pos->v = 0; pos->w = 0; - // pass the current position to the outside world - if (*haldata->in && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s in pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; } // kinematicsForward() @@ -357,24 +316,23 @@ int kinematicsInverse(const EmcPose * pos, { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file - double Lx = *haldata->x_pivot; - double Lz = *haldata->z_pivot; - double Dx = *haldata->x_offset; - double Dy = *haldata->y_offset; - double Drax = *haldata->x_rot_axis - Lx - Dx; - double Draz = *haldata->z_rot_axis - Lz; - double tc = *haldata->pre_rot; - double nu = *haldata->nut_angle; // degrees - double theta_1 = *haldata->prim_angle; // degrees - double theta_2 = *haldata->sec_angle; // degrees + double Lx = hal_get_real(haldata->x_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = *(haldata->tool_offset_z); + double Dt = hal_get_real(haldata->tool_offset_z); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -505,9 +463,5 @@ int kinematicsInverse(const EmcPose * pos, break; } - //example hal pin update (homing reqd before kinematicsInverse) - *haldata->out = *haldata->in; //dereference - //read from param example: *haldata->out = haldata->param_rw; - return 0; } // kinematicsInverse()