напиши автору в личку,так он точно увидит...
Код: Выделить всё
/********************************************************************
* Description: relkins.c
*
* Trivial kinematics for 3 axis Cartesian machine
* with position adjustment
*
* Author: Mikhail Vydrenko (mikhail@vydrenko.ru)
********************************************************************/
#define FOR_MACHINEKIT 0
#include "motion.h"
#include "kinematics.h" /* these decls */
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
#if FOR_MACHINEKIT
#include <stdlib.h>
#else
#include "rtapi_math.h"
#include "rtapi_string.h"
#include <linux/kernel.h>
#endif
#define ADJ_STEPS_MAX 255
#define PRINT_ERROR(MSG) \
rtapi_print_msg(RTAPI_MSG_ERR, "%s: "MSG"\n", comp_name)
#define PRINT_ERROR_AND_RETURN(MSG,RETVAL) \
{ PRINT_ERROR(MSG); return RETVAL; }
#if FOR_MACHINEKIT
#define VTVERSION VTKINEMATICS_VERSION1
#endif
MODULE_AUTHOR("Mikhail Vydrenko");
MODULE_DESCRIPTION("Trivial kinematics with position adjustment");
MODULE_LICENSE("GPL");
typedef struct
{
hal_bit_t enable;
hal_u32_t joint_id[EMCMOT_MAX_JOINTS];
hal_u32_t joint_cnt;
hal_bit_t adj_enable;
hal_s32_t adj_from[2];
hal_s32_t adj_offset;
hal_u32_t adj_steps;
hal_u32_t adj_step_size;
} axis_t;
#if FOR_MACHINEKIT
static int vtable_id;
#endif
static int comp_id;
static const char *comp_name = "relkins";
static const char *axis_name0 = "xyzabcuvw";
static const char *axis_name1 = "XYZABCUVW";
static KINEMATICS_TYPE ktype = -1;
static axis_t * _axis;
static hal_float_t ** _adj[EMCMOT_MAX_JOINTS];
static char *coordinates = "XYZABCUVW";
RTAPI_MP_STRING(coordinates, "Existing Axes");
static char *kinstype = "1"; // use KINEMATICS_IDENTITY
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");
static char *adjust = "";
RTAPI_MP_STRING(adjust, "adjust axis name/ID, comma separated");
static char *from = "";
RTAPI_MP_STRING(from, "related axes names/IDs, comma separated");
static char *offset = "";
RTAPI_MP_STRING(offset, "adjust from position offset in units, comma separated");
static char *steps = "";
RTAPI_MP_STRING(steps, "steps count, comma separated");
static char *step_size = "";
RTAPI_MP_STRING(step_size, "step size in units, comma separated");
#define adj_offset_f ((hal_float_t)_axis[axis].adj_offset)
#define adj_steps_f ((hal_float_t)_axis[axis].adj_steps)
#define adj_steps_i _axis[axis].adj_steps
#define adj_step_size_f ((hal_float_t)_axis[axis].adj_step_size)
int
kinematicsForward
(
const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags
)
{
static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
static int32_t step_0, step_1, axis, r0, r1, c0, c1;
static double adj, adj_0, adj_1, rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;
double* axis_pos[EMCMOT_MAX_JOINTS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
#define joint_num _axis[axis].joint_id[0]
#define joint_pos joints[joint_num]
#define rel_axis0 _axis[axis].adj_from[0]
#define rel_joint0_num _axis[rel_axis0].joint_id[0] // use only 1st joint of related axis
#define rel_joint0_pos joints[rel_joint0_num]
#define rel_joint0_offset_pos (rel_joint0_pos - adj_offset_f)
#define rel_axis1 _axis[axis].adj_from[1]
#define rel_joint1_num _axis[rel_axis1].joint_id[0]
#define rel_joint1_pos joints[rel_joint1_num]
#define rel_joint1_offset_pos (rel_joint1_pos - adj_offset_f)
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].enable ) continue;
*axis_pos[axis] = joint_pos;
if ( !_axis[axis].adj_enable ) continue;
// line adjustment
if ( rel_axis1 < 0 )
{
step_0 = (int32_t) (rel_joint0_offset_pos / adj_step_size_f);
step_1 = step_0 + 1;
adj_0 = step_0 >= 0 && step_0 < adj_steps_i ? *_adj[axis][step_0] : 0.0;
adj_1 = step_1 >= 0 && step_1 < adj_steps_i ? *_adj[axis][step_1] : 0.0;
adj = adj_0
+ (adj_1 - adj_0)
* (rel_joint0_offset_pos - adj_step_size_f * ((hal_float_t)step_0))
/ adj_step_size_f;
*axis_pos[axis] += adj;
}
// plane adjustment
else
{
// get row data
r0 = (int32_t) (rel_joint0_offset_pos / adj_step_size_f);
r1 = r0 + 1;
r0_valid = r0 >= 0 && r0 < adj_steps_i;
r1_valid = r1 >= 0 && r1 < adj_steps_i;
// get column data
c0 = (int32_t) (rel_joint1_offset_pos / adj_step_size_f);
c1 = c0 + 1;
c0_valid = c0 >= 0 && c0 < adj_steps_i;
c1_valid = c1 >= 0 && c1 < adj_steps_i;
// no adjustment if current position is out of adjustment zone
// if ( !r0_valid && !r1_valid && !c0_valid && !c1_valid ) continue;
// get values of 4 adjustment cells around current position
r0c0_adj = r0_valid && c0_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c0] : 0.0;
r0c1_adj = r0_valid && c1_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c1] : 0.0;
r1c0_adj = r1_valid && c0_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c0] : 0.0;
r1c1_adj = r1_valid && c1_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c1] : 0.0;
// calculate row/column multipliers
rm = (rel_joint0_offset_pos - ((double)r0) * adj_step_size_f) / adj_step_size_f;
cm = (rel_joint1_offset_pos - ((double)c0) * adj_step_size_f) / adj_step_size_f;
if ( rm < 0.000001 ) rm = 0.0;
else if ( rm > 0.999999 ) rm = 1.0;
if ( cm < 0.000001 ) cm = 0.0;
else if ( cm > 0.999999 ) cm = 1.0;
adj_0 = (((r1c1_adj - r1c0_adj) * rm) - ((r0c1_adj - r0c0_adj) * rm)) * cm;
adj_1 = (((r1c1_adj - r0c1_adj) * cm) - ((r1c0_adj - r0c0_adj) * cm)) * rm;
adj = (adj_0 + adj_1) / 2;
*axis_pos[axis] += adj;
}
}
#undef joint_num
#undef joint_pos
#undef rel_axis0
#undef rel_joint0_num
#undef rel_joint0_pos
#undef rel_joint0_offset_pos
#undef rel_axis1
#undef rel_joint1_num
#undef rel_joint1_pos
#undef rel_joint1_offset_pos
return 0;
}
int
kinematicsInverse
(
const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags
)
{
static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
static int32_t step_0, step_1, axis, n, r0, r1, c0, c1;
static double adj, adj_0, adj_1, rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;
const double* axis_pos[EMCMOT_MAX_JOINTS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
#define joint_num _axis[axis].joint_id[n]
#define joint_pos joints[joint_num]
#define rel_axis0 _axis[axis].adj_from[0]
#define rel_axis0_pos *axis_pos[rel_axis0]
#define rel_axis0_offset_pos (rel_axis0_pos - adj_offset_f)
#define rel_axis1 _axis[axis].adj_from[1]
#define rel_axis1_pos *axis_pos[rel_axis1]
#define rel_axis1_offset_pos (rel_axis1_pos - adj_offset_f)
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].enable ) continue;
for ( n = _axis[axis].joint_cnt; n--; )
{
joint_pos = *axis_pos[axis];
if ( !_axis[axis].adj_enable ) continue;
// line adjustment
if ( rel_axis1 < 0 )
{
step_0 = (int32_t) (rel_axis0_offset_pos / adj_step_size_f);
step_1 = step_0 + 1;
adj_0 = step_0 >= 0 && step_0 < adj_steps_i ? *_adj[axis][step_0] : 0.0;
adj_1 = step_1 >= 0 && step_1 < adj_steps_i ? *_adj[axis][step_1] : 0.0;
adj = adj_0
+ (adj_1 - adj_0)
* (rel_axis0_offset_pos - adj_step_size_f * ((hal_float_t)step_0))
/ adj_step_size_f;
joint_pos -= adj;
}
// plane adjustment
else
{
// get row data
r0 = (int32_t) (rel_axis0_offset_pos / adj_step_size_f);
r1 = r0 + 1;
r0_valid = r0 >= 0 && r0 < adj_steps_i;
r1_valid = r1 >= 0 && r1 < adj_steps_i;
// get column data
c0 = (int32_t) (rel_axis1_offset_pos / adj_step_size_f);
c1 = c0 + 1;
c0_valid = c0 >= 0 && c0 < adj_steps_i;
c1_valid = c1 >= 0 && c1 < adj_steps_i;
// no adjustment if current position is out of adjustment zone
// if ( !r0_valid && !r1_valid && !c0_valid && !c1_valid ) continue;
// get values of 4 adjustment cells around current position
r0c0_adj = r0_valid && c0_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c0] : 0.0;
r0c1_adj = r0_valid && c1_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c1] : 0.0;
r1c0_adj = r1_valid && c0_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c0] : 0.0;
r1c1_adj = r1_valid && c1_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c1] : 0.0;
// calculate row/column multipliers
rm = (rel_axis0_offset_pos - ((double)r0) * adj_step_size_f) / adj_step_size_f;
cm = (rel_axis1_offset_pos - ((double)c0) * adj_step_size_f) / adj_step_size_f;
if ( rm < 0.000001 ) rm = 0.0;
else if ( rm > 0.999999 ) rm = 1.0;
if ( cm < 0.000001 ) cm = 0.0;
else if ( cm > 0.999999 ) cm = 1.0;
adj_0 = (((r1c1_adj - r1c0_adj) * rm) - ((r0c1_adj - r0c0_adj) * rm)) * cm;
adj_1 = (((r1c1_adj - r0c1_adj) * cm) - ((r1c0_adj - r0c0_adj) * cm)) * rm;
adj = (adj_0 + adj_1) / 2;
joint_pos -= adj;
}
}
}
#undef joint_num
#undef joint_pos
#undef rel_axis0
#undef rel_axis0_pos
#undef rel_axis0_offset_pos
#undef rel_axis1
#undef rel_axis1_pos
#undef rel_axis1_offset_pos
return 0;
}
#undef adj_offset_f
#undef adj_steps_f
#undef adj_steps_i
#undef adj_step_size_f
/* implemented for these kinematics as giving joints preference */
int
kinematicsHome
(
EmcPose * world,
double * joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags
)
{
*fflags = 0;
*iflags = 0;
return kinematicsForward(joint, world, fflags, iflags);
}
KINEMATICS_TYPE
kinematicsType()
{
return ktype;
}
#if FOR_MACHINEKIT
static vtkins_t vtk =
{
.kinematicsForward = kinematicsForward,
.kinematicsInverse = kinematicsInverse,
// .kinematicsHome = kinematicsHome,
.kinematicsType = kinematicsType
};
#else
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
#endif
static int axis_name_to_id(char axis_char)
{
int axis;
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( axis_char == axis_name0[axis] || axis_char == axis_name1[axis] ) return axis;
}
return -1;
}
int rtapi_app_main(void)
{
int32_t retval, axis, joint, id, axis_list[EMCMOT_MAX_JOINTS] = {-1};
int32_t r, c;
char *data, *token;
// component init
comp_id = hal_init(comp_name);
if ( comp_id < 0 ) return -1;
#if FOR_MACHINEKIT
// export kinematics table
vtable_id = hal_export_vtable(comp_name, VTVERSION, &vtk, comp_id);
if ( vtable_id < 0 )
{
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
comp_name, comp_name, VTVERSION, &vtk, vtable_id);
return -1;
}
#endif
// shared memory allocation
_axis = hal_malloc(EMCMOT_MAX_JOINTS * sizeof(axis_t));
if ( !_axis ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// parse `kinstype`
switch (*kinstype)
{
case 'b': case 'B': ktype = KINEMATICS_BOTH; break;
case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
case '1': default: ktype = KINEMATICS_IDENTITY;
}
// parse `coordinates`
for ( joint = 0, token = coordinates; *token && joint < EMCMOT_MAX_JOINTS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS )
{
_axis[axis].enable = 1;
_axis[axis].joint_id[ _axis[axis].joint_cnt++ ] = joint;
}
joint++;
}
// parse `adjust`
for ( id = 0, token = adjust; *token && id < EMCMOT_MAX_JOINTS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS && _axis[axis].enable )
{
_axis[axis].adj_enable = 1;
axis_list[id] = axis;
}
id++;
}
// parse `from`
#if FOR_MACHINEKIT
for ( id = 0, data = from; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = from; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
_axis[axis].adj_from[0] = axis_name_to_id(token[0]);
_axis[axis].adj_from[1] = axis_name_to_id(token[1]);
if ( _axis[axis].adj_from[0] < 0 ) _axis[axis].adj_enable = 0;
}
// parse `steps`
#if FOR_MACHINEKIT
for ( id = 0, data = steps; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = steps; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_steps = (hal_u32_t) strtoul(token, NULL, 10);
#else
retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_steps);
#endif
if ( !_axis[axis].adj_steps ) _axis[axis].adj_enable = 0;
if ( _axis[axis].adj_steps > ADJ_STEPS_MAX ) _axis[axis].adj_steps = ADJ_STEPS_MAX;
}
// parse `step_size`
#if FOR_MACHINEKIT
for ( id = 0, data = step_size; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = step_size; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_step_size = (hal_u32_t) strtoul(token, NULL, 10);
#else
retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_step_size);
#endif
if ( !_axis[axis].adj_step_size ) _axis[axis].adj_enable = 0;
}
// parse `offset`
#if FOR_MACHINEKIT
for ( id = 0, data = offset; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = offset; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_offset = (hal_s32_t) strtol(token, NULL, 10);
#else
retval = kstrtos32(token, 10, (int32_t*) &_axis[axis].adj_offset);
#endif
}
// mem alloc, export pins
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].adj_enable ) continue;
// plane adjustment
if ( _axis[axis].adj_from[1] >= 0 )
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * _axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
for ( c = 0; c < _axis[axis].adj_steps; c++ )
{
retval += hal_pin_float_newf( HAL_IN,
&(_adj[axis][r * _axis[axis].adj_steps + c]), comp_id,
"%s.adj%c.%c%d_%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset),
axis_name1[ _axis[axis].adj_from[1] ],
(c * _axis[axis].adj_step_size + _axis[axis].adj_offset)
);
}
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL plane step pins export failed",-1);
}
// line adjustment
else
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
retval += hal_pin_float_newf( HAL_IN, &_adj[axis][r], comp_id,
"%s.adj%c.%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset) );
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL line step pins export failed",-1);
}
}
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
#if FOR_MACHINEKIT
hal_remove_vtable(vtable_id);
#endif
hal_exit(comp_id);
}
