From 676f429832e010b8b6416f38280ff7a762616fc7 Mon Sep 17 00:00:00 2001 From: Vlad A Date: Wed, 15 Mar 2023 20:18:59 -0400 Subject: [PATCH 1/3] Add new Kinematic system with skew correction New system adds ability to tweak skew correction, for cases when physical geometry of CNC machine is not ideal. Signed-off-by: Vlad A --- FluidNC/src/Kinematics/Cartesian.h | 1 + FluidNC/src/Kinematics/GenericCartesian.cpp | 187 ++++++++++++++++++ FluidNC/src/Kinematics/GenericCartesian.h | 91 +++++++++ FluidNC/src/Kinematics/Skewed.cpp | 132 +++++++++++++ FluidNC/src/Kinematics/Skewed.h | 62 ++++++ FluidNC/src/Machine/MachineConfig.h | 2 +- .../test/Configuration/YamlTreeBuilder.cpp | 8 +- 7 files changed, 478 insertions(+), 5 deletions(-) create mode 100644 FluidNC/src/Kinematics/GenericCartesian.cpp create mode 100644 FluidNC/src/Kinematics/GenericCartesian.h create mode 100644 FluidNC/src/Kinematics/Skewed.cpp create mode 100644 FluidNC/src/Kinematics/Skewed.h diff --git a/FluidNC/src/Kinematics/Cartesian.h b/FluidNC/src/Kinematics/Cartesian.h index 50476241a..aeb5d11b5 100644 --- a/FluidNC/src/Kinematics/Cartesian.h +++ b/FluidNC/src/Kinematics/Cartesian.h @@ -7,6 +7,7 @@ Cartesian.h This is a kinematic system for where the motors operate in the cartesian space. + All logical axis of the system perfectly alligned with physical axis of machine. */ #include "Kinematics.h" diff --git a/FluidNC/src/Kinematics/GenericCartesian.cpp b/FluidNC/src/Kinematics/GenericCartesian.cpp new file mode 100644 index 000000000..e2cc9bf6f --- /dev/null +++ b/FluidNC/src/Kinematics/GenericCartesian.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2020 - Bart Dring +// Copyright (c) 2023 - Vlad A. +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#include "GenericCartesian.h" + +#include "src/Machine/MachineConfig.h" +#include "src/Machine/Axes.h" // ambiguousLimit() +//#include "Skew.h" // Skew, SkewAxis +#include "src/Limits.h" + +namespace Kinematics { + void GenericCartesian::init() { + log_info("Kinematic system: " << name()); + init_position(); + } + + // Initialize the machine position + void GenericCartesian::init_position() { + auto n_axis = config->_axes->_numberAxis; + for (size_t axis = 0; axis < n_axis; axis++) { + set_motor_steps(axis, 0); // Set to zeros + } + } + + bool GenericCartesian::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { + if ( _mtx ) { + _mtx->transform( _buffer, target ); + return mc_move_motors( _buffer, pl_data); + } else + // Without skew correction motor space is the same cartesian space, so we do no transform. + return mc_move_motors(target, pl_data); + } + + void GenericCartesian::motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + if ( _rev ) { + _rev->transform( cartesian, motors ); + } + else + // Without skew correction motor space is the same cartesian space, so we do no transform. + copyAxes(cartesian, motors); + } + + void GenericCartesian::transform_cartesian_to_motors(float* motors, float* cartesian) { + // Without skew correction motor space is the same cartesian space, so we do no transform. + if ( _mtx ) { + _mtx->transform( motors, cartesian ); + } + else + copyAxes(motors, cartesian); + } + + bool GenericCartesian::canHome(AxisMask axisMask) { + if (ambiguousLimit()) { + log_error("Ambiguous limit switch touching. Manually clear all switches"); + return false; + } + return true; + } + + bool GenericCartesian::limitReached(AxisMask& axisMask, MotorMask& motorMask, MotorMask limited) { + // For Cartesian, the limit switches are associated with individual motors, since + // an axis can have dual motors each with its own limit switch. We clear the motors in + // the mask whose limits have been reached. + clear_bits(motorMask, limited); + + auto oldAxisMask = axisMask; + + // Set axisMask according to the motors that are still running. + axisMask = Machine::Axes::motors_to_axes(motorMask); + + // Return true when an axis drops out of the mask, causing replan + // on any remaining axes. + return axisMask != oldAxisMask; + } + + void GenericCartesian::releaseMotors(AxisMask axisMask, MotorMask motors) { + auto axes = config->_axes; + auto n_axis = axes->_numberAxis; + for (int axis = 0; axis < n_axis; axis++) { + if (bitnum_is_true(axisMask, axis)) { + auto paxis = axes->_axis[axis]; + if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 0))) { + paxis->_motors[0]->unlimit(); + } + if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 1))) { + paxis->_motors[1]->unlimit(); + } + } + } + } + + GenericCartesian::~GenericCartesian() { + if (_mtx) + delete _mtx; + + if (_rev) + delete _rev; + } + + //////////////////// + + template< typename number > + void GenericCartesian::Mtx< number >::dumpRow( const uint idx ) const { + // Prints a row of the matrix into log. + // Useful for debuging. + char line[256]; + char* C = line; + + for( uint i = 0; i < _pitch; ++i ) { + number V = value( idx, i ); + if ( V >= 0.0 ) + sprintf( C, " %4.4f ", V ); + else + sprintf( C, "%4.4f ", V ); + + C = C + strlen( C ); + } + + log_info( line ); + } + + template< typename number > + void GenericCartesian::Mtx< number >::dump() const { + for( uint i = 0; i < _lines; ++i ) { + dumpRow( i ); + } + } + + template< typename number > + void GenericCartesian::Mtx< number >::transform( number* to, const number* from ) const { + for( uint j = 0; j < _pitch; ++j ) { + number A = 0.0; + for( uint i = 0; i < _lines; ++i ) + A += from[ i ] * value( i, j ); + + to[ j ] = A; + } + } + + bool GenericCartesian::GJ_invertMatrix( const uint size, const Mtx* A, Mtx* const B ) { + //log_info( "GJ_invertMatrix" ); + // Gauss Jordan Matrix inversion. + Mtx T( size, size * 2 ); + uint i,j,k; + + T.allocate(); + + for( i = 0; i < size; ++i ) { + for( j = 0; j < size; ++j ) { + *T.ptr( i, j ) = A->value( i, j ); + *T.ptr( i, j + size ) = ( i == j ) ? 1.0 : 0.0; + } + } + + //T.dump(); + for( i = 0; i < size; ++i ) { + if ( T.value( i,i ) == 0 ) { + T.deallocate(); + return false; + } + + for( j = 0; j < size; ++j ) { + if ( i != j ) { + double S = T.value( j, i ) / T.value( i, i ); + for( k = 0; k < size*2; ++k ) + *T.ptr( j, k ) = T.value( j, k ) - S * T.value( i, k ); + + } + } + } + + //log_info( "After elimination" ); + //T.dump(); + for( i = 0; i < size; ++i ) { + for( j = 0; j < size; ++j ) + *B->ptr( i, j ) = static_cast< float >( T.value( i, j + size ) / T.value( i,i ) ); + } + + T.deallocate(); + return true; + } + + template void GenericCartesian::Mtx< float >::transform( float* to, const float* from ) const; + template void GenericCartesian::Mtx< float >::dump() const; + +} diff --git a/FluidNC/src/Kinematics/GenericCartesian.h b/FluidNC/src/Kinematics/GenericCartesian.h new file mode 100644 index 000000000..9c9d32855 --- /dev/null +++ b/FluidNC/src/Kinematics/GenericCartesian.h @@ -0,0 +1,91 @@ +// Copyright (c) 2020 - Bart Dring +// Copyright (c) 2023 - Vlad A. +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#pragma once + +/* + GenericCartesian.h + + This is a kinematic system for where the motors operate in the cartesian space. + + An abstract class which should be a base for diferent transformation cases. + + Unlike Cartisian kinamatic system, this system allow motor space to vary from machine space. + Transformation from system space to motor space is defined as matrix. + All axis must by linearly independent. Matrix has to be inversable. + +*/ + +#include "Kinematics.h" + +#define LOG_MATRIX_CONTENT + +namespace Kinematics { + + class GenericCartesian : public KinematicSystem { + public: + GenericCartesian() = default; + + GenericCartesian(const GenericCartesian&) = delete; + GenericCartesian(GenericCartesian&&) = delete; + GenericCartesian& operator=(const GenericCartesian&) = delete; + GenericCartesian& operator=(GenericCartesian&&) = delete; + + // Kinematic Interface + virtual void init() override; + virtual void init_position() override; + + virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override; + void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override; + void transform_cartesian_to_motors(float* cartesian, float* motors) override; + + bool canHome(AxisMask axisMask) override; + void releaseMotors(AxisMask axisMask, MotorMask motors) override; + bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) override; + + // Configuration handlers: +// void afterParse() override {} +// void group(Configuration::HandlerBase& handler) override; +// void validate() override {} + + // Name of the configurable. Must match the name registered in the cpp file. +// const char* name() const override { return "Generic Cartesian"; } + + protected: + template + class Mtx { + uint _pitch; + uint _lines; + number* _buffer; + + public: + Mtx(const uint row, const uint col) : _pitch(col), _lines(row) { _buffer = new number[_pitch * _lines]; }; + + void allocate() { } + void deallocate() { + } + + number* getBuffer() { return _buffer; } + number value(const uint row, const uint col) const { return _buffer[row * _pitch + col]; } + number* ptr(const uint row, const uint col) { return _buffer + row * _pitch + col; } + void transform(number* to, const number* from) const; + + void dumpRow(const uint idx) const; + void dump() const; + + ~Mtx() { + if ( _buffer ) + delete[] _buffer; + } + }; + + float _buffer[6]; + Mtx* _mtx = nullptr; + Mtx* _rev = nullptr; + + bool GJ_invertMatrix( const uint size, const Mtx* const A, Mtx* const B ); + + ~GenericCartesian(); + }; +} // namespace Kinematics diff --git a/FluidNC/src/Kinematics/Skewed.cpp b/FluidNC/src/Kinematics/Skewed.cpp new file mode 100644 index 000000000..7eb06553b --- /dev/null +++ b/FluidNC/src/Kinematics/Skewed.cpp @@ -0,0 +1,132 @@ +#include "Skewed.h" + +#include "src/Machine/MachineConfig.h" +#include "src/Machine/Axes.h" // ambiguousLimit() +#include "src/Limits.h" + +namespace Kinematics { + + void SkewAxis::group(Configuration::HandlerBase& handler) { + handler.item("dist", _dist, 1.0f, 100000.0f ); + handler.item("x", _x[0], -1000.0f, 1000.0f ); + handler.item("y", _x[1], -1000.0f, 1000.0f ); + handler.item("z", _x[2], -1000.0f, 1000.0f ); + handler.item("a", _x[3], -1000.0f, 1000.0f ); + handler.item("b", _x[4], -1000.0f, 1000.0f ); + handler.item("c", _x[5], -1000.0f, 1000.0f ); + } + + void SkewAxis::init() { + log_info( " Skew ( " << _x[0] << ", " << _x[1] << ", " << _x[2] << " ) over " << _dist << "mm" ); + } + + + // This return a row of transformation matrix + void SkewAxis::getRow( const uint count, float* buf ) { + for( uint i = 0; i < count; ++i ) + buf[ i ] = _x[ i ] / _dist + (( i == _axisIdx ) ? 1.0f : 0.0 ); + } + + //////////////////// + + Skewed::Skewed() : _numberAxis( MAX_N_AXIS ), _axis() { + for (int i = 0; i < MAX_N_AXIS; ++i) { + _axis[i] = nullptr; + } + + } + + void Skewed::init() { + GenericCartesian::init(); + + bool fail = false; + + if (_mtx) + delete _mtx; + + if (_rev) + delete _rev; + + _mtx = new Mtx(_numberAxis, _numberAxis); + _rev = new Mtx(_numberAxis, _numberAxis); + + for (size_t axis = 0; axis < _numberAxis; axis++) { + auto a = _axis[axis]; + if (a) { + a->init(); + a->getRow( _numberAxis, _mtx->ptr( axis, 0 ) ); + } else { + fail = true; + break; + } + } + + if (!fail) + fail = ! GJ_invertMatrix(_numberAxis, _mtx, _rev); + + if (!fail) { +#ifdef LOG_MATRIX_CONTENT + log_info("Direct transform"); + _mtx->dump(); + log_info("Reverse transform"); + _rev->dump(); +#endif + } else { + log_warn("Fail during building transformation matrices. Probably skew settings are too wild. Skew correction will be " + "disabled."); + + if (_mtx) { + delete _mtx; + _mtx = nullptr; + } + + if (_rev) { + delete _rev; + _rev = nullptr; + } + } + } + + void Skewed::afterParse() { + // Find the last axis that was declared and set _numberAxis accordingly + for (size_t i = MAX_N_AXIS; i > 0; --i) { + if (_axis[i - 1] != nullptr) { + _numberAxis = i; + break; + } + } + // Senders might assume 3 axes in reports + if (_numberAxis < 3) { + _numberAxis = 3; + } + + for (size_t i = 0; i < _numberAxis; ++i) { + if (_axis[i] == nullptr) { + _axis[i] = new SkewAxis(i); + } + } + } + + void Skewed::group(Configuration::HandlerBase& handler) { + char tmp[2]; + size_t n_axis = _numberAxis ? _numberAxis : MAX_N_AXIS; + + for (size_t i = 0; i < n_axis; ++i) { + tmp[0] = _names[i]; + tmp[1] = 0; + + handler.section(tmp, _axis[i], i); + } + } + + void Skewed::validate() { + log_info("validation for Skewed"); + init(); + log_info("validation is done") + } + + // Configuration registration + namespace { + KinematicsFactory::InstanceBuilder registration("Skewed"); + } +} diff --git a/FluidNC/src/Kinematics/Skewed.h b/FluidNC/src/Kinematics/Skewed.h new file mode 100644 index 000000000..e6c257a72 --- /dev/null +++ b/FluidNC/src/Kinematics/Skewed.h @@ -0,0 +1,62 @@ +// Copyright (c) 2020 - Bart Dring +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#pragma once + +/* + Cartesian.h + + This is a kinematic system for where the motors operate in the cartesian space. +*/ + +#include "GenericCartesian.h" + +namespace Kinematics { + class SkewAxis : public Configuration::Configurable { + uint _axisIdx; + public: + SkewAxis( int currentAxis ) : _axisIdx( currentAxis ) { _x[5] = _x[4] = _x[3] = _x[2] = _x[1] = _x[0] = 0.0f; }; + + float _dist = 10.0f; + float _x[ 6 ]; + + // Configuration system helpers: + void validate() override {}; + void afterParse() override {}; + void group(Configuration::HandlerBase& handler) override; + void init(); + + void getRow( const uint count, float* buf ); + }; + + + class Skewed : public GenericCartesian { + static constexpr const char* _names = "xyzabc"; + uint _numberAxis; + + public: + Skewed(); + + Skewed(const Skewed&) = delete; + Skewed(Skewed&&) = delete; + Skewed& operator=(const Skewed&) = delete; + Skewed& operator=(Skewed&&) = delete; + + // Kinematic Interface + virtual void init() override; + + // Configuration handlers: + void afterParse() override; + void group(Configuration::HandlerBase& handler) override; + void validate() override; + + // Name of the configurable. Must match the name registered in the cpp file. + const char* name() const override { return "Skew corrected Cartesian"; } + + protected: + SkewAxis* _axis[6]; +// float _buffer[6]; + + ~Skewed() {} + }; +} // namespace Kinematics diff --git a/FluidNC/src/Machine/MachineConfig.h b/FluidNC/src/Machine/MachineConfig.h index 16c0673e7..213e0cd98 100644 --- a/FluidNC/src/Machine/MachineConfig.h +++ b/FluidNC/src/Machine/MachineConfig.h @@ -117,7 +117,7 @@ namespace Machine { extern Machine::MachineConfig* config; template -void copyAxes(T* dest, T* src) { +void copyAxes(T* dest, const T* src) { auto n_axis = config->_axes->_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { dest[axis] = src[axis]; diff --git a/FluidNC/test/Configuration/YamlTreeBuilder.cpp b/FluidNC/test/Configuration/YamlTreeBuilder.cpp index 189d70b85..de9a70d4f 100644 --- a/FluidNC/test/Configuration/YamlTreeBuilder.cpp +++ b/FluidNC/test/Configuration/YamlTreeBuilder.cpp @@ -14,7 +14,7 @@ namespace Configuration { String b; String c; - void validate() const {} + void validate() {} void group(HandlerBase& handler) { handler.item("a", a); handler.item("b", b); @@ -27,7 +27,7 @@ namespace Configuration { String aap; int banaan; - void validate() const {} + void validate() {} void group(HandlerBase& handler) { handler.item("aap", aap); handler.item("banaan", banaan); @@ -51,7 +51,7 @@ namespace Configuration { int value; int banaan; - void validate() const {} + void validate() {} void group(HandlerBase& handler) override { handler.item("aap", aap); handler.item("type", value, stepTypes); @@ -65,7 +65,7 @@ namespace Configuration { TestBasic2* n2 = nullptr; int foo = 0; - void validate() const {} + void validate() {} void group(HandlerBase& handler) override { handler.section("n1", n1); handler.section("n2", n2); From ca48d49569d942f8779b5e65064400b58a65b2e4 Mon Sep 17 00:00:00 2001 From: Vlad A Date: Wed, 15 Mar 2023 20:18:59 -0400 Subject: [PATCH 2/3] Add new Kinematic system with skew correction New system adds ability to tweak skew correction, for cases when physical geometry of CNC machine is not ideal. Signed-off-by: Vlad A --- FluidNC/src/Kinematics/Cartesian.h | 1 + FluidNC/src/Kinematics/GenericCartesian.cpp | 187 ++++++++++++++++++ FluidNC/src/Kinematics/GenericCartesian.h | 91 +++++++++ FluidNC/src/Kinematics/Skewed.cpp | 132 +++++++++++++ FluidNC/src/Kinematics/Skewed.h | 62 ++++++ FluidNC/src/Machine/MachineConfig.h | 2 +- .../test/Configuration/YamlTreeBuilder.cpp | 8 +- 7 files changed, 478 insertions(+), 5 deletions(-) create mode 100644 FluidNC/src/Kinematics/GenericCartesian.cpp create mode 100644 FluidNC/src/Kinematics/GenericCartesian.h create mode 100644 FluidNC/src/Kinematics/Skewed.cpp create mode 100644 FluidNC/src/Kinematics/Skewed.h diff --git a/FluidNC/src/Kinematics/Cartesian.h b/FluidNC/src/Kinematics/Cartesian.h index 50476241a..aeb5d11b5 100644 --- a/FluidNC/src/Kinematics/Cartesian.h +++ b/FluidNC/src/Kinematics/Cartesian.h @@ -7,6 +7,7 @@ Cartesian.h This is a kinematic system for where the motors operate in the cartesian space. + All logical axis of the system perfectly alligned with physical axis of machine. */ #include "Kinematics.h" diff --git a/FluidNC/src/Kinematics/GenericCartesian.cpp b/FluidNC/src/Kinematics/GenericCartesian.cpp new file mode 100644 index 000000000..e2cc9bf6f --- /dev/null +++ b/FluidNC/src/Kinematics/GenericCartesian.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2020 - Bart Dring +// Copyright (c) 2023 - Vlad A. +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#include "GenericCartesian.h" + +#include "src/Machine/MachineConfig.h" +#include "src/Machine/Axes.h" // ambiguousLimit() +//#include "Skew.h" // Skew, SkewAxis +#include "src/Limits.h" + +namespace Kinematics { + void GenericCartesian::init() { + log_info("Kinematic system: " << name()); + init_position(); + } + + // Initialize the machine position + void GenericCartesian::init_position() { + auto n_axis = config->_axes->_numberAxis; + for (size_t axis = 0; axis < n_axis; axis++) { + set_motor_steps(axis, 0); // Set to zeros + } + } + + bool GenericCartesian::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { + if ( _mtx ) { + _mtx->transform( _buffer, target ); + return mc_move_motors( _buffer, pl_data); + } else + // Without skew correction motor space is the same cartesian space, so we do no transform. + return mc_move_motors(target, pl_data); + } + + void GenericCartesian::motors_to_cartesian(float* cartesian, float* motors, int n_axis) { + if ( _rev ) { + _rev->transform( cartesian, motors ); + } + else + // Without skew correction motor space is the same cartesian space, so we do no transform. + copyAxes(cartesian, motors); + } + + void GenericCartesian::transform_cartesian_to_motors(float* motors, float* cartesian) { + // Without skew correction motor space is the same cartesian space, so we do no transform. + if ( _mtx ) { + _mtx->transform( motors, cartesian ); + } + else + copyAxes(motors, cartesian); + } + + bool GenericCartesian::canHome(AxisMask axisMask) { + if (ambiguousLimit()) { + log_error("Ambiguous limit switch touching. Manually clear all switches"); + return false; + } + return true; + } + + bool GenericCartesian::limitReached(AxisMask& axisMask, MotorMask& motorMask, MotorMask limited) { + // For Cartesian, the limit switches are associated with individual motors, since + // an axis can have dual motors each with its own limit switch. We clear the motors in + // the mask whose limits have been reached. + clear_bits(motorMask, limited); + + auto oldAxisMask = axisMask; + + // Set axisMask according to the motors that are still running. + axisMask = Machine::Axes::motors_to_axes(motorMask); + + // Return true when an axis drops out of the mask, causing replan + // on any remaining axes. + return axisMask != oldAxisMask; + } + + void GenericCartesian::releaseMotors(AxisMask axisMask, MotorMask motors) { + auto axes = config->_axes; + auto n_axis = axes->_numberAxis; + for (int axis = 0; axis < n_axis; axis++) { + if (bitnum_is_true(axisMask, axis)) { + auto paxis = axes->_axis[axis]; + if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 0))) { + paxis->_motors[0]->unlimit(); + } + if (bitnum_is_true(motors, Machine::Axes::motor_bit(axis, 1))) { + paxis->_motors[1]->unlimit(); + } + } + } + } + + GenericCartesian::~GenericCartesian() { + if (_mtx) + delete _mtx; + + if (_rev) + delete _rev; + } + + //////////////////// + + template< typename number > + void GenericCartesian::Mtx< number >::dumpRow( const uint idx ) const { + // Prints a row of the matrix into log. + // Useful for debuging. + char line[256]; + char* C = line; + + for( uint i = 0; i < _pitch; ++i ) { + number V = value( idx, i ); + if ( V >= 0.0 ) + sprintf( C, " %4.4f ", V ); + else + sprintf( C, "%4.4f ", V ); + + C = C + strlen( C ); + } + + log_info( line ); + } + + template< typename number > + void GenericCartesian::Mtx< number >::dump() const { + for( uint i = 0; i < _lines; ++i ) { + dumpRow( i ); + } + } + + template< typename number > + void GenericCartesian::Mtx< number >::transform( number* to, const number* from ) const { + for( uint j = 0; j < _pitch; ++j ) { + number A = 0.0; + for( uint i = 0; i < _lines; ++i ) + A += from[ i ] * value( i, j ); + + to[ j ] = A; + } + } + + bool GenericCartesian::GJ_invertMatrix( const uint size, const Mtx* A, Mtx* const B ) { + //log_info( "GJ_invertMatrix" ); + // Gauss Jordan Matrix inversion. + Mtx T( size, size * 2 ); + uint i,j,k; + + T.allocate(); + + for( i = 0; i < size; ++i ) { + for( j = 0; j < size; ++j ) { + *T.ptr( i, j ) = A->value( i, j ); + *T.ptr( i, j + size ) = ( i == j ) ? 1.0 : 0.0; + } + } + + //T.dump(); + for( i = 0; i < size; ++i ) { + if ( T.value( i,i ) == 0 ) { + T.deallocate(); + return false; + } + + for( j = 0; j < size; ++j ) { + if ( i != j ) { + double S = T.value( j, i ) / T.value( i, i ); + for( k = 0; k < size*2; ++k ) + *T.ptr( j, k ) = T.value( j, k ) - S * T.value( i, k ); + + } + } + } + + //log_info( "After elimination" ); + //T.dump(); + for( i = 0; i < size; ++i ) { + for( j = 0; j < size; ++j ) + *B->ptr( i, j ) = static_cast< float >( T.value( i, j + size ) / T.value( i,i ) ); + } + + T.deallocate(); + return true; + } + + template void GenericCartesian::Mtx< float >::transform( float* to, const float* from ) const; + template void GenericCartesian::Mtx< float >::dump() const; + +} diff --git a/FluidNC/src/Kinematics/GenericCartesian.h b/FluidNC/src/Kinematics/GenericCartesian.h new file mode 100644 index 000000000..9c9d32855 --- /dev/null +++ b/FluidNC/src/Kinematics/GenericCartesian.h @@ -0,0 +1,91 @@ +// Copyright (c) 2020 - Bart Dring +// Copyright (c) 2023 - Vlad A. +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#pragma once + +/* + GenericCartesian.h + + This is a kinematic system for where the motors operate in the cartesian space. + + An abstract class which should be a base for diferent transformation cases. + + Unlike Cartisian kinamatic system, this system allow motor space to vary from machine space. + Transformation from system space to motor space is defined as matrix. + All axis must by linearly independent. Matrix has to be inversable. + +*/ + +#include "Kinematics.h" + +#define LOG_MATRIX_CONTENT + +namespace Kinematics { + + class GenericCartesian : public KinematicSystem { + public: + GenericCartesian() = default; + + GenericCartesian(const GenericCartesian&) = delete; + GenericCartesian(GenericCartesian&&) = delete; + GenericCartesian& operator=(const GenericCartesian&) = delete; + GenericCartesian& operator=(GenericCartesian&&) = delete; + + // Kinematic Interface + virtual void init() override; + virtual void init_position() override; + + virtual bool cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) override; + void motors_to_cartesian(float* cartesian, float* motors, int n_axis) override; + void transform_cartesian_to_motors(float* cartesian, float* motors) override; + + bool canHome(AxisMask axisMask) override; + void releaseMotors(AxisMask axisMask, MotorMask motors) override; + bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) override; + + // Configuration handlers: +// void afterParse() override {} +// void group(Configuration::HandlerBase& handler) override; +// void validate() override {} + + // Name of the configurable. Must match the name registered in the cpp file. +// const char* name() const override { return "Generic Cartesian"; } + + protected: + template + class Mtx { + uint _pitch; + uint _lines; + number* _buffer; + + public: + Mtx(const uint row, const uint col) : _pitch(col), _lines(row) { _buffer = new number[_pitch * _lines]; }; + + void allocate() { } + void deallocate() { + } + + number* getBuffer() { return _buffer; } + number value(const uint row, const uint col) const { return _buffer[row * _pitch + col]; } + number* ptr(const uint row, const uint col) { return _buffer + row * _pitch + col; } + void transform(number* to, const number* from) const; + + void dumpRow(const uint idx) const; + void dump() const; + + ~Mtx() { + if ( _buffer ) + delete[] _buffer; + } + }; + + float _buffer[6]; + Mtx* _mtx = nullptr; + Mtx* _rev = nullptr; + + bool GJ_invertMatrix( const uint size, const Mtx* const A, Mtx* const B ); + + ~GenericCartesian(); + }; +} // namespace Kinematics diff --git a/FluidNC/src/Kinematics/Skewed.cpp b/FluidNC/src/Kinematics/Skewed.cpp new file mode 100644 index 000000000..7eb06553b --- /dev/null +++ b/FluidNC/src/Kinematics/Skewed.cpp @@ -0,0 +1,132 @@ +#include "Skewed.h" + +#include "src/Machine/MachineConfig.h" +#include "src/Machine/Axes.h" // ambiguousLimit() +#include "src/Limits.h" + +namespace Kinematics { + + void SkewAxis::group(Configuration::HandlerBase& handler) { + handler.item("dist", _dist, 1.0f, 100000.0f ); + handler.item("x", _x[0], -1000.0f, 1000.0f ); + handler.item("y", _x[1], -1000.0f, 1000.0f ); + handler.item("z", _x[2], -1000.0f, 1000.0f ); + handler.item("a", _x[3], -1000.0f, 1000.0f ); + handler.item("b", _x[4], -1000.0f, 1000.0f ); + handler.item("c", _x[5], -1000.0f, 1000.0f ); + } + + void SkewAxis::init() { + log_info( " Skew ( " << _x[0] << ", " << _x[1] << ", " << _x[2] << " ) over " << _dist << "mm" ); + } + + + // This return a row of transformation matrix + void SkewAxis::getRow( const uint count, float* buf ) { + for( uint i = 0; i < count; ++i ) + buf[ i ] = _x[ i ] / _dist + (( i == _axisIdx ) ? 1.0f : 0.0 ); + } + + //////////////////// + + Skewed::Skewed() : _numberAxis( MAX_N_AXIS ), _axis() { + for (int i = 0; i < MAX_N_AXIS; ++i) { + _axis[i] = nullptr; + } + + } + + void Skewed::init() { + GenericCartesian::init(); + + bool fail = false; + + if (_mtx) + delete _mtx; + + if (_rev) + delete _rev; + + _mtx = new Mtx(_numberAxis, _numberAxis); + _rev = new Mtx(_numberAxis, _numberAxis); + + for (size_t axis = 0; axis < _numberAxis; axis++) { + auto a = _axis[axis]; + if (a) { + a->init(); + a->getRow( _numberAxis, _mtx->ptr( axis, 0 ) ); + } else { + fail = true; + break; + } + } + + if (!fail) + fail = ! GJ_invertMatrix(_numberAxis, _mtx, _rev); + + if (!fail) { +#ifdef LOG_MATRIX_CONTENT + log_info("Direct transform"); + _mtx->dump(); + log_info("Reverse transform"); + _rev->dump(); +#endif + } else { + log_warn("Fail during building transformation matrices. Probably skew settings are too wild. Skew correction will be " + "disabled."); + + if (_mtx) { + delete _mtx; + _mtx = nullptr; + } + + if (_rev) { + delete _rev; + _rev = nullptr; + } + } + } + + void Skewed::afterParse() { + // Find the last axis that was declared and set _numberAxis accordingly + for (size_t i = MAX_N_AXIS; i > 0; --i) { + if (_axis[i - 1] != nullptr) { + _numberAxis = i; + break; + } + } + // Senders might assume 3 axes in reports + if (_numberAxis < 3) { + _numberAxis = 3; + } + + for (size_t i = 0; i < _numberAxis; ++i) { + if (_axis[i] == nullptr) { + _axis[i] = new SkewAxis(i); + } + } + } + + void Skewed::group(Configuration::HandlerBase& handler) { + char tmp[2]; + size_t n_axis = _numberAxis ? _numberAxis : MAX_N_AXIS; + + for (size_t i = 0; i < n_axis; ++i) { + tmp[0] = _names[i]; + tmp[1] = 0; + + handler.section(tmp, _axis[i], i); + } + } + + void Skewed::validate() { + log_info("validation for Skewed"); + init(); + log_info("validation is done") + } + + // Configuration registration + namespace { + KinematicsFactory::InstanceBuilder registration("Skewed"); + } +} diff --git a/FluidNC/src/Kinematics/Skewed.h b/FluidNC/src/Kinematics/Skewed.h new file mode 100644 index 000000000..e6c257a72 --- /dev/null +++ b/FluidNC/src/Kinematics/Skewed.h @@ -0,0 +1,62 @@ +// Copyright (c) 2020 - Bart Dring +// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file. + +#pragma once + +/* + Cartesian.h + + This is a kinematic system for where the motors operate in the cartesian space. +*/ + +#include "GenericCartesian.h" + +namespace Kinematics { + class SkewAxis : public Configuration::Configurable { + uint _axisIdx; + public: + SkewAxis( int currentAxis ) : _axisIdx( currentAxis ) { _x[5] = _x[4] = _x[3] = _x[2] = _x[1] = _x[0] = 0.0f; }; + + float _dist = 10.0f; + float _x[ 6 ]; + + // Configuration system helpers: + void validate() override {}; + void afterParse() override {}; + void group(Configuration::HandlerBase& handler) override; + void init(); + + void getRow( const uint count, float* buf ); + }; + + + class Skewed : public GenericCartesian { + static constexpr const char* _names = "xyzabc"; + uint _numberAxis; + + public: + Skewed(); + + Skewed(const Skewed&) = delete; + Skewed(Skewed&&) = delete; + Skewed& operator=(const Skewed&) = delete; + Skewed& operator=(Skewed&&) = delete; + + // Kinematic Interface + virtual void init() override; + + // Configuration handlers: + void afterParse() override; + void group(Configuration::HandlerBase& handler) override; + void validate() override; + + // Name of the configurable. Must match the name registered in the cpp file. + const char* name() const override { return "Skew corrected Cartesian"; } + + protected: + SkewAxis* _axis[6]; +// float _buffer[6]; + + ~Skewed() {} + }; +} // namespace Kinematics diff --git a/FluidNC/src/Machine/MachineConfig.h b/FluidNC/src/Machine/MachineConfig.h index 16c0673e7..213e0cd98 100644 --- a/FluidNC/src/Machine/MachineConfig.h +++ b/FluidNC/src/Machine/MachineConfig.h @@ -117,7 +117,7 @@ namespace Machine { extern Machine::MachineConfig* config; template -void copyAxes(T* dest, T* src) { +void copyAxes(T* dest, const T* src) { auto n_axis = config->_axes->_numberAxis; for (size_t axis = 0; axis < n_axis; axis++) { dest[axis] = src[axis]; diff --git a/FluidNC/test/Configuration/YamlTreeBuilder.cpp b/FluidNC/test/Configuration/YamlTreeBuilder.cpp index 189d70b85..de9a70d4f 100644 --- a/FluidNC/test/Configuration/YamlTreeBuilder.cpp +++ b/FluidNC/test/Configuration/YamlTreeBuilder.cpp @@ -14,7 +14,7 @@ namespace Configuration { String b; String c; - void validate() const {} + void validate() {} void group(HandlerBase& handler) { handler.item("a", a); handler.item("b", b); @@ -27,7 +27,7 @@ namespace Configuration { String aap; int banaan; - void validate() const {} + void validate() {} void group(HandlerBase& handler) { handler.item("aap", aap); handler.item("banaan", banaan); @@ -51,7 +51,7 @@ namespace Configuration { int value; int banaan; - void validate() const {} + void validate() {} void group(HandlerBase& handler) override { handler.item("aap", aap); handler.item("type", value, stepTypes); @@ -65,7 +65,7 @@ namespace Configuration { TestBasic2* n2 = nullptr; int foo = 0; - void validate() const {} + void validate() {} void group(HandlerBase& handler) override { handler.section("n1", n1); handler.section("n2", n2); From fee1712b8d4b329b6e4705d5bf3c7676a5d2f95a Mon Sep 17 00:00:00 2001 From: Vlad A Date: Fri, 31 Mar 2023 23:46:43 -0400 Subject: [PATCH 3/3] Various fixes and cosmetic changes from PR review https://github.com/bdring/FluidNC/pull/849 Signed-off-by: Vlad A --- FluidNC/src/Kinematics/GenericCartesian.cpp | 133 ++++++++++---------- FluidNC/src/Kinematics/GenericCartesian.h | 40 +++--- FluidNC/src/Kinematics/Skewed.cpp | 75 ++++++----- FluidNC/src/Kinematics/Skewed.h | 19 ++- 4 files changed, 127 insertions(+), 140 deletions(-) diff --git a/FluidNC/src/Kinematics/GenericCartesian.cpp b/FluidNC/src/Kinematics/GenericCartesian.cpp index e2cc9bf6f..fc71f2dc6 100644 --- a/FluidNC/src/Kinematics/GenericCartesian.cpp +++ b/FluidNC/src/Kinematics/GenericCartesian.cpp @@ -9,8 +9,12 @@ //#include "Skew.h" // Skew, SkewAxis #include "src/Limits.h" +#include +#include +#include + namespace Kinematics { - void GenericCartesian::init() { + void GenericCartesian::init() { log_info("Kinematic system: " << name()); init_position(); } @@ -24,30 +28,31 @@ namespace Kinematics { } bool GenericCartesian::cartesian_to_motors(float* target, plan_line_data_t* pl_data, float* position) { - if ( _mtx ) { - _mtx->transform( _buffer, target ); - return mc_move_motors( _buffer, pl_data); - } else - // Without skew correction motor space is the same cartesian space, so we do no transform. - return mc_move_motors(target, pl_data); + if (_mtx) { + _mtx->transform(_buffer, target); + return mc_move_motors(_buffer, pl_data); + } + + // Without skew correction motor space is the same cartesian space, so we do no transform. + return mc_move_motors(target, pl_data); } void GenericCartesian::motors_to_cartesian(float* cartesian, float* motors, int n_axis) { - if ( _rev ) { - _rev->transform( cartesian, motors ); - } - else + if (_rev) { + _rev->transform(cartesian, motors); + } else { // Without skew correction motor space is the same cartesian space, so we do no transform. copyAxes(cartesian, motors); + } } void GenericCartesian::transform_cartesian_to_motors(float* motors, float* cartesian) { // Without skew correction motor space is the same cartesian space, so we do no transform. - if ( _mtx ) { - _mtx->transform( motors, cartesian ); - } - else + if (_mtx) { + _mtx->transform(motors, cartesian); + } else { copyAxes(motors, cartesian); + } } bool GenericCartesian::canHome(AxisMask axisMask) { @@ -91,97 +96,93 @@ namespace Kinematics { } GenericCartesian::~GenericCartesian() { - if (_mtx) + if (_mtx) { delete _mtx; + } - if (_rev) + if (_rev) { delete _rev; + } } //////////////////// - template< typename number > - void GenericCartesian::Mtx< number >::dumpRow( const uint idx ) const { + template + void GenericCartesian::Mtx::dumpRow(const size_t rowIdx) const { // Prints a row of the matrix into log. // Useful for debuging. - char line[256]; - char* C = line; - - for( uint i = 0; i < _pitch; ++i ) { - number V = value( idx, i ); - if ( V >= 0.0 ) - sprintf( C, " %4.4f ", V ); - else - sprintf( C, "%4.4f ", V ); - - C = C + strlen( C ); + std::ostringstream msg; + for (size_t col = 0; col < _pitch; ++col) { + number V = value( rowIdx, col ); + msg << std::fixed << std::setw(9) << std::setprecision(5) << V; } - log_info( line ); + log_debug( msg.str() ); } - template< typename number > - void GenericCartesian::Mtx< number >::dump() const { - for( uint i = 0; i < _lines; ++i ) { - dumpRow( i ); + template + void GenericCartesian::Mtx::dump() const { + for (size_t i = 0; i < _lines; ++i) { + dumpRow(i); } } - template< typename number > - void GenericCartesian::Mtx< number >::transform( number* to, const number* from ) const { - for( uint j = 0; j < _pitch; ++j ) { + template + void GenericCartesian::Mtx::transform(number* to, const number* from) const { + for (size_t j = 0; j < _pitch; ++j) { number A = 0.0; - for( uint i = 0; i < _lines; ++i ) - A += from[ i ] * value( i, j ); + for (size_t i = 0; i < _lines; ++i) { + A += from[i] * value(i, j); + } - to[ j ] = A; + to[j] = A; } } - bool GenericCartesian::GJ_invertMatrix( const uint size, const Mtx* A, Mtx* const B ) { - //log_info( "GJ_invertMatrix" ); - // Gauss Jordan Matrix inversion. - Mtx T( size, size * 2 ); - uint i,j,k; + bool GenericCartesian::GJ_invertMatrix(const size_t size, const Mtx* A, Mtx* const B) { + //log_debug( "GJ_invertMatrix" ); + // Gauss Jordan Matrix inversion. + Mtx T(size, size * 2); + size_t i, j, k; T.allocate(); - - for( i = 0; i < size; ++i ) { - for( j = 0; j < size; ++j ) { - *T.ptr( i, j ) = A->value( i, j ); - *T.ptr( i, j + size ) = ( i == j ) ? 1.0 : 0.0; + + for (i = 0; i < size; ++i) { + for (j = 0; j < size; ++j) { + *T.ptr(i, j) = A->value(i, j); + *T.ptr(i, j + size) = (i == j) ? 1.0 : 0.0; } } //T.dump(); - for( i = 0; i < size; ++i ) { - if ( T.value( i,i ) == 0 ) { + for (i = 0; i < size; ++i) { + if (T.value(i, i) == 0) { T.deallocate(); return false; } - for( j = 0; j < size; ++j ) { - if ( i != j ) { - double S = T.value( j, i ) / T.value( i, i ); - for( k = 0; k < size*2; ++k ) - *T.ptr( j, k ) = T.value( j, k ) - S * T.value( i, k ); - + for (j = 0; j < size; ++j) { + if (i != j) { + double S = T.value(j, i) / T.value(i, i); + for (k = 0; k < size * 2; ++k) { + *T.ptr(j, k) = T.value(j, k) - S * T.value(i, k); + } } } } - //log_info( "After elimination" ); - //T.dump(); - for( i = 0; i < size; ++i ) { - for( j = 0; j < size; ++j ) - *B->ptr( i, j ) = static_cast< float >( T.value( i, j + size ) / T.value( i,i ) ); + //log_debug( "After elimination" ); + //T.dump(); + for (i = 0; i < size; ++i) { + for (j = 0; j < size; ++j) + *B->ptr(i, j) = static_cast(T.value(i, j + size) / T.value(i, i)); } T.deallocate(); return true; } - template void GenericCartesian::Mtx< float >::transform( float* to, const float* from ) const; - template void GenericCartesian::Mtx< float >::dump() const; + template void GenericCartesian::Mtx::transform(float* to, const float* from) const; + template void GenericCartesian::Mtx::dump() const; } diff --git a/FluidNC/src/Kinematics/GenericCartesian.h b/FluidNC/src/Kinematics/GenericCartesian.h index 9c9d32855..3e0e67fcf 100644 --- a/FluidNC/src/Kinematics/GenericCartesian.h +++ b/FluidNC/src/Kinematics/GenericCartesian.h @@ -11,16 +11,14 @@ An abstract class which should be a base for diferent transformation cases. - Unlike Cartisian kinamatic system, this system allow motor space to vary from machine space. - Transformation from system space to motor space is defined as matrix. - All axis must by linearly independent. Matrix has to be inversable. + Unlike Cartisian kinematic system which uses the identity transform between the + axis and motor spaces, this system uses an arbitrary linear transform + defined by an invertible matrix. */ #include "Kinematics.h" -#define LOG_MATRIX_CONTENT - namespace Kinematics { class GenericCartesian : public KinematicSystem { @@ -44,47 +42,39 @@ namespace Kinematics { void releaseMotors(AxisMask axisMask, MotorMask motors) override; bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited) override; - // Configuration handlers: -// void afterParse() override {} -// void group(Configuration::HandlerBase& handler) override; -// void validate() override {} - - // Name of the configurable. Must match the name registered in the cpp file. -// const char* name() const override { return "Generic Cartesian"; } - protected: template class Mtx { - uint _pitch; - uint _lines; + size_t _pitch; // Here, it's equal the number of columns in a matrix row + size_t _lines; // The number of rows number* _buffer; public: - Mtx(const uint row, const uint col) : _pitch(col), _lines(row) { _buffer = new number[_pitch * _lines]; }; + Mtx(const size_t row, const size_t col) : _pitch(col), _lines(row) { _buffer = new number[_pitch * _lines]; }; - void allocate() { } - void deallocate() { - } + void allocate() {} + void deallocate() {} number* getBuffer() { return _buffer; } - number value(const uint row, const uint col) const { return _buffer[row * _pitch + col]; } - number* ptr(const uint row, const uint col) { return _buffer + row * _pitch + col; } + number value(const size_t row, const size_t col) const { return _buffer[row * _pitch + col]; } + number* ptr(const size_t row, const size_t col) { return _buffer + row * _pitch + col; } void transform(number* to, const number* from) const; - void dumpRow(const uint idx) const; + void dumpRow(const size_t idx) const; void dump() const; ~Mtx() { - if ( _buffer ) + if (_buffer) { delete[] _buffer; + } } }; - float _buffer[6]; + float _buffer[6]; Mtx* _mtx = nullptr; Mtx* _rev = nullptr; - bool GJ_invertMatrix( const uint size, const Mtx* const A, Mtx* const B ); + bool GJ_invertMatrix(const size_t size, const Mtx* const A, Mtx* const B); ~GenericCartesian(); }; diff --git a/FluidNC/src/Kinematics/Skewed.cpp b/FluidNC/src/Kinematics/Skewed.cpp index 7eb06553b..f55a51d00 100644 --- a/FluidNC/src/Kinematics/Skewed.cpp +++ b/FluidNC/src/Kinematics/Skewed.cpp @@ -7,31 +7,25 @@ namespace Kinematics { void SkewAxis::group(Configuration::HandlerBase& handler) { - handler.item("dist", _dist, 1.0f, 100000.0f ); - handler.item("x", _x[0], -1000.0f, 1000.0f ); - handler.item("y", _x[1], -1000.0f, 1000.0f ); - handler.item("z", _x[2], -1000.0f, 1000.0f ); - handler.item("a", _x[3], -1000.0f, 1000.0f ); - handler.item("b", _x[4], -1000.0f, 1000.0f ); - handler.item("c", _x[5], -1000.0f, 1000.0f ); + handler.item("distance_mm", _dist, 1.0f, 100000.0f ); + handler.item("offset_x_mm", _offsets[0], -1000.0f, 1000.0f ); + handler.item("offset_y_mm", _offsets[1], -1000.0f, 1000.0f ); + handler.item("offset_z_mm", _offsets[2], -1000.0f, 1000.0f ); + handler.item("offset_a_mm", _offsets[3], -1000.0f, 1000.0f ); + handler.item("offset_b_mm", _offsets[4], -1000.0f, 1000.0f ); + handler.item("offset_c_mm", _offsets[5], -1000.0f, 1000.0f ); } void SkewAxis::init() { - log_info( " Skew ( " << _x[0] << ", " << _x[1] << ", " << _x[2] << " ) over " << _dist << "mm" ); + log_debug( " Skew ( " << _offsets[0] << ", " << _offsets[1] << ", " << _offsets[2] << " ) over " << _dist << "mm" ); } - // This return a row of transformation matrix - void SkewAxis::getRow( const uint count, float* buf ) { - for( uint i = 0; i < count; ++i ) - buf[ i ] = _x[ i ] / _dist + (( i == _axisIdx ) ? 1.0f : 0.0 ); - } - //////////////////// - Skewed::Skewed() : _numberAxis( MAX_N_AXIS ), _axis() { + Skewed::Skewed() : _numberSkewAxis( MAX_N_AXIS ) { for (int i = 0; i < MAX_N_AXIS; ++i) { - _axis[i] = nullptr; + _skewAxis[i] = nullptr; } } @@ -47,14 +41,21 @@ namespace Kinematics { if (_rev) delete _rev; - _mtx = new Mtx(_numberAxis, _numberAxis); - _rev = new Mtx(_numberAxis, _numberAxis); + _mtx = new Mtx( _numberSkewAxis, _numberSkewAxis ); + _rev = new Mtx( _numberSkewAxis, _numberSkewAxis ); - for (size_t axis = 0; axis < _numberAxis; axis++) { - auto a = _axis[axis]; + for (size_t axis = 0; axis < _numberSkewAxis; axis++) { + auto a = _skewAxis[axis]; if (a) { a->init(); - a->getRow( _numberAxis, _mtx->ptr( axis, 0 ) ); + + float* row = _mtx->ptr( axis, 0 ); + const SkewAxis* skew = _skewAxis[axis]; + + for( uint i = 0; i < _numberSkewAxis; ++i ) { + row[ i ] = skew->_offsets[ i ] / skew->_dist; + row[ i ] += ( i == axis ) ? 1.0f : 0.0f; + } } else { fail = true; break; @@ -62,15 +63,13 @@ namespace Kinematics { } if (!fail) - fail = ! GJ_invertMatrix(_numberAxis, _mtx, _rev); + fail = ! GJ_invertMatrix( _numberSkewAxis, _mtx, _rev ); if (!fail) { -#ifdef LOG_MATRIX_CONTENT - log_info("Direct transform"); + log_debug("Direct transform"); _mtx->dump(); - log_info("Reverse transform"); + log_debug("Reverse transform"); _rev->dump(); -#endif } else { log_warn("Fail during building transformation matrices. Probably skew settings are too wild. Skew correction will be " "disabled."); @@ -88,41 +87,39 @@ namespace Kinematics { } void Skewed::afterParse() { - // Find the last axis that was declared and set _numberAxis accordingly + // Find the last axis that was declared and set _numberSkewAxis accordingly for (size_t i = MAX_N_AXIS; i > 0; --i) { - if (_axis[i - 1] != nullptr) { - _numberAxis = i; + if (_skewAxis[i - 1] != nullptr) { + _numberSkewAxis = i; break; } } // Senders might assume 3 axes in reports - if (_numberAxis < 3) { - _numberAxis = 3; + if (_numberSkewAxis < 3) { + _numberSkewAxis = 3; } - for (size_t i = 0; i < _numberAxis; ++i) { - if (_axis[i] == nullptr) { - _axis[i] = new SkewAxis(i); + for (size_t i = 0; i < _numberSkewAxis; ++i) { + if (_skewAxis[i] == nullptr) { + _skewAxis[i] = new SkewAxis(); } } } void Skewed::group(Configuration::HandlerBase& handler) { char tmp[2]; - size_t n_axis = _numberAxis ? _numberAxis : MAX_N_AXIS; + size_t n_axis = _numberSkewAxis ? _numberSkewAxis : MAX_N_AXIS; for (size_t i = 0; i < n_axis; ++i) { - tmp[0] = _names[i]; + tmp[0] = config->_axes->axisName( i ); tmp[1] = 0; - handler.section(tmp, _axis[i], i); + handler.section(tmp, _skewAxis[i]); } } void Skewed::validate() { - log_info("validation for Skewed"); init(); - log_info("validation is done") } // Configuration registration diff --git a/FluidNC/src/Kinematics/Skewed.h b/FluidNC/src/Kinematics/Skewed.h index e6c257a72..96113248e 100644 --- a/FluidNC/src/Kinematics/Skewed.h +++ b/FluidNC/src/Kinematics/Skewed.h @@ -4,35 +4,34 @@ #pragma once /* - Cartesian.h + Skewed.h - This is a kinematic system for where the motors operate in the cartesian space. + This is a kinematic system for where the motors operate in the cartesian space. + But unlike Cartesian kinematic system, it adds ability to tweak skew correction, + for cases when physical geometry of CNC machine is not ideal. */ #include "GenericCartesian.h" namespace Kinematics { class SkewAxis : public Configuration::Configurable { - uint _axisIdx; public: - SkewAxis( int currentAxis ) : _axisIdx( currentAxis ) { _x[5] = _x[4] = _x[3] = _x[2] = _x[1] = _x[0] = 0.0f; }; + SkewAxis() { _offsets[5] = _offsets[4] = _offsets[3] = _offsets[2] = _offsets[1] = _offsets[0] = 0.0f; }; float _dist = 10.0f; - float _x[ 6 ]; + float _offsets[ 6 ]; // Configuration system helpers: void validate() override {}; void afterParse() override {}; void group(Configuration::HandlerBase& handler) override; - void init(); - void getRow( const uint count, float* buf ); + void init(); }; class Skewed : public GenericCartesian { - static constexpr const char* _names = "xyzabc"; - uint _numberAxis; + uint _numberSkewAxis; public: Skewed(); @@ -54,7 +53,7 @@ namespace Kinematics { const char* name() const override { return "Skew corrected Cartesian"; } protected: - SkewAxis* _axis[6]; + SkewAxis* _skewAxis[6]; // float _buffer[6]; ~Skewed() {}