1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
|
/* userkfuncs.c: template file for a user set of
** switchable kinematics functions.
** License GPL Version 2
**
** Example Usage (for customizing the genser-switchkins module):
** (works with rtpreempt only rtai --> Makefile needs work)
**
** LDIR is LinuxCNC git root directory
** UDIR is user directory (not in LinuxCNC git tree)
** 1) $ cp LDIR/src/emc/kinematics/userkfuncs.c UDIR/my_userk.c
** 2) $ edit UDIR/my_userk.c as required
** 3) $ source LDIR/scripts/rip-environment
** 4) For genser-switchkins module use make command line option:
** $ cd LDIR/src
** $ userkfuncs=UDIR/my_userk.c make && sudo make setuid
*/
// typical includes:
//#include "rtapi_math.h" // if reqd
#include "kinematics.h"
#include "hal.h"
// Add for kins based on genserkins:
// #include "genserkins.h" //includes gomath,hal
//**********************************************************************
// static local variables and functions go here
static int userk_inited = 0;
static struct udata {
hal_s32_t *fct;
hal_s32_t *ict;
} *udata;
//**********************************************************************
int userkKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* kp)
{
int res=0;
rtapi_print("\nuserkKinematicsSetup:\n"
" %s <%s> max_joints=%d allow_duplicates=%d\n\n",
__FILE__,coordinates,
kp->max_joints,kp->allow_duplicates);
udata = hal_malloc(sizeof(struct udata));
if (!udata) goto error;
// HAL_IO used to allow resetting demo pins:
res += hal_pin_s32_new("userk.fct", HAL_IO, &(udata->fct), comp_id);
res += hal_pin_s32_new("userk.ict", HAL_IO, &(udata->ict), comp_id);
if (res) goto error;
userk_inited = 1;
return 0; // 0 ==> OK
error:
return -1;
}
int userkKinematicsForward(const double *joint,
struct EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
if (!userk_inited) {
rtapi_print_msg(RTAPI_MSG_ERR,
"userkKinematics: not initialized\n");
return -1;
}
(*udata->fct)++;
return identityKinematicsForward(joint,world,fflags,iflags);
}
int userkKinematicsInverse(const EmcPose * pos,
double *joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(*udata->ict)++;
return identityKinematicsInverse(pos,joint,iflags,fflags);
}
|
