按推荐建议,继续执行

结论:继续按 LinuxCNC 源码直接复用路线推进,新增 corexy、rotate、rose、max 四个运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
2026-06-07 19:44:25 +08:00
parent e634adfa56
commit d936e9f10c
12 changed files with 1008 additions and 1 deletions

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

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

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

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