Created
October 7, 2016 09:01
-
-
Save TurBoss/1957bf3c056bea5efe9eef43b91a2d8d to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/******************************************************************** | |
* Description: galvo9kins.c | |
* Kinematics for 2 axis galvanometer scanner | |
* Derived from: | |
* | |
* Author: Jozsef Papp / hg5bsd | |
* License: coding and fitting it into EMC2 from our part is GPL alike | |
* | |
* System: realtime Linux, EMC2 | |
* Copyright (c) 2010 All rights reserved. | |
* Last change: | |
* 2011.06. 26. only > EMC2 2.4.0 | |
********************************************************************/ | |
#include "kinematics.h" /* these decls */ | |
#include "rtapi_math.h" | |
/* robot geometry */ | |
/* 2 axis galvanometer scanner kinematics for slow scan cnc laser-plotter. | |
d1 - tükör-tengelyek távolsága / mirror-axis distance | |
d2 - AD (joints[0]) és az ernyõ távolsága / AD (joint[0] and screen distance | |
AD (joints[0]) - ernyõ felöli tükör mozgató motor, y irány / screen side mirror moving motor, y direction | |
CD (joints[1]) - lézer felöli tükörmozgató motor, x irány / laser side mirror moving motor, x direction | |
*/ | |
#ifdef RTAPI | |
#include "rtapi.h" /* RTAPI realtime OS API */ | |
#include "rtapi_app.h" /* RTAPI realtime module decls */ | |
#include "hal.h" | |
#include "rtapi_math.h" | |
struct haldata { | |
hal_float_t *d1, *d2; | |
} *haldata = 0; | |
#define D1 (*(haldata->d1)) /*tükör-tengelyek távolsága*/ | |
#define D2 (*(haldata->d2)) /*AD (joints[0]) és az ernyõ távolsága*/ | |
#else | |
double D1, D2; | |
#endif | |
#define sq(x) ((x)*(x)) | |
#define PI 3.14159265358979323846 | |
int kinematicsForward( const double *joints, | |
EmcPose *pos, | |
const KINEMATICS_FORWARD_FLAGS *fflags, | |
KINEMATICS_INVERSE_FLAGS *iflags) | |
{ | |
#define AD (joints[0]) /*ernyõ felöli tükör mozgató motor, y irány*/ | |
#define CD (joints[1]) /*lézer felöli tükörmozgató motor, x irány*/ | |
#define Dx (pos->tran.x) | |
#define Dy (pos->tran.y) | |
Dx=(D1+sqrt(sq(D2)+sq(Dy)))*tan(2*(CD)*PI/180); | |
Dy=D2*tan(2*(AD)*PI/180); | |
/* pos->a = 0.0; | |
pos->b = 0.0; | |
pos->c = 0.0;*/ | |
return 0; | |
#undef AD | |
#undef CD | |
#undef Dx | |
#undef Dy | |
} | |
int kinematicsInverse(const EmcPose * pos, | |
double * joints, | |
const KINEMATICS_INVERSE_FLAGS * iflags, | |
KINEMATICS_FORWARD_FLAGS * fflags) | |
{ | |
#define AD (joints[0]) | |
#define CD (joints[1]) | |
#define Dx (pos->tran.x) | |
#define Dy (pos->tran.y) | |
AD=0.5*atan(Dy/D2)*180/PI; | |
CD=0.5*atan(Dx/(D1+sqrt(sq(D2)+sq(Dy))))*180/PI; | |
return 0; | |
#undef AD | |
#undef CD | |
#undef Dx | |
#undef Dy | |
} | |
/* 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; | |
} | |
#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("galvo9kins"); | |
if(comp_id < 0) return comp_id; | |
haldata = hal_malloc(sizeof(struct haldata)); | |
if(!haldata) goto error; | |
if((res = hal_pin_float_new("galvo9kins.d1", HAL_IO, &(haldata->d1), comp_id)) < 0) goto error; | |
if((res = hal_pin_float_new("galvo9kins.d2", HAL_IO, &(haldata->d2), comp_id)) < 0) goto error; | |
D1 = D2 = 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 | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment