5 Axis ParPort Stepper Setup with RTCP

This is movement of XZB when C axis is 0

it is holding spindle center Point not tool tip

This is Movement of XZB axis when C ia Rotated -180 Degree.

All I can suggest is checking values in Halmeter and/or Halscope to make sure that the numbers going in to the kinematics are as-expected, and to make sure that your kinematics calculations are really doing what you want.

Do the calculations include the tool length offset? Does the Vismach model accurately represent the tool length? Vismach just plots the position of the end of the tool shape in its own private space, it knowns nothing at all about the kinematics or the real machine.

Your kinematics was meant to ignore C. Does it?

This is 5Axiskin i am testing now. need some hint where to focus??

Yes andypugh i want to ignore C axis.

as i have solved trigonometry

For X = [X DRO + {(Sin( B )) * (Pivot length + Tool offset)}]

For Z = [Z DRO + {(Pivot Length + Tool Offset) -(Cos( B )*(Pivot Length+ Tool Offset))}]

but i am noob in C ...so not able to figure out how to include this calculation



/********************************************************************
* Description: 5axiskins.c
* Trivial kinematics for 3 axis Cartesian machine
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2007 Chris Radek
*
* Last change:
********************************************************************/

#include "kinematics.h" /* these decls */
#include "posemath.h"
#include "hal.h"
#include "rtapi_math.h"

#define d2r(d) ((d)*PM_PI/180.0)
#define r2d(r) ((r)*180.0/PM_PI)

struct haldata {
hal_float_t *tool_length;
hal_float_t *pivot_length;
} *haldata;

static PmCartesian s2r(double r, double t, double p) {
PmCartesian c;
t = d2r(t), p = d2r(p);

c.x = r * sin(p) * cos(t);
c.y = r * sin(p) * sin(t);
c.z = r * cos(p);

return c;
}

int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
PmCartesian r = s2r(*(haldata->pivot_length) + joints[8], 0, 180.0 - joints[4]);

pos->tran.x = joints[0] + r.x;
pos->tran.y = joints[1] + r.y;
pos->tran.z = joints[2] + *(haldata->pivot_length) + r.z;
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)
{

PmCartesian r = s2r(*(haldata->pivot_length) + pos->w, 0, 180.0 - pos->b);

joints[0] = pos->tran.x - r.x;
joints[1] = pos->tran.y - r.y;
joints[2] = pos->tran.z - *(haldata->pivot_length) - r.z;
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;
}

/* 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) {
int result;
comp_id = hal_init("5axiskins");
if(comp_id < 0) return comp_id;

haldata = hal_malloc(sizeof(struct haldata));

result = hal_pin_float_new("5axiskins.tooloffset.z", HAL_IN, &(haldata->tool_length), comp_id);
if(result < 0) goto error;
result = hal_pin_float_new("5axiskins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
if(result < 0) goto error;

*(haldata->pivot_length) = 250.0;

hal_ready(comp_id);
return 0;

error:
hal_exit(comp_id);
return result;
}

void rtapi_app_exit(void) { hal_exit(comp_id); }

It is fairly clear that tool_length isn't being used in the calculations. That solves one puzzle.

As for the rest, you may not need the pmcartesian, and d2r and r2d are degrees to radian conversions because C trigonometry functions use radians.

So, if it's easier for you just put the equations in axis/joint caclulations line.

The functions and constants you can think of using are here:
www.cs.cf.ac.uk/Dave/C/node17.html

You will just have to become a little less noob about C to figure out the syntax. You can only use () brackets and the line has to end with ;
You need to use *(haldata->pivot_length) to get the pivot length value from the HAL pin and *(haldata->tool_length) to get thte tool length.

Then is is just a matter of reading the error reports and acting on them until it compiles and works.

thanks for quick and prompt reply @andypugh,

i am in lots of question on your answers ... i m not understanding what you are saying to change ... can you please explain me ??