Add new Kinematic system with skew correction

FluidNC/src/Configuration/Configurable.h

@@ -19,7 +19,7 @@ namespace Configuration {
     public:
         Configurable() = default;
 
-        virtual void validate() const {};
+        virtual void validate() {};
         virtual void group(HandlerBase& handler) = 0;
         virtual void afterParse() {}
         // virtual const char* name() const = 0;

FluidNC/src/Configuration/ConfigurationHandlers.md

@@ -40,7 +40,7 @@ public:
     Pin _data;
     Pin _ws;
 
-    void validate() const override {
+    void validate() override {
         if (!_bck.undefined() || !_data.undefined() || !_ws.undefined()) {
             Assert(!_bck.undefined(), "I2SO BCK pin should be configured once.");
             Assert(!_data.undefined(), "I2SO Data pin should be configured once.");

FluidNC/src/Configuration/_Overview.md

@@ -45,7 +45,7 @@ public:
     Pin _bck;
     // ...
 
-    void validate() const override {
+    void validate() override {
         if (!_bck.undefined() || !_data.undefined() || !_ws.undefined()) {
             Assert(!_bck.undefined(), "I2SO BCK pin should be configured once.");
             // ...

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"
@@ -36,7 +37,7 @@ namespace Kinematics {
         // Configuration handlers:
         void afterParse() override {}
         void group(Configuration::HandlerBase& handler) override {}
-        void validate() const override {}
+        void validate() override {}
 
         // Name of the configurable. Must match the name registered in the cpp file.
         const char* name() const override { return "Cartesian"; }

FluidNC/src/Kinematics/CoreXY.h

@@ -36,7 +36,7 @@ namespace Kinematics {
         bool limitReached(AxisMask& axisMask, MotorMask& motors, MotorMask limited);
 
         // Configuration handlers:
-        void         validate() const override {}
+        void         validate() override {}
         virtual void group(Configuration::HandlerBase& handler) override;
         void         afterParse() override {}
 

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<float>* A, Mtx<float>* const B ) {
+        //log_info( "GJ_invertMatrix" );
+        // Gauss Jordan Matrix inversion. 
+        Mtx<double> 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;
+
+}

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 <typename number>
+        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<float>* _mtx = nullptr;
+        Mtx<float>* _rev = nullptr;
+
+        bool GJ_invertMatrix( const uint size, const Mtx<float>* const A, Mtx<float>* const B );
+
+        ~GenericCartesian();
+    };
+}  //  namespace Kinematics

FluidNC/src/Kinematics/Kinematics.h

@@ -80,7 +80,7 @@ namespace Kinematics {
         // Configuration interface.
         void afterParse() override {}
         void group(Configuration::HandlerBase& handler) override {}
-        void validate() const override {}
+        void validate() override {}
 
         // Name of the configurable. Must match the name registered in the cpp file.
         virtual const char* name() const = 0;