5 Axis ParPort Stepper Setup with RTCP

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); }

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 ??

andypugh wrote:

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

can you guide me where to include with what lines exactly ??

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 do i need to remove those lines ???

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.

okay thats what i want to do ...i want to remove all clutter expect the equations for x and z ... but how ?

*(haldata->tool_length) t

do you mean i need to add new line ?? after ..


*(haldata->pivot_length) = 250.0;

i guess ??

Yes yes trying to

btw thanks big time for taking this much hassle