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
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
|
/********************************************************************
* Description: gantrykins.c
* Gantry (ganged joint) kinematics for 3 axis Cartesian machine
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* License: GPL Version 2
*
* Copyright (c) 2006 All rights reserved.
*
********************************************************************/
#include "motion.h" /* these decls */
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"
#include "rtapi_string.h"
struct data {
hal_s32_t joints[EMCMOT_MAX_JOINTS];
} *data;
#define SET(f) pos->f = joints[i]
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
int i;
for(i=0; i<9; i++) {
switch(data->joints[i]) {
case 0: SET(tran.x); break;
case 1: SET(tran.y); break;
case 2: SET(tran.z); break;
case 3: SET(a); break;
case 4: SET(b); break;
case 5: SET(c); break;
case 6: SET(u); break;
case 7: SET(v); break;
case 8: SET(w); break;
}
}
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
int i;
for(i=0; i<9; i++) {
switch(data->joints[i]) {
case 0: joints[i] = pos->tran.x; break;
case 1: joints[i] = pos->tran.y; break;
case 2: joints[i] = pos->tran.z; break;
case 3: joints[i] = pos->a; break;
case 4: joints[i] = pos->b; break;
case 5: joints[i] = pos->c; break;
case 6: joints[i] = pos->u; break;
case 7: joints[i] = pos->v; break;
case 8: joints[i] = pos->w; break;
}
}
return 0;
}
/* 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 KINEMATICS_BOTH;
}
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
char *coordinates = "XYZABC";
RTAPI_MP_STRING(coordinates, "Mapping from axes to joints");
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");
static int next_axis_number(void) {
while(*coordinates) {
switch(*coordinates) {
case 'x': case 'X': coordinates++; return 0;
case 'y': case 'Y': coordinates++; return 1;
case 'z': case 'Z': coordinates++; return 2;
case 'a': case 'A': coordinates++; return 3;
case 'b': case 'B': coordinates++; return 4;
case 'c': case 'C': coordinates++; return 5;
case 'u': case 'U': coordinates++; return 6;
case 'v': case 'V': coordinates++; return 7;
case 'w': case 'W': coordinates++; return 8;
case ' ': case '\t': coordinates++; continue;
}
rtapi_print_msg(RTAPI_MSG_ERR,
"GANTRYKINS: ERROR: Invalid character '%c' in coordinates",
*coordinates);
}
return -1;
}
int comp_id;
int rtapi_app_main(void) {
int result, i;
comp_id = hal_init("gantrykins");
if(comp_id < 0) return comp_id;
data = hal_malloc(sizeof(struct data));
for(i=0; i<EMCMOT_MAX_JOINTS; i++) {
result = hal_param_s32_newf(HAL_RW, &(data->joints[i]), comp_id,
"gantrykins.joint-%d", i);
if(result < 0) goto error;
data->joints[i] = next_axis_number();
rtapi_print_msg(RTAPI_MSG_ERR,
"GANTRYKINS: joints[%d] = %d\n", i, data->joints[i]);
}
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return result;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
