Hello, folks!
I have several questions about following part of my kinematics module
(I have added the numbering at the beginning of lines, so that it is
easier to understand, which phrase or line is being discussed):
1 #include "rtapi_math.h"
2 #include "kinematics.h" /* these decls */
3
4 const double head_offset = 200;
5
6 PmCartesian old;
7
8 in t kinematicsForward(const double *joints,
9 EmcPose * pos,
10 const KINEMATICS_FORWARD_FLAGS * fflags,
11 KINEMATICS_INVERSE_FLAGS * iflags)
12 {
13 double xy_tan = atan2(pos->tran.y - old.y,pos->tran.x - old.x);
14 pos->tran.x = joints[0] - head_offset * sin(joints[5]);
15 pos->tran.y = joints[2] + head_offset * sin(joints[4]);
16 pos->tran.z = joints[3] + cos(joints[4]) * cos(joints[5]) * head_offset;
17 pos->a = asin(sin(joints[4]) / cos(xy_tan));
18 if xy_tan = 90 + (180 * i), where i is not 0, but is 1, 2, 3, etc
or -1, -2, -3, -4 etc
19 then pos->a = asin(sin(joints[5]) / sin(xy_tan));
20 pos->c = joints[6];
21 pos->u = joints[7];
22 pos->v = joints[8];
23
24 return 0;
25 }
26
27 int kinematicsInverse(const EmcPose * pos,
28 double *joints,
29 const KINEMATICS_INVERSE_FLAGS * iflags,
30 KINEMATICS_FORWARD_FLAGS * fflags)
31 {
32 double xy_tan = atan2(pos->tran.y - old.y,pos->tran.x - old.x);
33 joints[0] = pos->tran.x + (head_offset * sin(joints[5]));
34 joints[1] = pos->tran.x + (head_offset * sin(joints[5]));
35 joints[2] = pos->tran.y - (head_offset * sin(joints[4]));
36 joints[3] = pos->tran.z - (cos(joints[4]) * cos(joints[5]) *
head_offset);
37 OR joints[3] = pos_.tran.z - (cos(pos->a) * head_offset);
38 joints[4] = asin(sin(pos->a) * cos(xy_tan));
39 joints[5] = asin(sin(pos->a) * sin(xy_tan));
40 joints[6] = pos->c;
41 joints[7] = pos->u;
42 joints[8] = pos->v;
43
44 return 0;
45 }
1) What would correct syntax for line 18 and 19 look like?
2) Is the current assignement between XYZ axis and joints[1], [2], [3]
and [4] going to work correctly? Is there anything specific to be
changed in HAL for these kinematics to work?
3) How do I include these lines in xy_tan (it is the tangent for the
nozzle movement in XY plane) calculation:
For extremes:
If X - old.X = 0, and Y - old.Y > 0, then C = 0
If X - old.X = 0, and Y - old.Y < 0, then C = 180
If Y - old.Y = 0, and X - old.X > 0, then C = 90
If Y - old.Y = 0, and X - old.X < 0, then C = 270
All the rest 4 quadrants are appropriate value of tangent functions as follows:
If X - old.X > 0, and Y - old.Y > 0, then 270 < C < 360
If X - old.X < 0, and Y - old.Y > 0, then 0 < C < 90
If X - old.X < 0, and Y - old.Y < 0, then 90 < C < 180
If X - old.X > 0, and Y - old.Y < 0, then 180 < C < 270
4) Are the formulas correct at all? Yesterday I was convinced that
they are perfect, but now I am not that sure, because of the fact that
sin and cos values repeat every 180 degrees, values of tan and cotan
functions repeat every 180 degrees and the more I think about them,
the more I get confused.
Please, share Your thoughts, opinions and comments! They will be
really appreciated, as they can help get this thing working correctly.
Thank You in advance!
with best regards,
Viesturs
P.S> I have attached whole module file, if anyone is interested.
/********************************************************************
* Description: ABkins.c
* Kinematics for a 5 axis gantry-style waterjet for dynamic taper compensation.
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author: Viesturs Lācis
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2010 All rights reserved.
*
********************************************************************/
#include "rtapi_math.h"
#include "kinematics.h" /* these decls */
const double head_offset = 200;
PmCartesian old;
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double xy_tan = atan2(pos->tran.y - old.y,pos->tran.x - old.x);
pos->tran.x = joints[0] - head_offset * sin(joints[5]);
pos->tran.y = joints[2] + head_offset * sin(joints[4]);
pos->tran.z = joints[3] + cos(joints[4]) * cos(joints[5]) * head_offset;
pos->a = asin(joints[4] / cos(xy_tan));
if xy_tan = 90 + (180 * i), where i is not 0, but is 1, 2, 3, etc or -1, -2, -3, -4 etc
then pos->a = asin(joints[5] / sin(xy_tan));
pos->c = joints[6];
pos->u = joints[7];
pos->v = joints[8];
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
double xy_tan = atan2(pos->tran.y - old.y,pos->tran.x - old.x);
joints[0] = pos->tran.x + (head_offset * sin(joints[5]));
joints[1] = pos->tran.x + (head_offset * sin(joints[5]));
joints[2] = pos->tran.y - (head_offset * sin(joints[4]));
joints[3] = pos->tran.z - (cos(joints[4]) * cos(joints[5]) * head_offset);
OR joints[3] = pos_.tran.z - (cos(pos->a) * head_offset);
joints[4] = asin(sin(pos->a) * cos(xy_tan));
joints[5] = asin(sin(pos->a) * sin(xy_tan));
joints[6] = pos->c;
joints[7] = pos->u;
joints[8] = pos->v;
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"
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");
int comp_id;
int rtapi_app_main(void) {
comp_id = hal_init("taperkins");
if(comp_id > 0) {
hal_ready(comp_id);
return 0;
}
return comp_id;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
------------------------------------------------------------------------------
ThinkGeek and WIRED's GeekDad team up for the Ultimate
GeekDad Father's Day Giveaway. ONE MASSIVE PRIZE to the
lucky parental unit. See the prize list and enter to win:
http://p.sf.net/sfu/thinkgeek-promo
_______________________________________________
Emc-users mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/emc-users
