按推荐建议,继续执行
结论:继续按 LinuxCNC 源码直接复用路线推进,新增 corexy、rotate、rose、max 四个运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
89
wasm-port/vendor/linuxcnc/src/emc/kinematics/corexykins.c
vendored
Normal file
89
wasm-port/vendor/linuxcnc/src/emc/kinematics/corexykins.c
vendored
Normal file
@@ -0,0 +1,89 @@
|
||||
/********************************************************************
|
||||
* Description: kinematics for corexy
|
||||
* Adapted from trivkins.c
|
||||
* ref: http://corexy.com/theory.html
|
||||
********************************************************************/
|
||||
|
||||
#include <rtapi.h>
|
||||
#include <rtapi.h>
|
||||
#include <rtapi_app.h>
|
||||
#include <rtapi_math.h>
|
||||
#include <rtapi_string.h>
|
||||
#include <hal.h>
|
||||
#include <emcmotcfg.h>
|
||||
#include <kinematics.h>
|
||||
|
||||
static struct data {
|
||||
hal_s32_t joints[EMCMOT_MAX_JOINTS];
|
||||
} *data;
|
||||
|
||||
int kinematicsForward(const double *joints
|
||||
,EmcPose *pos
|
||||
,const KINEMATICS_FORWARD_FLAGS *fflags
|
||||
,KINEMATICS_INVERSE_FLAGS *iflags
|
||||
) {
|
||||
(void)fflags;
|
||||
(void)iflags;
|
||||
pos->tran.x = 0.5 * (joints[0] + joints[1]);
|
||||
pos->tran.y = 0.5 * (joints[0] - joints[1]);
|
||||
pos->tran.z = joints[2];
|
||||
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
|
||||
) {
|
||||
(void)iflags;
|
||||
(void)fflags;
|
||||
joints[0] = pos->tran.x + pos->tran.y;
|
||||
joints[1] = pos->tran.x - pos->tran.y;
|
||||
joints[2] = pos->tran.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;
|
||||
}
|
||||
|
||||
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; }
|
||||
|
||||
KINS_NOT_SWITCHABLE
|
||||
EXPORT_SYMBOL(kinematicsType);
|
||||
EXPORT_SYMBOL(kinematicsForward);
|
||||
EXPORT_SYMBOL(kinematicsInverse);
|
||||
MODULE_LICENSE("GPL");
|
||||
|
||||
static int comp_id;
|
||||
int rtapi_app_main(void) {
|
||||
comp_id = hal_init("corexykins");
|
||||
if(comp_id < 0) return comp_id;
|
||||
|
||||
data = hal_malloc(sizeof(struct data));
|
||||
|
||||
hal_ready(comp_id);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||
148
wasm-port/vendor/linuxcnc/src/emc/kinematics/maxkins.c
vendored
Normal file
148
wasm-port/vendor/linuxcnc/src/emc/kinematics/maxkins.c
vendored
Normal file
@@ -0,0 +1,148 @@
|
||||
/********************************************************************
|
||||
* Description: maxkins.c
|
||||
* Kinematics for Chris Radek's tabletop 5 axis mill named 'max'.
|
||||
* This mill has a tilting head (B axis) and horizontal rotary
|
||||
* mounted to the table (C axis).
|
||||
*
|
||||
* Author: Chris Radek
|
||||
* License: GPL Version 2
|
||||
*
|
||||
* Copyright (c) 2007 Chris Radek
|
||||
********************************************************************/
|
||||
|
||||
/********************************************************************
|
||||
* Note: The direction of the B axis is the opposite of the
|
||||
* conventional axis direction. See
|
||||
* https://linuxcnc.org/docs/html/gcode/machining-center.html
|
||||
********************************************************************/
|
||||
|
||||
#include <rtapi.h>
|
||||
#include <rtapi_app.h>
|
||||
#include <rtapi_math.h>
|
||||
#include <hal.h>
|
||||
#include <kinematics.h> /* these decls */
|
||||
|
||||
#define d2r(d) ((d)*PM_PI/180.0)
|
||||
#define r2d(r) ((r)*180.0/PM_PI)
|
||||
|
||||
#ifndef hypot
|
||||
#define hypot(a,b) (sqrt((a)*(a)+(b)*(b)))
|
||||
#endif
|
||||
|
||||
struct haldata {
|
||||
hal_float_t *pivot_length;
|
||||
hal_bit_t *conventional_directions; //default is false
|
||||
} *haldata;
|
||||
|
||||
int kinematicsForward(const double *joints,
|
||||
EmcPose * pos,
|
||||
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||
{
|
||||
(void)fflags;
|
||||
(void)iflags;
|
||||
|
||||
const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
|
||||
|
||||
// B correction
|
||||
const double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4]));
|
||||
const double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4]));
|
||||
|
||||
// C correction
|
||||
const double xyr = hypot(joints[0], joints[1]);
|
||||
const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]);
|
||||
|
||||
// U correction
|
||||
const double zv = joints[6] * sin(d2r(joints[4]));
|
||||
const double xv = joints[6] * cos(d2r(joints[4]));
|
||||
|
||||
// V correction is always in joint 1 only
|
||||
|
||||
pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv;
|
||||
pos->tran.y = xyr * sin(xytheta) - joints[7];
|
||||
pos->tran.z = joints[2] - zb - (con * zv) + *(haldata->pivot_length);
|
||||
|
||||
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)
|
||||
{
|
||||
(void)iflags;
|
||||
(void)fflags;
|
||||
|
||||
const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
|
||||
|
||||
// B correction
|
||||
const double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b));
|
||||
const double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b));
|
||||
|
||||
// C correction
|
||||
const double xyr = hypot(pos->tran.x, pos->tran.y);
|
||||
const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c);
|
||||
|
||||
// U correction
|
||||
const double zv = pos->u * sin(d2r(pos->b));
|
||||
const double xv = pos->u * cos(d2r(pos->b));
|
||||
|
||||
// V correction is always in joint 1 only
|
||||
|
||||
joints[0] = xyr * cos(xytheta) + (con * xb) + xv;
|
||||
joints[1] = xyr * sin(xytheta) + pos->v;
|
||||
joints[2] = pos->tran.z + zb - (con * zv) - *(haldata->pivot_length);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
KINEMATICS_TYPE kinematicsType()
|
||||
{
|
||||
return KINEMATICS_BOTH;
|
||||
}
|
||||
|
||||
KINS_NOT_SWITCHABLE
|
||||
EXPORT_SYMBOL(kinematicsType);
|
||||
EXPORT_SYMBOL(kinematicsInverse);
|
||||
EXPORT_SYMBOL(kinematicsForward);
|
||||
MODULE_LICENSE("GPL");
|
||||
|
||||
int comp_id;
|
||||
int rtapi_app_main(void) {
|
||||
int result;
|
||||
comp_id = hal_init("maxkins");
|
||||
if(comp_id < 0) return comp_id;
|
||||
|
||||
haldata = hal_malloc(sizeof(struct haldata));
|
||||
|
||||
result = hal_pin_float_new("maxkins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
|
||||
|
||||
result += hal_pin_bit_new("maxkins.conventional-directions", HAL_IN, &(haldata->conventional_directions), comp_id);
|
||||
|
||||
if(result < 0) goto error;
|
||||
|
||||
*(haldata->pivot_length) = 0.666;
|
||||
*(haldata->conventional_directions) = 0; // default is unconventional
|
||||
hal_ready(comp_id);
|
||||
return 0;
|
||||
|
||||
error:
|
||||
hal_exit(comp_id);
|
||||
return result;
|
||||
}
|
||||
|
||||
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||
143
wasm-port/vendor/linuxcnc/src/emc/kinematics/rosekins.c
vendored
Normal file
143
wasm-port/vendor/linuxcnc/src/emc/kinematics/rosekins.c
vendored
Normal file
@@ -0,0 +1,143 @@
|
||||
/*
|
||||
Copyright 2016 Dewey Garrett <dgarrett@panix.com>
|
||||
|
||||
This program is free software; you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation; either version 2 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program; if not, write to the Free Software
|
||||
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||
*/
|
||||
|
||||
#include <rtapi.h>
|
||||
#include <rtapi_math.h>
|
||||
#include <rtapi_app.h>
|
||||
#include <hal.h>
|
||||
#include <kinematics.h>
|
||||
|
||||
KINS_NOT_SWITCHABLE
|
||||
EXPORT_SYMBOL(kinematicsType);
|
||||
EXPORT_SYMBOL(kinematicsInverse);
|
||||
EXPORT_SYMBOL(kinematicsForward);
|
||||
MODULE_LICENSE("GPL");
|
||||
|
||||
#ifndef hypot
|
||||
#define hypot(a,b) (sqrt((a)*(a)+(b)*(b)))
|
||||
#endif
|
||||
|
||||
struct haldata {
|
||||
hal_float_t *revolutions;
|
||||
hal_float_t *theta_degrees;
|
||||
hal_float_t *bigtheta_degrees;
|
||||
} *haldata;
|
||||
|
||||
int kinematicsForward(const double *joints,
|
||||
EmcPose * pos,
|
||||
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||
{
|
||||
(void)fflags;
|
||||
(void)iflags;
|
||||
double radius,z,theta;
|
||||
|
||||
radius = joints[0];
|
||||
z = joints[1];
|
||||
theta = TO_RAD * joints[2];
|
||||
|
||||
pos->tran.x = radius * cos(theta);
|
||||
pos->tran.y = radius * sin(theta);
|
||||
pos->tran.z = z;
|
||||
pos->a = 0;
|
||||
pos->b = 0;
|
||||
pos->c = 0;
|
||||
pos->u = 0;
|
||||
pos->v = 0;
|
||||
pos->w = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int kinematicsInverse(const EmcPose * pos,
|
||||
double *joints,
|
||||
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||
{
|
||||
(void)iflags;
|
||||
(void)fflags;
|
||||
// There is a potential problem when accumulating bigtheta -- loss of
|
||||
// precision based on size of mantissa -- but in practice, it is probably ok
|
||||
|
||||
static int oldquad;
|
||||
static int revolutions;
|
||||
|
||||
double theta,bigtheta;
|
||||
int nowquad = 0;
|
||||
double x = pos->tran.x;
|
||||
double y = pos->tran.y;
|
||||
double z = pos->tran.z;
|
||||
|
||||
if (x >= 0 && y >= 0) nowquad = 1;
|
||||
else if (x < 0 && y >= 0) nowquad = 2;
|
||||
else if (x < 0 && y < 0) nowquad = 3;
|
||||
else if (x >= 0 && y < 0) nowquad = 4;
|
||||
|
||||
if (oldquad == 2 && nowquad == 3) {revolutions += 1;}
|
||||
if (oldquad == 3 && nowquad == 2) {revolutions -= 1;}
|
||||
|
||||
theta = atan2(y,x);
|
||||
bigtheta = theta + PM_2_PI * revolutions;
|
||||
|
||||
*(haldata->revolutions) = revolutions;
|
||||
*(haldata->theta_degrees) = theta * TO_DEG;
|
||||
*(haldata->bigtheta_degrees) = bigtheta * TO_DEG;
|
||||
|
||||
joints[0] = hypot(x,y);
|
||||
joints[1] = z;
|
||||
joints[2] = TO_DEG * bigtheta;
|
||||
joints[3] = 0;
|
||||
joints[4] = 0;
|
||||
joints[5] = 0;
|
||||
joints[6] = 0;
|
||||
joints[7] = 0;
|
||||
joints[8] = 0;
|
||||
|
||||
oldquad = nowquad;
|
||||
return 0;
|
||||
}
|
||||
|
||||
KINEMATICS_TYPE kinematicsType()
|
||||
{
|
||||
return KINEMATICS_BOTH;
|
||||
}
|
||||
|
||||
static int comp_id;
|
||||
|
||||
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||
|
||||
int rtapi_app_main(void) {
|
||||
int ans;
|
||||
comp_id = hal_init("rosekins");
|
||||
if(comp_id < 0) return comp_id;
|
||||
|
||||
haldata = hal_malloc(sizeof(struct haldata));
|
||||
|
||||
if((ans = hal_pin_float_new("rosekins.revolutions",
|
||||
HAL_OUT, &(haldata->revolutions), comp_id)) < 0) goto error;
|
||||
if((ans = hal_pin_float_new("rosekins.theta_degrees",
|
||||
HAL_OUT, &(haldata->theta_degrees), comp_id)) < 0) goto error;
|
||||
if((ans = hal_pin_float_new("rosekins.bigtheta_degrees",
|
||||
HAL_OUT, &(haldata->bigtheta_degrees), comp_id)) < 0) goto error;
|
||||
|
||||
hal_ready(comp_id);
|
||||
return 0;
|
||||
|
||||
error:
|
||||
return ans;
|
||||
}
|
||||
96
wasm-port/vendor/linuxcnc/src/emc/kinematics/rotatekins.c
vendored
Normal file
96
wasm-port/vendor/linuxcnc/src/emc/kinematics/rotatekins.c
vendored
Normal file
@@ -0,0 +1,96 @@
|
||||
/********************************************************************
|
||||
* Description: rotatekins.c
|
||||
* Simple example kinematics for a rotary table in software
|
||||
*
|
||||
* Derived from a work by Fred Proctor & Will Shackleford
|
||||
*
|
||||
* Author: Chris Radek
|
||||
* License: GPL Version 2
|
||||
* System: Linux
|
||||
*
|
||||
* Copyright (c) 2006 All rights reserved.
|
||||
*
|
||||
********************************************************************/
|
||||
|
||||
#include <rtapi.h>
|
||||
#include <rtapi_app.h>
|
||||
#include <rtapi_math.h>
|
||||
#include <hal.h>
|
||||
#include <kinematics.h> /* these decls */
|
||||
|
||||
int kinematicsForward(const double *joints,
|
||||
EmcPose * pos,
|
||||
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||
{
|
||||
(void)fflags;
|
||||
(void)iflags;
|
||||
double c_rad = -joints[5]*M_PI/180;
|
||||
pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad);
|
||||
pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad);
|
||||
pos->tran.z = joints[2];
|
||||
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)
|
||||
{
|
||||
(void)iflags;
|
||||
(void)fflags;
|
||||
double c_rad = pos->c*M_PI/180;
|
||||
joints[0] = pos->tran.x * cos(c_rad) - pos->tran.y * sin(c_rad);
|
||||
joints[1] = pos->tran.x * sin(c_rad) + pos->tran.y * cos(c_rad);
|
||||
joints[2] = pos->tran.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;
|
||||
}
|
||||
|
||||
KINS_NOT_SWITCHABLE
|
||||
EXPORT_SYMBOL(kinematicsType);
|
||||
EXPORT_SYMBOL(kinematicsForward);
|
||||
EXPORT_SYMBOL(kinematicsInverse);
|
||||
MODULE_LICENSE("GPL");
|
||||
|
||||
int comp_id;
|
||||
int rtapi_app_main(void) {
|
||||
comp_id = hal_init("rotatekins");
|
||||
if(comp_id > 0) {
|
||||
hal_ready(comp_id);
|
||||
return 0;
|
||||
}
|
||||
return comp_id;
|
||||
}
|
||||
|
||||
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||
Reference in New Issue
Block a user