Hello,
I’m trying to make my little machine to work.
I have a problem with the math of kinematicsInverse i think ?
The machine is an horizontal rotating table with X=0,Y=0 in the center.
and one linear movement on top.
I’m trying to do a movement within X,Y coordinates with a rotating movement
A en a linear movement B, this is working fine.
So
A = angle of 45 deg.
B= linear 7.07
Equals
X=5
Y=5
but if i run the code:
G00 X5.
G00 Y5.
G00 X-5.
G00 Y-5
It stops with a joint[3] following error on (+/- 180 degree’s)
I hope that someone can help me with this.
Kind regards,
my modded kins.
/********************************************************************
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/
#include "rtapi_math.h"
#include "kinematics.h"
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double a3,radangle ;
// rotating correction
a3 =joints[3];
rtapi_set_msg_level(4);
radangle= a3* ( PM_PI / 180 );
pos->tran.x = cos(radangle)*joints[4];
pos->tran.y = sin(radangle)*joints[4];
pos->tran.z = joints[2];
pos->a = a3;
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)
{
double x, y, angle, radangle, ilenght;
double currenthoek, berekendhoek ;
double anglediff ,targetangle;
currenthoek = pos->a;
berekendhoek = currenthoek -(floor(currenthoek /360)* 360);
x = pos->tran.x;
y = pos->tran.y;
radangle= atan2 (y,x);
targetangle= radangle * 180 / PM_PI;
ilenght=0;
radangle=0;
ilenght= (y==0) ?fabs(x) : y / sin(atan2 (y,x));
anglediff = targetangle -berekendhoek ;
anglediff += (anglediff>180) ? -360 : (anglediff<-180) ? 360 : 0;
//anglediff difference between angle's
angle = currenthoek+anglediff;
joints[0] = pos->tran.x;
joints[1] = pos->tran.y;
joints[2] = pos->tran.z;
joints[3] = angle;
joints[4] = ilenght;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;
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_IDENTITY;
}
#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("trivkins");
if(comp_id > 0) {
hal_ready(comp_id);
return 0;
}
return comp_id;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
------------------------------------------------------------------------------
This SF email is sponsosred by:
Try Windows Azure free for 90 days Click Here
http://p.sf.net/sfu/sfd2d-msazure
_______________________________________________
Emc-developers mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/emc-developers
