The bipod kinematics was implemented as descibed in chapter 18 "Kinematics in EMC2" in the [Integrator Manual]. In src/emc/kinematics/bipodkins.c we added the following code:
#include "motion.h" /* these decls */ #include "rtapi_math.h"
/* ident tag */ #ifndef __GNUC__ #ifndef __attribute__ #define __attribute__(x) #endif #endif
![[Home]](/chips.gif)
#ifdef RTAPI
#include "hal.h"
struct {
hal_float_t bx;
} *haldata = 0;
#define Bx (haldata->bx) #else double Bx; #endif
#define sq(x) ((x)*(x))
int kinematicsForward(const double * joints,
EmcPose? * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
#define AD (joints[0])
#define BD (joints[1])
#define Dx (pos->tran.x)
#define Dy (pos->tran.y)
#define Dz (pos->tran.z)
double AD2 = sq(joints[0]); double BD2 = sq(joints[1]);
double x = (AD2 - BD2 + sq(Bx)) / (2 * Bx); double y2 = AD2 - x * x;
if (y2 < 0)
return -1;
Dx = x; Dy = sqrt(y2); Dz = joints[2];
return 0;
#undef AD #undef BD #undef Dx #undef Dy #undef Dz }
int kinematicsInverse(const EmcPose? * pos, double * joints, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { #define AD (joints[0]) #define BD (joints[1]) #define Dx (pos->tran.x) #define Dy (pos->tran.y) #define Dz (pos->tran.z)
double x2 = sq(Dx); double y2 = sq(Dy); AD = sqrt(x2 + y2); BD = sqrt(sq(Bx - Dx) + y2); joints[2] = Dz;
return 0;
#undef AD #undef BD #undef Dx #undef Dy #undef Dz }
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
#ifdef MAIN
#include <stdio.h> #include <string.h>
/* Interactive testing of kins.
Syntax: a.out <Bx>
*/
int main(int argc, char *argv[])
{
#ifndef BUFFERLEN
#define BUFFERLEN 256
#endif
char buffer[BUFFERLEN];
char cmd[BUFFERLEN];
EmcPose? pos, vel;
double joints[3], jointvels[3];
char inverse;
char flags;
KINEMATICS_FORWARD_FLAGS fflags;
inverse = 0; /* forwards, by default */
flags = 0; /* didn't provide flags */
fflags = 0; /* above xy plane, by default */
if (argc != 2 || 1 != sscanf(argv[1], "%lf", &Bx)) {
fprintf(stderr, "syntax: %s Bx\n", argv[0]);
return 1;
}
while (! feof(stdin)) {
if (inverse) {
printf("inv> ");
}
else {
printf("fwd> ");
}
fflush(stdout);
if (NULL == fgets(buffer, BUFFERLEN, stdin)) {
break;
}
if (1 != sscanf(buffer, "%s", cmd)) {
continue;
}
if (! strcmp(cmd, "quit")) {
break;
}
if (! strcmp(cmd, "i")) {
inverse = 1;
continue;
}
if (! strcmp(cmd, "f")) {
inverse = 0;
continue;
}
if (! strcmp(cmd, "ff")) {
if (1 != sscanf(buffer, "%*s %d", &fflags)) {
printf("need forward flag\n");
}
continue;
}
if (inverse) { /* inverse kins */
if (3 != sscanf(buffer, "%lf %lf %lf",
&pos.tran.x,
&pos.tran.y,
&pos.tran.z)) {
printf("need X Y Z\n");
continue;
}
if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) {
printf("inverse kin error\n");
}
else {
printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]);
if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) {
printf("forward kin error\n");
}
else {
printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z);
}
}
}
else { /* forward kins */
if (flags) {
if (4 != sscanf(buffer, "%lf %lf %lf %d",
&joints[0],
&joints[1],
&joints[2],
&fflags)) {
printf("need 3 strut values and flag\n");
continue;
}
}
else {
if (3 != sscanf(buffer, "%lf %lf %lf",
&joints[0],
&joints[1],
&joints[2])) {
printf("need 3 strut values\n");
continue;
}
}
if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) {
printf("forward kin error\n");
}
else {
printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z);
if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) {
printf("inverse kin error\n");
}
else {
printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]);
}
}
}
} /* end while (! feof(stdin)) */
return 0; }
#endif /* MAIN */
#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("bipodkins");
if(comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(*haldata));
if(!haldata) goto error;
Bx = 1.0;
if((res = hal_param_float_new("bipodkins.Bx", HAL_RW, &haldata->bx, comp_id)) != HAL_SUCCESS) goto error;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
#endif
In src/Makefile? we added the following lines:
src/Makefile?:obj-m += bipodkins.o src/Makefile?:bipodkins-objs := emc/kinematics/bipodkins.o src/Makefile?:../rtlib/bipodkins$(MODULE_EXT): $(addprefix objects/rt,$(bipodkins-objs))
In configs/bipod_stepper.hal we added the lines:
loadrt bipodkins setp bipodkins.Bx 1220
where "1220" is the space in mm between the two motor axes.
The next three photos show the bipod plotting the Inkscape drawing [Month, Day, Weekday, Day/Night, Hour, Minute, Second] by Tavmjong Bah:
The Z axis of the bipod:
The timing belt and the second axis joint.
The IBM Thinkpad X30 connects to the stepper motor drivers via its parallel port:
The software cam.py was used to trace the paths of the drawing and create the ngc file:
The AXIS GUI:
[Inkscape] and a patched version of cam.py were used to create the ngc file.
With the use of EMC2 and the bipod kinematics applied the machine moves XY (in carthesian space) just fine. The machine was inspired by Alex Joni's Toy and by the work of [Jürg Lehni and Uli Franke]. Over time several people around the world enjoyed creating their own incarnation of hektor:
* Vic Aprea and Paul Grzymkowski: http://instruct1.cit.cornell.edu/courses/ee476/FinalProjects/s2001/vp2/ * Casey Canfield, Roland Crosby, Jessi Murray and Hermes Taylor: http://rolocroz.com/wallrus/ * Miguel Sánchez: http://fightpc.blogspot.com/2008/02/vertical-plotter-article.html * Romado12187: http://spritesmods.com/?art=whiteboard&f=had * Jan Van Gilsen: http://www.youtube.com/watch?v=WdSAGfyygMs
Feel free to contact me on the jabber network at koppi@jabber.ccc.de. Say hi!