Skip to content

hal: Update more kinematics to getter/setter. #4297

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
BsAtHome wants to merge 1 commit into
base: master
Choose a base branch
from
Open

hal: Update more kinematics to getter/setter. #4297

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
85 changes: 40 additions & 45 deletions src/hal/components/matrixkins.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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;

Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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;
Expand Down
56 changes: 27 additions & 29 deletions src/hal/components/millturn.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand All @@ -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) {
Expand All @@ -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) {
Expand All @@ -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);
Expand All @@ -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;}

Expand All @@ -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
Expand Down Expand Up @@ -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__);
Expand Down Expand Up @@ -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()
34 changes: 17 additions & 17 deletions src/hal/components/userkins.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand All @@ -71,25 +71,25 @@ 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
// purposes not related to the kinematics functions.
if (fpin == 0) {
rtapi_print("fdemo function added to thread\n");
}
fpin++;
fpin_set(fpin + 1);
}

static int userkins_setup(void) {
Expand All @@ -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);
Expand Down Expand Up @@ -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__);
Expand Down Expand Up @@ -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()
Loading
Loading