[РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
[РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Имеем оси, на которых по 2 мотора. Известно, что точка контакта шестерни и рейки (шарико-винтовой пары) = именно тот узел, который реально обеспечивает жесткость и геометрию. Проектируя станок Тадам-3 я заведомо проектировал конструкцию таким образом, чтобы работающие по 2 мотора на одной оси (2 по Х и 2 по У) решали одновременно несколько задач = синхронность + выборку зазоров + выравнивание геометрии (не только в плоскости ХУ, но и YZ & XZ). При хоминге ( viewtopic.php?f=15&t=33499) Х работающие два датчика хоминга позволяют после измерений неперпеникулярности Х и У (метод здесь viewtopic.php?f=152&t=28376&p=559316) - корректируя координаты Home offset каждого из моторов (они не одианковые). При хоминге У используется один датчик и аналогичный подход - корректировка Home offset каждого мотора У. Это создает предварительное напряжение в конструкции. Конструкция Тадам-3 стала больше и значительно жестче, нежели Тадам-2, потому выравнивание (сдвиг) 4мм между моторами Х обеспечивает в положении покоя 25-30% номинальной нагрзуки на каждом из моторов Х (используется IHSV57 180вт). Это вцелом обеспечивает постоянный контакт шертерни по одной стороне зубьев зубчатой рейки (=выборка зазора, люфта). Но также это неизбежно влияет на наклон портала относительно плоскости рабочего стола. Откуда следует вывод - требуется программная коррекция кривизны, в дополнение к хомингу по разным кординатам. Это все была присказка.
В Тадам-2 я использовал viewtopic.php?f=15&t=20385&p=611647.
Сейчас я использую LinuxCNC 2.8.2 и конфигурация с пятью моторами, схема XXYYZ. Увы, станок у меня большой. Оказался достаточно жестким и недостаточно прямым. То там, то сям гуляет 0.04-0.1, а где-то что-то может и больше. Суть в том, что если я выставлю при хоминге точную перпендикулярность, то вдоль длинной трехметровой оси момент удержания на моих ihsv57 180Вт может достигать 100%, что приводит к сильному нагреву и перегрузкам. Разумеется, это следствие кривизны где-то чего-то. Но не повод не работать. В итоге я в настройках хоминга не стал требовтаь перпендикулярности. И пришлось вернуться к использованию программной коррекци. В обстоятельствах 2022-август, LinuxCNC2.8.2 рабтоающее решение такое -
0. Берем приложенный модуль, он уже скомпилирован . Инсталлируем его командой -
sudo halcompile --install relkins.c
если halcompile не запускается (вследствие отсутствия), то ставим -
sudo apt-get install linuxcnc-dev (подтянет две зависимости)
1. в ini файле своей конфигурации под свои варианты конфигурации осей (в версии LinuxCNC2.7 этого не было) -
[KINS]
JOINTS = 5
#KINEMATICS = trivkins coordinates=XXYYZ
#Версия программной коррекции
KINEMATICS = relkins coordinates=XXYYZ
2. в hal файле своей конфигурации, в самом его начале и с вашими цифрами коррекций-
#родной раздел выбора кинематики -
#loadrt [KINS]KINEMATICS
#вариант программной коррекции осей
#вариант простой, коррекция вдоль У по двум крайним точка
loadrt relkins coordinates=XXYYZ adjust=X from=Y steps=2 step_size=1225 offset=-25
setp relkins.adjX.Y-25 0
setp relkins.adjX.Y1000 3
#в консoли -
#halcmd setp relkins.adjX.Y1000 3.15
#посмотреть действующие значения
#halcmd show pin relkins
#вариант "многоплановая коррекция"
#loadrt krivkins coordinates=XXYYZ adjust=X,Y from=Y,X steps=5,2 step_size=500,1200 offset=0,0
## настройка точек коррекции для оси X от позиции оси Y
# setp relkins.adjX.Y-25 0.0
# setp relkins.adjX.Y1200 0.0
#
## настройка точек коррекции для оси Y от позиции оси X
# setp relkins.adjY.X100 0.0
# setp relkins.adjY.X200 0.1
# setp relkins.adjY.X300 0.2
# setp relkins.adjY.X400 0.1
# setp relkins.adjY.X500 0.0
#
Как проверить, что все это работает? - достаточно просто. Поправили hal & ini, рапустили LinuxCNC. Если запустился, провдим хоминг. Если вдруг "оси (моторы)" едут не так и не туда - проверяем правильность написания своих XXYYZ. Хоминг прошел. Откатываемся от краев. В терминале даем команду, указав по Y ваше крайнее значение (у меня 1025) и смещение такое, чтобы от текущего Х смещение не вышло за пределы рабочего стола (я поставил 300) -
halcmd setp relkins.adjX.Y1025 300
затем двигаем ось У и видим, что одновременно с У едет и Х, а линия на экране ровная.
Этот подход среди прочего хорош тем, что не требует перезагрузки LinuxCNC и повторного хоминга (который из-за погрешности датчиков всегда дает новые результаты). Включал станок, проводил измерения по угольнику (небольшая программа на минуту работы), вводил в консоли команду = результат.
И сразу хочется бОльшего - задать не одну линию коррекции, а несколько. Механиз от MX_Master это позволяет, но я пока не освоил.
В Тадам-2 я использовал viewtopic.php?f=15&t=20385&p=611647.
Сейчас я использую LinuxCNC 2.8.2 и конфигурация с пятью моторами, схема XXYYZ. Увы, станок у меня большой. Оказался достаточно жестким и недостаточно прямым. То там, то сям гуляет 0.04-0.1, а где-то что-то может и больше. Суть в том, что если я выставлю при хоминге точную перпендикулярность, то вдоль длинной трехметровой оси момент удержания на моих ihsv57 180Вт может достигать 100%, что приводит к сильному нагреву и перегрузкам. Разумеется, это следствие кривизны где-то чего-то. Но не повод не работать. В итоге я в настройках хоминга не стал требовтаь перпендикулярности. И пришлось вернуться к использованию программной коррекци. В обстоятельствах 2022-август, LinuxCNC2.8.2 рабтоающее решение такое -
0. Берем приложенный модуль, он уже скомпилирован . Инсталлируем его командой -
sudo halcompile --install relkins.c
если halcompile не запускается (вследствие отсутствия), то ставим -
sudo apt-get install linuxcnc-dev (подтянет две зависимости)
1. в ini файле своей конфигурации под свои варианты конфигурации осей (в версии LinuxCNC2.7 этого не было) -
[KINS]
JOINTS = 5
#KINEMATICS = trivkins coordinates=XXYYZ
#Версия программной коррекции
KINEMATICS = relkins coordinates=XXYYZ
2. в hal файле своей конфигурации, в самом его начале и с вашими цифрами коррекций-
#родной раздел выбора кинематики -
#loadrt [KINS]KINEMATICS
#вариант программной коррекции осей
#вариант простой, коррекция вдоль У по двум крайним точка
loadrt relkins coordinates=XXYYZ adjust=X from=Y steps=2 step_size=1225 offset=-25
setp relkins.adjX.Y-25 0
setp relkins.adjX.Y1000 3
#в консoли -
#halcmd setp relkins.adjX.Y1000 3.15
#посмотреть действующие значения
#halcmd show pin relkins
#вариант "многоплановая коррекция"
#loadrt krivkins coordinates=XXYYZ adjust=X,Y from=Y,X steps=5,2 step_size=500,1200 offset=0,0
## настройка точек коррекции для оси X от позиции оси Y
# setp relkins.adjX.Y-25 0.0
# setp relkins.adjX.Y1200 0.0
#
## настройка точек коррекции для оси Y от позиции оси X
# setp relkins.adjY.X100 0.0
# setp relkins.adjY.X200 0.1
# setp relkins.adjY.X300 0.2
# setp relkins.adjY.X400 0.1
# setp relkins.adjY.X500 0.0
#
Как проверить, что все это работает? - достаточно просто. Поправили hal & ini, рапустили LinuxCNC. Если запустился, провдим хоминг. Если вдруг "оси (моторы)" едут не так и не туда - проверяем правильность написания своих XXYYZ. Хоминг прошел. Откатываемся от краев. В терминале даем команду, указав по Y ваше крайнее значение (у меня 1025) и смещение такое, чтобы от текущего Х смещение не вышло за пределы рабочего стола (я поставил 300) -
halcmd setp relkins.adjX.Y1025 300
затем двигаем ось У и видим, что одновременно с У едет и Х, а линия на экране ровная.
Этот подход среди прочего хорош тем, что не требует перезагрузки LinuxCNC и повторного хоминга (который из-за погрешности датчиков всегда дает новые результаты). Включал станок, проводил измерения по угольнику (небольшая программа на минуту работы), вводил в консоли команду = результат.
И сразу хочется бОльшего - задать не одну линию коррекции, а несколько. Механиз от MX_Master это позволяет, но я пока не освоил.
Последний раз редактировалось a321 06 авг 2022, 00:21, всего редактировалось 4 раза.
Re: LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Ну тогда давайте попробуем обратиться - все ранее размещенные ссылки на гит или гитлаб превратились в тыкву. Пожалуйста актуализируйе проект компоненты для LCNC 2.8 и инструкцию по сборке, подключении и пр.
Если же 2.8 имеет внутри себя "коробочные" решения типа обсуждавшихся viewtopic.php?p=127248#p127248 - поделитесь пожалуйста ссылкой.
Несмотря на то, что 2.8 имеет коробчное решение выравнивания портала, это "полное" выравнивание может создавать повышенную нагрузку на моторы и есть необходимость дополнительной коррекции.
- MX_Master
- Мастер
- Сообщения: 7480
- Зарегистрирован: 27 июн 2015, 19:45
- Репутация: 3101
- Настоящее имя: Михаил
- Откуда: Алматы
- Контактная информация:
Re: LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Файл могу найти, но к нему не прилагается никакого автосканера поверхности или чего-то подобного. Чисто руками в HAL файле поправки вбивать. Какие-то доки не прилагаются, всё как есть. И, по-моему, там ещё что-то не работало (было отключено), если коррекция шла по нескольким осям одновременно. Что-то с расчётами. Если кто-то желает заняться, милости просим. Возможно, коллективный разум справится лучше. У меня бесплатного времени пока очень мало
Собирать, по-моему, также как и другие RT компоненты - sudo halcompile --install krivkins.c
Собирать, по-моему, также как и другие RT компоненты - sudo halcompile --install krivkins.c
Код: Выделить всё
#define FOR_MACHINEKIT 0
#include "motion.h"
#include "kinematics.h" /* these decls */
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
#if FOR_MACHINEKIT
#include <stdlib.h>
#else
#include "rtapi_math.h"
#include "rtapi_string.h"
#include <linux/kernel.h>
#endif
#define ADJ_STEPS_MAX 255
#define ADJ_FORWARD 0
#define ADJ_INVERSE 1
#define PRINT_ERROR(MSG) \
rtapi_print_msg(RTAPI_MSG_ERR, "%s: "MSG"\n", comp_name)
#define PRINT_ERROR_AND_RETURN(MSG,RETVAL) \
{ PRINT_ERROR(MSG); return RETVAL; }
#if FOR_MACHINEKIT
#define VTVERSION VTKINEMATICS_VERSION1
#endif
MODULE_AUTHOR("Mikhail Vydrenko");
MODULE_DESCRIPTION("Trivial kinematics with position adjustment");
MODULE_LICENSE("GPL");
typedef struct
{
hal_bit_t enable;
hal_u32_t joint_id[EMCMOT_MAX_JOINTS];
hal_u32_t joint_cnt;
hal_bit_t adj_enable;
hal_s32_t adj_from[2];
hal_s32_t adj_offset;
hal_u32_t adj_steps;
hal_u32_t adj_step_size;
} axis_t;
#if FOR_MACHINEKIT
static int vtable_id;
#endif
static int comp_id;
static const char *comp_name = "krivkins";
static const char *axis_name0 = "xyzabcuvw";
static const char *axis_name1 = "XYZABCUVW";
static KINEMATICS_TYPE ktype = -1;
static axis_t * _axis;
static hal_float_t ** _adj[EMCMOT_MAX_JOINTS];
static char *coordinates = "XYZABCUVW";
RTAPI_MP_STRING(coordinates, "Existing Axes");
static char *kinstype = "1"; // use KINEMATICS_IDENTITY
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");
static char *adjust = "";
RTAPI_MP_STRING(adjust, "adjust axis name/ID, comma separated");
static char *from = "";
RTAPI_MP_STRING(from, "related axes names/IDs, comma separated");
static char *offset = "";
RTAPI_MP_STRING(offset, "adjust from position offset in units, comma separated");
static char *steps = "";
RTAPI_MP_STRING(steps, "steps count, comma separated");
static char *step_size = "";
RTAPI_MP_STRING(step_size, "step size in units, comma separated");
double get_adj(uint8_t axis, double src_pos0, double src_pos1);
int
kinematicsForward
(
const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags
)
{
static int32_t axis;
double* axis_pos[EMCMOT_MAX_JOINTS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].enable ) continue;
*axis_pos[axis] = joints[ _axis[axis].joint_id[0] ];
#if ADJ_FORWARD
if ( !_axis[axis].adj_enable ) continue;
if ( _axis[axis].adj_from[1] < 0 )
{
*axis_pos[axis] += get_adj(
axis,
joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
0.0);
}
else
{
*axis_pos[axis] += get_adj(
axis,
joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
joints[ _axis[ _axis[axis].adj_from[1] ].joint_id[0] ]);
}
#endif
}
return 0;
}
int
kinematicsInverse
(
const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags
)
{
static int32_t axis, n;
const double* axis_pos[EMCMOT_MAX_JOINTS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].enable ) continue;
for ( n = _axis[axis].joint_cnt; n--; )
{
joints[ _axis[axis].joint_id[n] ] = *axis_pos[axis];
#if ADJ_INVERSE
if ( !_axis[axis].adj_enable ) continue;
if ( _axis[axis].adj_from[1] < 0 )
{
joints[ _axis[axis].joint_id[n] ] -= get_adj(
axis,
*axis_pos[ _axis[axis].adj_from[0] ],
0.0);
}
else
{
joints[ _axis[axis].joint_id[n] ] -= get_adj(
axis,
*axis_pos[ _axis[axis].adj_from[0] ],
*axis_pos[ _axis[axis].adj_from[1] ]);
}
#endif
}
}
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 ktype;
}
#if FOR_MACHINEKIT
static vtkins_t vtk =
{
.kinematicsForward = kinematicsForward,
.kinematicsInverse = kinematicsInverse,
// .kinematicsHome = kinematicsHome,
.kinematicsType = kinematicsType
};
#else
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
#endif
#define adj_offset_i _axis[axis].adj_offset
#define adj_steps_i _axis[axis].adj_steps
#define adj_step_size_i _axis[axis].adj_step_size
#define rel_axis0 _axis[axis].adj_from[0]
#define rel_axis1 _axis[axis].adj_from[1]
double get_adj(uint8_t axis, double src_pos0, double src_pos1)
{
static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
static int32_t r0, r1, c0, c1;
static double adj, adj0, adj1, pos0, pos1;
static double rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;
pos0 = src_pos0 - _axis[axis].adj_offset;
// line adjustment
if ( rel_axis1 < 0 )
{
r0 = (int32_t) (pos0 / adj_step_size_i);
if ( pos0 < 0 ) r0--;
r1 = r0 + 1;
adj0 = r0 >= 0 && r0 < adj_steps_i ? *_adj[axis][r0] : 0.0;
adj1 = r1 >= 0 && r1 < adj_steps_i ? *_adj[axis][r1] : 0.0;
adj = adj0 + (adj1 - adj0) * (pos0 - adj_step_size_i * r0) / adj_step_size_i;
}
// plane adjustment
else
{
pos1 = src_pos1 - _axis[axis].adj_offset;
// get row data
r0 = (int32_t) (pos0 / adj_step_size_i);
if ( pos0 < 0 ) r0--;
r1 = r0 + 1;
r0_valid = r0 >= 0 && r0 < adj_steps_i;
r1_valid = r1 >= 0 && r1 < adj_steps_i;
// get column data
c0 = (int32_t) (pos1 / adj_step_size_i);
if ( pos1 < 0 ) c0--;
c1 = c0 + 1;
c0_valid = c0 >= 0 && c0 < adj_steps_i;
c1_valid = c1 >= 0 && c1 < adj_steps_i;
// get values of 4 adjustment cells around current position
r0c0_adj = r0_valid && c0_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c0] : 0.0;
r0c1_adj = r0_valid && c1_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c1] : 0.0;
r1c0_adj = r1_valid && c0_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c0] : 0.0;
r1c1_adj = r1_valid && c1_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c1] : 0.0;
// calculate row/column multipliers
rm = (pos0 - r0 * adj_step_size_i) / adj_step_size_i;
cm = (pos1 - c0 * adj_step_size_i) / adj_step_size_i;
adj0 = (r0c0_adj + (r0c1_adj - r0c0_adj) * cm)
+ ( (r1c0_adj + (r1c1_adj - r1c0_adj) * cm)
- (r0c0_adj + (r0c1_adj - r0c0_adj) * cm) ) * rm;
adj1 = (r0c0_adj + (r1c0_adj - r0c0_adj) * rm)
+ ( (r0c1_adj + (r1c1_adj - r0c1_adj) * rm)
- (r0c0_adj + (r1c0_adj - r0c0_adj) * rm) ) * cm;
adj = (adj0 + adj1) / 2;
}
return adj;
}
static int axis_name_to_id(char axis_char)
{
int axis;
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( axis_char == axis_name0[axis] || axis_char == axis_name1[axis] ) return axis;
}
return -1;
}
int rtapi_app_main(void)
{
int32_t retval, axis, joint, id, axis_list[EMCMOT_MAX_JOINTS] = {-1};
int32_t r, c;
char *data, *token;
// component init
comp_id = hal_init(comp_name);
if ( comp_id < 0 ) return -1;
#if FOR_MACHINEKIT
// export kinematics table
vtable_id = hal_export_vtable(comp_name, VTVERSION, &vtk, comp_id);
if ( vtable_id < 0 )
{
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
comp_name, comp_name, VTVERSION, &vtk, vtable_id);
return -1;
}
#endif
// shared memory allocation
_axis = hal_malloc(EMCMOT_MAX_JOINTS * sizeof(axis_t));
if ( !_axis ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// parse `kinstype`
switch (*kinstype)
{
case 'b': case 'B': ktype = KINEMATICS_BOTH; break;
case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
case '1': default: ktype = KINEMATICS_IDENTITY;
}
// parse `coordinates`
for ( joint = 0, token = coordinates; *token && joint < EMCMOT_MAX_JOINTS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS )
{
_axis[axis].enable = 1;
_axis[axis].joint_id[ _axis[axis].joint_cnt++ ] = joint;
}
joint++;
}
// parse `adjust`
for ( id = 0, token = adjust; *token && id < EMCMOT_MAX_JOINTS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS && _axis[axis].enable )
{
_axis[axis].adj_enable = 1;
axis_list[id] = axis;
}
id++;
}
// parse `from`
#if FOR_MACHINEKIT
for ( id = 0, data = from; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = from; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
_axis[axis].adj_from[0] = axis_name_to_id(token[0]);
_axis[axis].adj_from[1] = axis_name_to_id(token[1]);
if ( _axis[axis].adj_from[0] < 0 ) _axis[axis].adj_enable = 0;
}
// parse `steps`
#if FOR_MACHINEKIT
for ( id = 0, data = steps; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = steps; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_steps = (hal_u32_t) strtoul(token, NULL, 10);
#else
retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_steps);
#endif
if ( !_axis[axis].adj_steps ) _axis[axis].adj_enable = 0;
if ( _axis[axis].adj_steps > ADJ_STEPS_MAX ) _axis[axis].adj_steps = ADJ_STEPS_MAX;
}
// parse `step_size`
#if FOR_MACHINEKIT
for ( id = 0, data = step_size; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = step_size; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_step_size = (hal_u32_t) strtoul(token, NULL, 10);
#else
retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_step_size);
#endif
if ( !_axis[axis].adj_step_size ) _axis[axis].adj_enable = 0;
}
// parse `offset`
#if FOR_MACHINEKIT
for ( id = 0, data = offset; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = offset; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_offset = (hal_s32_t) strtol(token, NULL, 10);
#else
retval = kstrtos32(token, 10, (int32_t*) &_axis[axis].adj_offset);
#endif
}
// mem alloc, export pins
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].adj_enable ) continue;
// plane adjustment
if ( _axis[axis].adj_from[1] >= 0 )
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * _axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
for ( c = 0; c < _axis[axis].adj_steps; c++ )
{
retval += hal_pin_float_newf( HAL_IN,
&(_adj[axis][r * _axis[axis].adj_steps + c]), comp_id,
"%s.adj%c.%c%d_%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset),
axis_name1[ _axis[axis].adj_from[1] ],
(c * _axis[axis].adj_step_size + _axis[axis].adj_offset)
);
}
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL plane step pins export failed",-1);
}
// line adjustment
else
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
retval += hal_pin_float_newf( HAL_IN, &_adj[axis][r], comp_id,
"%s.adj%c.%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset) );
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL line step pins export failed",-1);
}
}
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
#if FOR_MACHINEKIT
hal_remove_vtable(vtable_id);
#endif
hal_exit(comp_id);
}
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Актуальное решение в шапке темы viewtopic.php?p=634350#p634350
- verser
- Мастер
- Сообщения: 1895
- Зарегистрирован: 21 июл 2013, 22:28
- Репутация: 1282
- Настоящее имя: Сергей
- Откуда: Тбилиси
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Эта хорошая выравнивалка, которой я пользовался, спасибо MX_Master за эту работу!, больше не работает при переходе на LinuxCNC v2.9.
Может кто-нибудь уже адаптировал?
Может кто-нибудь уже адаптировал?
- MX_Master
- Мастер
- Сообщения: 7480
- Зарегистрирован: 27 июн 2015, 19:45
- Репутация: 3101
- Настоящее имя: Михаил
- Откуда: Алматы
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
А чё там не работает или поменялось в 2.9? Или, просто, не компилируется?
- verser
- Мастер
- Сообщения: 1895
- Зарегистрирован: 21 июл 2013, 22:28
- Репутация: 1282
- Настоящее имя: Сергей
- Откуда: Тбилиси
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Да, не компилируется. Что-то изменилось.
- MX_Master
- Мастер
- Сообщения: 7480
- Зарегистрирован: 27 июн 2015, 19:45
- Репутация: 3101
- Настоящее имя: Михаил
- Откуда: Алматы
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Подправил функции получения чисел из строк. Корректно собирается на PREEMPT_RT ядре 6.6.21. В работе не тестировал!
Код: Выделить всё
/********************************************************************
* Description: krivkins.c
*
* Trivial kinematics for 3 axis Cartesian machine
* with position adjustment
*
* Author: Mikhail Vydrenko (mikhail@vydrenko.ru)
* Date: 21.04.2024
********************************************************************/
#define FOR_MACHINEKIT 0
#include "motion.h"
#include "kinematics.h" /* these decls */
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
#include "rtapi_math.h"
#include "rtapi_string.h"
#include <linux/kernel.h>
#include <stdlib.h>
#define ADJ_STEPS_MAX 255
#define ADJ_FORWARD 0
#define ADJ_INVERSE 1
#define PRINT_ERROR(MSG) \
rtapi_print_msg(RTAPI_MSG_ERR, "%s: "MSG"\n", comp_name)
#define PRINT_ERROR_AND_RETURN(MSG,RETVAL) \
{ PRINT_ERROR(MSG); return RETVAL; }
#if FOR_MACHINEKIT
#define VTVERSION VTKINEMATICS_VERSION1
#endif
MODULE_AUTHOR("Mikhail Vydrenko");
MODULE_DESCRIPTION("Trivial kinematics with position adjustment");
MODULE_LICENSE("GPL");
typedef struct
{
hal_bit_t enable;
hal_u32_t joint_id[EMCMOT_MAX_JOINTS];
hal_u32_t joint_cnt;
hal_bit_t adj_enable;
hal_s32_t adj_from[2];
hal_s32_t adj_offset;
hal_u32_t adj_steps;
hal_u32_t adj_step_size;
} axis_t;
#if FOR_MACHINEKIT
static int vtable_id;
#endif
static int comp_id;
static const char *comp_name = "krivkins";
static const char *axis_name0 = "xyzabcuvw";
static const char *axis_name1 = "XYZABCUVW";
static KINEMATICS_TYPE ktype = -1;
static axis_t * _axis;
static hal_float_t ** _adj[EMCMOT_MAX_JOINTS];
static char *coordinates = "XYZABCUVW";
RTAPI_MP_STRING(coordinates, "Existing Axes");
static char *kinstype = "1"; // use KINEMATICS_IDENTITY
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");
static char *adjust = "";
RTAPI_MP_STRING(adjust, "adjust axis name/ID, comma separated");
static char *from = "";
RTAPI_MP_STRING(from, "related axes names/IDs, comma separated");
static char *offset = "";
RTAPI_MP_STRING(offset, "adjust from position offset in units, comma separated");
static char *steps = "";
RTAPI_MP_STRING(steps, "steps count, comma separated");
static char *step_size = "";
RTAPI_MP_STRING(step_size, "step size in units, comma separated");
double get_adj(uint8_t axis, double src_pos0, double src_pos1);
int
kinematicsForward
(
const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags
)
{
static int32_t axis;
double* axis_pos[EMCMOT_MAX_JOINTS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].enable ) continue;
*axis_pos[axis] = joints[ _axis[axis].joint_id[0] ];
#if ADJ_FORWARD
if ( !_axis[axis].adj_enable ) continue;
if ( _axis[axis].adj_from[1] < 0 )
{
*axis_pos[axis] += get_adj(
axis,
joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
0.0);
}
else
{
*axis_pos[axis] += get_adj(
axis,
joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
joints[ _axis[ _axis[axis].adj_from[1] ].joint_id[0] ]);
}
#endif
}
return 0;
}
int
kinematicsInverse
(
const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags
)
{
static int32_t axis, n;
const double* axis_pos[EMCMOT_MAX_JOINTS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].enable ) continue;
for ( n = _axis[axis].joint_cnt; n--; )
{
joints[ _axis[axis].joint_id[n] ] = *axis_pos[axis];
#if ADJ_INVERSE
if ( !_axis[axis].adj_enable ) continue;
if ( _axis[axis].adj_from[1] < 0 )
{
joints[ _axis[axis].joint_id[n] ] -= get_adj(
axis,
*axis_pos[ _axis[axis].adj_from[0] ],
0.0);
}
else
{
joints[ _axis[axis].joint_id[n] ] -= get_adj(
axis,
*axis_pos[ _axis[axis].adj_from[0] ],
*axis_pos[ _axis[axis].adj_from[1] ]);
}
#endif
}
}
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 ktype;
}
#if FOR_MACHINEKIT
static vtkins_t vtk =
{
.kinematicsForward = kinematicsForward,
.kinematicsInverse = kinematicsInverse,
// .kinematicsHome = kinematicsHome,
.kinematicsType = kinematicsType
};
#else
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
#endif
#define adj_offset_i _axis[axis].adj_offset
#define adj_steps_i _axis[axis].adj_steps
#define adj_step_size_i _axis[axis].adj_step_size
#define rel_axis0 _axis[axis].adj_from[0]
#define rel_axis1 _axis[axis].adj_from[1]
double get_adj(uint8_t axis, double src_pos0, double src_pos1)
{
static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
static int32_t r0, r1, c0, c1;
static double adj, adj0, adj1, pos0, pos1;
static double rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;
pos0 = src_pos0 - _axis[axis].adj_offset;
// line adjustment
if ( rel_axis1 < 0 )
{
r0 = (int32_t) (pos0 / adj_step_size_i);
if ( pos0 < 0 ) r0--;
r1 = r0 + 1;
adj0 = r0 >= 0 && r0 < adj_steps_i ? *_adj[axis][r0] : 0.0;
adj1 = r1 >= 0 && r1 < adj_steps_i ? *_adj[axis][r1] : 0.0;
adj = adj0 + (adj1 - adj0) * (pos0 - adj_step_size_i * r0) / adj_step_size_i;
}
// plane adjustment
else
{
pos1 = src_pos1 - _axis[axis].adj_offset;
// get row data
r0 = (int32_t) (pos0 / adj_step_size_i);
if ( pos0 < 0 ) r0--;
r1 = r0 + 1;
r0_valid = r0 >= 0 && r0 < adj_steps_i;
r1_valid = r1 >= 0 && r1 < adj_steps_i;
// get column data
c0 = (int32_t) (pos1 / adj_step_size_i);
if ( pos1 < 0 ) c0--;
c1 = c0 + 1;
c0_valid = c0 >= 0 && c0 < adj_steps_i;
c1_valid = c1 >= 0 && c1 < adj_steps_i;
// get values of 4 adjustment cells around current position
r0c0_adj = r0_valid && c0_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c0] : 0.0;
r0c1_adj = r0_valid && c1_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c1] : 0.0;
r1c0_adj = r1_valid && c0_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c0] : 0.0;
r1c1_adj = r1_valid && c1_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c1] : 0.0;
// calculate row/column multipliers
rm = (pos0 - r0 * adj_step_size_i) / adj_step_size_i;
cm = (pos1 - c0 * adj_step_size_i) / adj_step_size_i;
adj0 = (r0c0_adj + (r0c1_adj - r0c0_adj) * cm)
+ ( (r1c0_adj + (r1c1_adj - r1c0_adj) * cm)
- (r0c0_adj + (r0c1_adj - r0c0_adj) * cm) ) * rm;
adj1 = (r0c0_adj + (r1c0_adj - r0c0_adj) * rm)
+ ( (r0c1_adj + (r1c1_adj - r0c1_adj) * rm)
- (r0c0_adj + (r1c0_adj - r0c0_adj) * rm) ) * cm;
adj = (adj0 + adj1) / 2;
}
return adj;
}
static int axis_name_to_id(char axis_char)
{
int axis;
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( axis_char == axis_name0[axis] || axis_char == axis_name1[axis] ) return axis;
}
return -1;
}
int rtapi_app_main(void)
{
int32_t retval, axis, joint, id, axis_list[EMCMOT_MAX_JOINTS] = {-1};
int32_t r, c;
char *data, *token;
// component init
comp_id = hal_init(comp_name);
if ( comp_id < 0 ) return -1;
#if FOR_MACHINEKIT
// export kinematics table
vtable_id = hal_export_vtable(comp_name, VTVERSION, &vtk, comp_id);
if ( vtable_id < 0 )
{
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
comp_name, comp_name, VTVERSION, &vtk, vtable_id);
return -1;
}
#endif
// shared memory allocation
_axis = hal_malloc(EMCMOT_MAX_JOINTS * sizeof(axis_t));
if ( !_axis ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// parse `kinstype`
switch (*kinstype)
{
case 'b': case 'B': ktype = KINEMATICS_BOTH; break;
case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
case '1': default: ktype = KINEMATICS_IDENTITY;
}
// parse `coordinates`
for ( joint = 0, token = coordinates; *token && joint < EMCMOT_MAX_JOINTS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS )
{
_axis[axis].enable = 1;
_axis[axis].joint_id[ _axis[axis].joint_cnt++ ] = joint;
}
joint++;
}
// parse `adjust`
for ( id = 0, token = adjust; *token && id < EMCMOT_MAX_JOINTS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_JOINTS && _axis[axis].enable )
{
_axis[axis].adj_enable = 1;
axis_list[id] = axis;
}
id++;
}
// parse `from`
#if FOR_MACHINEKIT
for ( id = 0, data = from; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = from; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
_axis[axis].adj_from[0] = axis_name_to_id(token[0]);
_axis[axis].adj_from[1] = axis_name_to_id(token[1]);
if ( _axis[axis].adj_from[0] < 0 ) _axis[axis].adj_enable = 0;
}
// parse `steps`
#if FOR_MACHINEKIT
for ( id = 0, data = steps; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = steps; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
_axis[axis].adj_steps = (hal_u32_t) strtoul(token, NULL, 10);
if ( !_axis[axis].adj_steps ) _axis[axis].adj_enable = 0;
if ( _axis[axis].adj_steps > ADJ_STEPS_MAX ) _axis[axis].adj_steps = ADJ_STEPS_MAX;
}
// parse `step_size`
#if FOR_MACHINEKIT
for ( id = 0, data = step_size; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = step_size; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
_axis[axis].adj_step_size = (hal_u32_t) strtoul(token, NULL, 10);
if ( !_axis[axis].adj_step_size ) _axis[axis].adj_enable = 0;
}
// parse `offset`
#if FOR_MACHINEKIT
for ( id = 0, data = offset; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = offset; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
_axis[axis].adj_offset = (hal_s32_t) strtol(token, NULL, 10);
}
// mem alloc, export pins
for ( axis = EMCMOT_MAX_JOINTS; axis--; )
{
if ( !_axis[axis].adj_enable ) continue;
// plane adjustment
if ( _axis[axis].adj_from[1] >= 0 )
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * _axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
for ( c = 0; c < _axis[axis].adj_steps; c++ )
{
retval += hal_pin_float_newf( HAL_IN,
&(_adj[axis][r * _axis[axis].adj_steps + c]), comp_id,
"%s.adj%c.%c%d_%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset),
axis_name1[ _axis[axis].adj_from[1] ],
(c * _axis[axis].adj_step_size + _axis[axis].adj_offset)
);
}
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL plane step pins export failed",-1);
}
// line adjustment
else
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
retval += hal_pin_float_newf( HAL_IN, &_adj[axis][r], comp_id,
"%s.adj%c.%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset) );
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL line step pins export failed",-1);
}
}
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
#if FOR_MACHINEKIT
hal_remove_vtable(vtable_id);
#endif
hal_exit(comp_id);
}
- Вложения
-
- krivkins.c
- (14.18 КБ) 18 скачиваний
- verser
- Мастер
- Сообщения: 1895
- Зарегистрирован: 21 июл 2013, 22:28
- Репутация: 1282
- Настоящее имя: Сергей
- Откуда: Тбилиси
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Спасибо!
Скомпилировалось успешно.
При запуске ругается:
Скомпилировалось успешно.
При запуске ругается:
мои пины в .hal:check_config: Unchecked: [KINS]KINEMATICS={krivkins coordinates=XYZ adjust=X,Y from=Y,Z steps=2,2 step_size=245,245 offset=1,1}
Starting LinuxCNC...
linuxcnc TPMOD=tpmod HOMEMOD=homemod EMCMOT=motmod
Note: Using POSIX realtime
Found file(REL): ./core_sim.hal
HAL: ERROR: duplicate variable 'krivkins.adjr.i1_'
HAL: ERROR: duplicate variable 'krivkins.adjr.i246_'
krivkins: HAL plane step pins export failed
krivkins: rtapi_app_main: Operation not permitted (-1)
./core_sim.hal:5: waitpid failed /usr/bin/rtapi_app krivkins
./core_sim.hal:5: /usr/bin/rtapi_app exited without becoming ready
./core_sim.hal:5: insmod for krivkins failed, returned -1
Shutting down and cleaning up LinuxCNC...
krivkins: not loaded
<commandline>:0: exit value: 255
<commandline>:0: rmmod failed, returned -1
<commandline>:0: unloadrt failed
setp krivkins.adjX.Y1 0
setp krivkins.adjX.Y246 0.245
setp krivkins.adjY.Z1 0
setp krivkins.adjY.Z246 -0.515
- MX_Master
- Мастер
- Сообщения: 7480
- Зарегистрирован: 27 июн 2015, 19:45
- Репутация: 3101
- Настоящее имя: Михаил
- Откуда: Алматы
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
А, понял в чём дело. Заменил EMCMOT_MAX_JOINTS (раньше было 9, стало 16) на EMCMOT_MAX_AXIS (сейчас это 9). Ну и всякие strtol()/kstrtou32() заменил на современные ядрёные simple_strtol().
Код: Выделить всё
/********************************************************************
* Description: krivkins.c
*
* Trivial kinematics for 3 axis Cartesian machine
* with position adjustment
*
* Author: Mikhail Vydrenko (mikhail@vydrenko.ru)
********************************************************************/
#define FOR_MACHINEKIT 0
#include "motion.h"
#include "kinematics.h" /* these decls */
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
#if FOR_MACHINEKIT
#include <stdlib.h>
#else
#include "rtapi_math.h"
#include "rtapi_string.h"
#include <linux/kernel.h>
#endif
#define ADJ_STEPS_MAX 255
#define ADJ_FORWARD 0
#define ADJ_INVERSE 1
#define PRINT_ERROR(MSG) \
rtapi_print_msg(RTAPI_MSG_ERR, "%s: "MSG"\n", comp_name)
#define PRINT_ERROR_AND_RETURN(MSG,RETVAL) \
{ PRINT_ERROR(MSG); return RETVAL; }
#if FOR_MACHINEKIT
#define VTVERSION VTKINEMATICS_VERSION1
#endif
MODULE_AUTHOR("Mikhail Vydrenko");
MODULE_DESCRIPTION("Trivial kinematics with position adjustment");
MODULE_LICENSE("GPL");
typedef struct
{
hal_bit_t enable;
hal_u32_t joint_id[EMCMOT_MAX_AXIS];
hal_u32_t joint_cnt;
hal_bit_t adj_enable;
hal_s32_t adj_from[2];
hal_s32_t adj_offset;
hal_u32_t adj_steps;
hal_u32_t adj_step_size;
} axis_t;
#if FOR_MACHINEKIT
static int vtable_id;
#endif
static int comp_id;
static const char *comp_name = "krivkins";
static const char *axis_name0 = "xyzabcuvw";
static const char *axis_name1 = "XYZABCUVW";
static KINEMATICS_TYPE ktype = -1;
static axis_t * _axis;
static hal_float_t ** _adj[EMCMOT_MAX_AXIS];
static char *coordinates = "XYZABCUVW";
RTAPI_MP_STRING(coordinates, "Existing Axes");
static char *kinstype = "1"; // use KINEMATICS_IDENTITY
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");
static char *adjust = "";
RTAPI_MP_STRING(adjust, "adjust axis name/ID, comma separated");
static char *from = "";
RTAPI_MP_STRING(from, "related axes names/IDs, comma separated");
static char *offset = "";
RTAPI_MP_STRING(offset, "adjust from position offset in units, comma separated");
static char *steps = "";
RTAPI_MP_STRING(steps, "steps count, comma separated");
static char *step_size = "";
RTAPI_MP_STRING(step_size, "step size in units, comma separated");
double get_adj(uint8_t axis, double src_pos0, double src_pos1);
int
kinematicsForward
(
const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags
)
{
static int32_t axis;
double* axis_pos[EMCMOT_MAX_AXIS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
for ( axis = EMCMOT_MAX_AXIS; axis--; )
{
if ( !_axis[axis].enable ) continue;
*axis_pos[axis] = joints[ _axis[axis].joint_id[0] ];
#if ADJ_FORWARD
if ( !_axis[axis].adj_enable ) continue;
if ( _axis[axis].adj_from[1] < 0 )
{
*axis_pos[axis] += get_adj(
axis,
joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
0.0);
}
else
{
*axis_pos[axis] += get_adj(
axis,
joints[ _axis[ _axis[axis].adj_from[0] ].joint_id[0] ],
joints[ _axis[ _axis[axis].adj_from[1] ].joint_id[0] ]);
}
#endif
}
return 0;
}
int
kinematicsInverse
(
const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags
)
{
static int32_t axis, n;
const double* axis_pos[EMCMOT_MAX_AXIS] =
{
&(pos->tran.x), &(pos->tran.y), &(pos->tran.z),
&(pos->a), &(pos->b), &(pos->c),
&(pos->u), &(pos->v), &(pos->w)
};
for ( axis = EMCMOT_MAX_AXIS; axis--; )
{
if ( !_axis[axis].enable ) continue;
for ( n = _axis[axis].joint_cnt; n--; )
{
joints[ _axis[axis].joint_id[n] ] = *axis_pos[axis];
#if ADJ_INVERSE
if ( !_axis[axis].adj_enable ) continue;
if ( _axis[axis].adj_from[1] < 0 )
{
joints[ _axis[axis].joint_id[n] ] -= get_adj(
axis,
*axis_pos[ _axis[axis].adj_from[0] ],
0.0);
}
else
{
joints[ _axis[axis].joint_id[n] ] -= get_adj(
axis,
*axis_pos[ _axis[axis].adj_from[0] ],
*axis_pos[ _axis[axis].adj_from[1] ]);
}
#endif
}
}
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 ktype;
}
#if FOR_MACHINEKIT
static vtkins_t vtk =
{
.kinematicsForward = kinematicsForward,
.kinematicsInverse = kinematicsInverse,
// .kinematicsHome = kinematicsHome,
.kinematicsType = kinematicsType
};
#else
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
#endif
#define adj_offset_i _axis[axis].adj_offset
#define adj_steps_i _axis[axis].adj_steps
#define adj_step_size_i _axis[axis].adj_step_size
#define rel_axis0 _axis[axis].adj_from[0]
#define rel_axis1 _axis[axis].adj_from[1]
double get_adj(uint8_t axis, double src_pos0, double src_pos1)
{
static hal_bit_t r0_valid, r1_valid, c0_valid, c1_valid;
static int32_t r0, r1, c0, c1;
static double adj, adj0, adj1, pos0, pos1;
static double rm, cm, r0c0_adj, r0c1_adj, r1c0_adj, r1c1_adj;
pos0 = src_pos0 - _axis[axis].adj_offset;
// line adjustment
if ( rel_axis1 < 0 )
{
r0 = (int32_t) (pos0 / adj_step_size_i);
if ( pos0 < 0 ) r0--;
r1 = r0 + 1;
adj0 = r0 >= 0 && r0 < adj_steps_i ? *_adj[axis][r0] : 0.0;
adj1 = r1 >= 0 && r1 < adj_steps_i ? *_adj[axis][r1] : 0.0;
adj = adj0 + (adj1 - adj0) * (pos0 - adj_step_size_i * r0) / adj_step_size_i;
}
// plane adjustment
else
{
pos1 = src_pos1 - _axis[axis].adj_offset;
// get row data
r0 = (int32_t) (pos0 / adj_step_size_i);
if ( pos0 < 0 ) r0--;
r1 = r0 + 1;
r0_valid = r0 >= 0 && r0 < adj_steps_i;
r1_valid = r1 >= 0 && r1 < adj_steps_i;
// get column data
c0 = (int32_t) (pos1 / adj_step_size_i);
if ( pos1 < 0 ) c0--;
c1 = c0 + 1;
c0_valid = c0 >= 0 && c0 < adj_steps_i;
c1_valid = c1 >= 0 && c1 < adj_steps_i;
// get values of 4 adjustment cells around current position
r0c0_adj = r0_valid && c0_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c0] : 0.0;
r0c1_adj = r0_valid && c1_valid ? *_adj[axis][r0*_axis[axis].adj_steps + c1] : 0.0;
r1c0_adj = r1_valid && c0_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c0] : 0.0;
r1c1_adj = r1_valid && c1_valid ? *_adj[axis][r1*_axis[axis].adj_steps + c1] : 0.0;
// calculate row/column multipliers
rm = (pos0 - r0 * adj_step_size_i) / adj_step_size_i;
cm = (pos1 - c0 * adj_step_size_i) / adj_step_size_i;
adj0 = (r0c0_adj + (r0c1_adj - r0c0_adj) * cm)
+ ( (r1c0_adj + (r1c1_adj - r1c0_adj) * cm)
- (r0c0_adj + (r0c1_adj - r0c0_adj) * cm) ) * rm;
adj1 = (r0c0_adj + (r1c0_adj - r0c0_adj) * rm)
+ ( (r0c1_adj + (r1c1_adj - r0c1_adj) * rm)
- (r0c0_adj + (r1c0_adj - r0c0_adj) * rm) ) * cm;
adj = (adj0 + adj1) / 2;
}
return adj;
}
static int axis_name_to_id(char axis_char)
{
int axis;
for ( axis = EMCMOT_MAX_AXIS; axis--; )
{
if ( axis_char == axis_name0[axis] || axis_char == axis_name1[axis] ) return axis;
}
return -1;
}
int rtapi_app_main(void)
{
int32_t retval, axis, joint, id, axis_list[EMCMOT_MAX_AXIS] = {-1};
int32_t r, c;
char *data, *token;
// component init
comp_id = hal_init(comp_name);
if ( comp_id < 0 ) return -1;
#if FOR_MACHINEKIT
// export kinematics table
vtable_id = hal_export_vtable(comp_name, VTVERSION, &vtk, comp_id);
if ( vtable_id < 0 )
{
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
comp_name, comp_name, VTVERSION, &vtk, vtable_id);
return -1;
}
#endif
// shared memory allocation
_axis = hal_malloc(EMCMOT_MAX_AXIS * sizeof(axis_t));
if ( !_axis ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// parse `kinstype`
switch (*kinstype)
{
case 'b': case 'B': ktype = KINEMATICS_BOTH; break;
case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
case '1': default: ktype = KINEMATICS_IDENTITY;
}
// parse `coordinates`
for ( joint = 0, token = coordinates; *token && joint < EMCMOT_MAX_AXIS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_AXIS )
{
_axis[axis].enable = 1;
_axis[axis].joint_id[ _axis[axis].joint_cnt++ ] = joint;
}
joint++;
}
// parse `adjust`
for ( id = 0, token = adjust; *token && id < EMCMOT_MAX_AXIS; token++ )
{
if ( *token == ' ' || *token == '\t' || *token == ',' ) continue;
axis = axis_name_to_id(*token);
if ( axis >= 0 && axis < EMCMOT_MAX_AXIS && _axis[axis].enable )
{
_axis[axis].adj_enable = 1;
axis_list[id] = axis;
}
id++;
}
// parse `from`
#if FOR_MACHINEKIT
for ( id = 0, data = from; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = from; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
_axis[axis].adj_from[0] = axis_name_to_id(token[0]);
_axis[axis].adj_from[1] = axis_name_to_id(token[1]);
if ( _axis[axis].adj_from[0] < 0 ) _axis[axis].adj_enable = 0;
}
// parse `steps`
#if FOR_MACHINEKIT
for ( id = 0, data = steps; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = steps; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_steps = (hal_u32_t) strtoul(token, NULL, 10);
#else
//retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_steps);
_axis[axis].adj_steps = (hal_u32_t) simple_strtol(token, NULL, 10);
#endif
if ( !_axis[axis].adj_steps ) _axis[axis].adj_enable = 0;
if ( _axis[axis].adj_steps > ADJ_STEPS_MAX ) _axis[axis].adj_steps = ADJ_STEPS_MAX;
}
// parse `step_size`
#if FOR_MACHINEKIT
for ( id = 0, data = step_size; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = step_size; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_step_size = (hal_u32_t) strtoul(token, NULL, 10);
#else
//retval = kstrtou32(token, 10, (uint32_t*) &_axis[axis].adj_step_size);
_axis[axis].adj_step_size = (hal_u32_t) simple_strtol(token, NULL, 10);
#endif
if ( !_axis[axis].adj_step_size ) _axis[axis].adj_enable = 0;
}
// parse `offset`
#if FOR_MACHINEKIT
for ( id = 0, data = offset; (token = strtok(data, ",")) != NULL; id++ )
{
if ( data != NULL ) data = NULL;
#else
for ( id = 0, data = offset; (token = strsep(&data, ",")) != NULL; id++ )
{
#endif
axis = axis_list[id];
if ( axis < 0 || !_axis[axis].adj_enable ) break;
#if FOR_MACHINEKIT
_axis[axis].adj_offset = (hal_s32_t) strtol(token, NULL, 10);
#else
//retval = simple_strtol(token, 10, (int32_t*) &_axis[axis].adj_offset);
_axis[axis].adj_offset = (hal_s32_t) simple_strtol(token, NULL, 10);
#endif
}
// mem alloc, export pins
for ( axis = EMCMOT_MAX_AXIS; axis--; )
{
if ( !_axis[axis].adj_enable ) continue;
// plane adjustment
if ( _axis[axis].adj_from[1] >= 0 )
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * _axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
for ( c = 0; c < _axis[axis].adj_steps; c++ )
{
retval += hal_pin_float_newf( HAL_IN,
&(_adj[axis][r * _axis[axis].adj_steps + c]), comp_id,
"%s.adj%c.%c%d_%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset),
axis_name1[ _axis[axis].adj_from[1] ],
(c * _axis[axis].adj_step_size + _axis[axis].adj_offset)
);
}
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL plane step pins export failed",-1);
}
// line adjustment
else
{
// mem alloc
_adj[axis] = hal_malloc(_axis[axis].adj_steps * sizeof(hal_float_t*));
if ( !_adj[axis] ) PRINT_ERROR_AND_RETURN("hal_malloc() failed",-1);
// HAL pins export
for ( r = 0, retval = 0; r < _axis[axis].adj_steps; r++ )
{
retval += hal_pin_float_newf( HAL_IN, &_adj[axis][r], comp_id,
"%s.adj%c.%c%d", comp_name, axis_name1[axis],
axis_name1[ _axis[axis].adj_from[0] ],
(r * _axis[axis].adj_step_size + _axis[axis].adj_offset) );
}
if ( retval ) PRINT_ERROR_AND_RETURN("HAL line step pins export failed",-1);
}
}
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void)
{
#if FOR_MACHINEKIT
hal_remove_vtable(vtable_id);
#endif
hal_exit(comp_id);
}
- Вложения
-
- krivkins.c
- (14.72 КБ) 15 скачиваний
- verser
- Мастер
- Сообщения: 1895
- Зарегистрирован: 21 июл 2013, 22:28
- Репутация: 1282
- Настоящее имя: Сергей
- Откуда: Тбилиси
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Супер! На виртуалке заработало.
Сперва ругнулась
Сперва ругнулась
я всунул макрос KINS_NOT_SWITCHABLE и пошлоmotmod: dlopen: /usr/lib/linuxcnc/modules/motmod.so: undefined symbol: kinematicsSwitch
- verser
- Мастер
- Сообщения: 1895
- Зарегистрирован: 21 июл 2013, 22:28
- Репутация: 1282
- Настоящее имя: Сергей
- Откуда: Тбилиси
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Итак, при переходе на linuxcnc 2.9 в работе krivkins появились 2 проблемы:
1. Подстройка зависимой оси стала отображаться в DRO (двигаются и abs, и G54), что портит весь смысл корректировки.
2. При сбросе в 0 (в G54) оси XY самопроизвольно смещаются на несколько соток, отрисовывают это смещение в DRO, и показывают не 0, а это смещение. При повторном сбросе в 0 это смещение и цифры в DRO нарастают.
Михаил, может будут идеи, пожалуйста дай знать.
1. Подстройка зависимой оси стала отображаться в DRO (двигаются и abs, и G54), что портит весь смысл корректировки.
2. При сбросе в 0 (в G54) оси XY самопроизвольно смещаются на несколько соток, отрисовывают это смещение в DRO, и показывают не 0, а это смещение. При повторном сбросе в 0 это смещение и цифры в DRO нарастают.
Михаил, может будут идеи, пожалуйста дай знать.
- MX_Master
- Мастер
- Сообщения: 7480
- Зарегистрирован: 27 июн 2015, 19:45
- Репутация: 3101
- Настоящее имя: Михаил
- Откуда: Алматы
- Контактная информация:
Re: [РЕШЕНО] LinuxCNC2.8 и выше + Программная коррекция перпендикулярности осей
Я ненадолго заглянул в код логики и понял, что там без пузыря не разобраться легче заново всё сделать с блэкджеком и шлю.. Но надо всё продумать