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
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
|
component millturn "Switchable kinematics for a mill-turn machine";
description
"""
.if \\n[.g] .mso www.tmac
This is a switchable kinematics module using 3 cartesian linear joints (XYZ)
and 1 angular joint (A). The module contains two kinematic models:
type0 (default) is a mill (XYZA) configuration with A being a
rotary axis.
type1 is a turn (Z-YX) configuration with A configured to be a spindle.
For an example configuration, run the sim config: 'configs/sim/axis/vismach/millturn/millturn.ini'.
Further explanations can be found in the README in 'configs/sim/axis/vismach/millturn'.
millturn.comp was constructed by modifying the template file:
userkins.comp.
For more information on how to modify userkins.comp run: $ man
userkins. Also, see additional information inside: 'userkins.comp'.
For information on kinematics in general see the kinematics
document chapter (docs/src/motion/kinematics.txt) and for
switchable kinematics in particular see the switchkins document
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";
function fdemo;
license "GPL";
author "David Mueller";
;;
#include "rtapi_math.h"
#include "kinematics.h"
static struct haldata {
// Example pin pointers:
hal_u32_t *in;
hal_u32_t *out;
// Example parameters:
hal_float_t param_rw;
hal_float_t param_ro;
//Declare hal pin pointers used for switchable kinematics
hal_bit_t *kinstype_is_0;
hal_bit_t *kinstype_is_1;
} *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 millturn_setup(void) {
#define HAL_PREFIX "millturn"
int res=0;
// 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;
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 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
if (res) goto error;
hal_ready(comp_id);
rtapi_print("*** %s setup ok\n",__FILE__);
return 0;
error:
rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res);
return -1;
#undef HAL_PREFIX
}
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsSwitchable);
EXPORT_SYMBOL(kinematicsSwitch);
EXPORT_SYMBOL(kinematicsInverse);
EXPORT_SYMBOL(kinematicsForward);
static hal_u32_t switchkins_type;
int kinematicsSwitchable() {return 1;}
int kinematicsSwitch(int new_switchkins_type)
{
switchkins_type = new_switchkins_type;
rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type);
// create case structure for switchable kinematics
switch (switchkins_type) {
case 0: rtapi_print_msg(RTAPI_MSG_INFO,
"kinematicsSwitch:TYPE0\n");
*haldata->kinstype_is_0 = 1;
*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;
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;
return -1; // FAIL
}
return 0; // ok
}
KINEMATICS_TYPE kinematicsType()
{
static bool is_setup=0;
if (!is_setup) millturn_setup();
return KINEMATICS_BOTH; // set as required
// Note: If kinematics are identity, using KINEMATICS_BOTH
// may be used in order to allow a gui to display
// joint values in preview prior to homing
} // kinematicsType()
static bool is_ready=0;
int kinematicsForward(const double *j,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
static bool gave_msg;
// define forward kinematic models using case structure for
// for switchable kinematics
switch (switchkins_type) {
case 0:
pos->tran.x = j[0];
pos->tran.y = j[1];
pos->tran.z = j[2];
pos->a = j[3];
break;
case 1:
pos->tran.x = j[2];
pos->tran.y = -j[1];
pos->tran.z = j[0];
pos->a = j[3];
break;
}
// unused coordinates:
pos->b = 0;
pos->c = 0;
pos->u = 0;
pos->v = 0;
pos->w = 0;
if (*haldata->in && !is_ready && !gave_msg) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s the 'in' pin not echoed until Inverse called\n",
__FILE__);
gave_msg=1;
}
return 0;
} // kinematicsForward()
int kinematicsInverse(const EmcPose * pos,
double *j,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH
// Update the kinematic joints specified by the
// [KINS]JOINTS setting (4 required for this template).
// define forward kinematic models using case structure for
// for switchable kinematics
switch (switchkins_type) {
case 0:
j[0] = pos->tran.x;
j[1] = pos->tran.y;
j[2] = pos->tran.z;
j[3] = pos->a;
break;
case 1:
j[2] = pos->tran.x;
j[1] = -pos->tran.y;
j[0] = pos->tran.z;
j[3] = pos->a;
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()
|
