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
|
/********************************************************************
* Description: maxkins.c
* Kinematics for Chris Radek's tabletop 5 axis mill named 'max'.
* This mill has a tilting head (B axis) and horizontal rotary
* mounted to the table (C axis).
*
* Author: Chris Radek
* License: GPL Version 2
*
* Copyright (c) 2007 Chris Radek
********************************************************************/
#include "kinematics.h" /* these decls */
#include "posemath.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"
#define d2r(d) ((d)*PM_PI/180.0)
#define r2d(r) ((r)*180.0/PM_PI)
#ifndef hypot
#define hypot(a,b) (sqrt((a)*(a)+(b)*(b)))
#endif
struct haldata {
hal_float_t *pivot_length;
} *haldata;
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
// B correction
double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4]));
double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4]));
// C correction
double xyr = hypot(joints[0], joints[1]);
double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]);
// U correction
double zv = joints[6] * sin(d2r(joints[4]));
double xv = joints[6] * cos(d2r(joints[4]));
// V correction is always in joint 1 only
pos->tran.x = xyr * cos(xytheta) + xb - xv;
pos->tran.y = xyr * sin(xytheta) - joints[7];
pos->tran.z = joints[2] - zb + zv + *(haldata->pivot_length);
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
pos->u = joints[6];
pos->v = joints[7];
pos->w = joints[8];
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
// B correction
double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b));
double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b));
// C correction
double xyr = hypot(pos->tran.x, pos->tran.y);
double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c);
// U correction
double zv = pos->u * sin(d2r(pos->b));
double xv = pos->u * cos(d2r(pos->b));
// V correction is always in joint 1 only
joints[0] = xyr * cos(xytheta) - xb + xv;
joints[1] = xyr * sin(xytheta) + pos->v;
joints[2] = pos->tran.z + zb + zv - *(haldata->pivot_length);
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;
return 0;
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsInverse);
EXPORT_SYMBOL(kinematicsForward);
MODULE_LICENSE("GPL");
int comp_id;
int rtapi_app_main(void) {
int result;
comp_id = hal_init("maxkins");
if(comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
result = hal_pin_float_new("maxkins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
if(result < 0) goto error;
*(haldata->pivot_length) = 0.666;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return result;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
