[SOLVED]strange soft limit area is possible??
- bkt
-
Topic Author
- Offline
- Platinum Member
-
Less
More
- Posts: 1224
- Thank you received: 113
17 Oct 2016 08:09 - 15 Mar 2017 15:18 #81713
by bkt
[SOLVED]strange soft limit area is possible?? was created by bkt
Last edit: 15 Mar 2017 15:18 by bkt.
Please Log in or Create an account to join the conversation.
- andypugh
-
- Offline
- Moderator
-
Less
More
- Posts: 23320
- Thank you received: 4946
17 Oct 2016 22:38 #81748
by andypugh
Replied by andypugh on topic strange soft limit area is possible??
Currently there is no way to have a non-cubic soft limit envelope.
It's a pity, it would be nice to be able to define a soft-limit mesh on quite a range of machines.
It's a pity, it would be nice to be able to define a soft-limit mesh on quite a range of machines.
Please Log in or Create an account to join the conversation.
- bkt
-
Topic Author
- Offline
- Platinum Member
-
Less
More
- Posts: 1224
- Thank you received: 113
19 Oct 2016 11:44 #81835
by bkt
Replied by bkt on topic strange soft limit area is possible??
ok I put it on my "to-do" bag ....
Regards
giorgio
Regards
giorgio
Please Log in or Create an account to join the conversation.
- bkt
-
Topic Author
- Offline
- Platinum Member
-
Less
More
- Posts: 1224
- Thank you received: 113
15 Mar 2017 15:18 #89672
by bkt
Replied by bkt on topic strange soft limit area is possible??
Sorry for the very late reply ... but today I had to face the problem again so I wrote this horrible solution ... but it works!
the solution:
I hope these help
Giorgio
the solution:
#ifdef RTAPI
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
struct haldata {
hal_float_t *e, *f, *re, *rf, *encval, *encdx, *encdy, *encdz, *xincr, *yincr, *zincr, *aincr, *bincr, *posarea, *secareax, *secareay;
hal_bit_t *resenc, *isteleop, *ismanual, *ishomed, *jx, *jy, *jz, *ja, *jb, *secarea;
} *haldata = 0;
#define delta_e (*(haldata->e))
#define delta_f (*(haldata->f))
#define delta_re (*(haldata->re))
#define delta_rf (*(haldata->rf))
#define delta_encval (*(haldata->encval))
#define delta_encdx (*(haldata->encdx))
#define delta_encdy (*(haldata->encdy))
#define delta_encdz (*(haldata->encdz))
#define delta_xincr (*(haldata->xincr))
#define delta_yincr (*(haldata->yincr))
#define delta_zincr (*(haldata->zincr))
#define delta_aincr (*(haldata->aincr))
#define delta_bincr (*(haldata->bincr))
#define delta_resenc (*(haldata->resenc))
#define delta_isteleop (*(haldata->isteleop))
#define delta_ismanual (*(haldata->ismanual))
#define delta_ishomed (*(haldata->ishomed))
#define delta_jx (*(haldata->jx))
#define delta_jy (*(haldata->jy))
#define delta_jz (*(haldata->jz))
#define delta_ja (*(haldata->ja))
#define delta_jb (*(haldata->jb))
#define delta_secarea (*(haldata->secarea))
#define delta_posarea (*(haldata->posarea))
#define delta_secareax (*(haldata->secareax))
#define delta_secareay (*(haldata->secareay))
#else
double delta_e, delta_f, delta_re, delta_rf, delta_encval, delta_encdx, delta_encdy, delta_encdz, delta_secareax, delta_secareay, delta_posarea;
#endif
/********* other kins part ************/
int kinematicsInverse(const EmcPose *world,
double *joints,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags)
{
double x0, y0, z0, theta1, theta2, theta3;
int status;
x0 = world->tran.x;
y0 = world->tran.y;
z0 = world->tran.z;
if((x0 > delta_secareax) && (y0 > delta_secareay) && (delta_posarea == 1))
{
delta_secarea = 1;
}
else if ((x0 > delta_secareax) && (y0 < delta_secareay) && (delta_posarea == 2))
{
delta_secarea = 1;
}
else if((x0 < delta_secareax) && (y0 < delta_secareay) && (delta_posarea == 3))
{
delta_secarea = 1;
}
else if((x0 < delta_secareax) && (y0 > delta_secareay) && (delta_posarea == 4))
{
delta_secarea = 1;
}
else
{
delta_secarea = 0;
}
joints[3] = world->a;
joints[4] = world->b;
joints[5] = world->c;
joints[6] = world->u;
joints[7] = world->v;
joints[8] = world->w;
theta1 = theta2 = theta3 = 0;
status = delta_calcAngleYZ(x0, y0, z0, &theta1);
if (status == 0) joints[0] = theta1;
if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120,y0*cos120-x0*sin120, z0, &theta2); /*rotate coords to +120 deg*/
if (status == 0) joints[1] = theta2;
if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120,y0*cos120+x0*sin120, z0, &theta3); /*rotate coords to -120 deg*/
if (status == 0) joints[2] = theta3;
return status;
}
/* 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;
/* return KINEMATICS_INVERSE_ONLY;*/
}
#ifdef RTAPI
#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) {
int res = 0;
comp_id = hal_init("delta_AAB_inv");
if(comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
if(!haldata) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.e", HAL_IO, &(haldata->e),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.f", HAL_IO, &(haldata->f),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.re", HAL_IO, &(haldata->re),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.rf", HAL_IO, &(haldata->rf),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.encval", HAL_IN, &(haldata->encval),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.encdx", HAL_OUT, &(haldata->encdx),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.encdy", HAL_OUT, &(haldata->encdy),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.encdz", HAL_OUT, &(haldata->encdz),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.xincr", HAL_OUT, &(haldata->xincr),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.yincr", HAL_OUT, &(haldata->yincr),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.zincr", HAL_OUT, &(haldata->zincr),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.aincr", HAL_OUT, &(haldata->aincr),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.bincr", HAL_OUT, &(haldata->bincr),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.secarea", HAL_OUT, &(haldata->secarea),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.posarea", HAL_IN, &(haldata->posarea),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.secareax", HAL_IN, &(haldata->secareax),comp_id)) != 0) goto error;
if((res = hal_pin_float_new("delta_AAB_inv.secareay", HAL_IN, &(haldata->secareay),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.isteleop", HAL_IN, &(haldata->isteleop),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.ismanual", HAL_IN, &(haldata->ismanual),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.ishomed", HAL_IN, &(haldata->ishomed),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.jx", HAL_IN, &(haldata->jx),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.jy", HAL_IN, &(haldata->jy),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.jz", HAL_IN, &(haldata->jz),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.ja", HAL_IN, &(haldata->ja),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.jb", HAL_IN, &(haldata->jb),comp_id)) != 0) goto error;
if((res = hal_pin_bit_new("delta_AAB_inv.resenc", HAL_OUT, &(haldata->resenc),comp_id)) != 0) goto error;
delta_e = delta_f = delta_re = delta_rf = 1.0;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
#endif
I hope these help
Giorgio
Please Log in or Create an account to join the conversation.
Time to create page: 0.073 seconds