Finalize libraries for separation

All libraries tested
I'm happy with the way the CMakeLists.txt file is
All libraries used in Sheldrake without issues

CMakeLists.txt

@@ -1,10 +1,10 @@
 # CMakeList.txt : Top-level CMake project file, do global configuration
 # and include sub-projects here.
 
-cmake_minimum_required (VERSION 3.10)
+cmake_minimum_required (VERSION 3.12)
 
 # Options
-option(TINYMATRIXMATH_BUILD_TESTS    "Build all projects in the 'test' folder" ON)
+option(TINYMATRIXMATH_BUILD_TESTS    "Build all projects in the 'test' folder"     ON)
 option(TINYMATRIXMATH_BUILD_EXAMPLES "Build all projects in the 'examples' folder" ON)
 option(TINYMATRIXMATH_BUILD_DOCS     "Build all projects in the 'examples' folder" ON)
 
@@ -42,11 +42,7 @@ message("${BoldMagenta}CMake utility directory: ${MACRO_DIRECTORY}${ColorReset}"
 # By using CMake, it's safe to assume that the Standard Library is available.
 # (That's not the case when we're using this library in an Arduino library!)
 # Manually define a compile-time definition
-if(CMAKE_VERSION VERSION_GREATER 3.12)
-    add_compile_definitions(USING_STANDARD_LIBRARY=1)
-else()
-    add_definitions("-DUSING_STANDARD_LIBRARY=1")
-endif()
+# add_compile_definitions(USING_STANDARD_LIBRARY=1)
 
 
 # Serializer
@@ -56,6 +52,7 @@ src/Serializer.cpp
 )
 target_link_libraries (serializer)
 target_include_directories(serializer PUBLIC ${PROJECT_SOURCE_DIR}/src)
+target_compile_definitions(serializer PUBLIC USING_STANDARD_LIBRARY)
 
 
 # TinyMatrixMath
@@ -68,16 +65,24 @@ src/TinyMatrixMath.cpp
 )
 target_link_libraries (tinymatrixmath)
 target_include_directories(tinymatrixmath PUBLIC ${PROJECT_SOURCE_DIR}/src)
+target_compile_definitions(tinymatrixmath PUBLIC USING_STANDARD_LIBRARY)
 
 # TinyControlPolicy
 add_library (tinycontrolpolicies ${PROJECT_SOURCES}
-src/TCP_control_policy.hpp
-src/TCP_controller_PID.hpp
-src/TinyControlPolicies.cpp
+src/TCP_control_policy.hpp 
+src/TCP_controller_LTI.hpp 
+src/TCP_controller_passthrough.hpp 
+src/TCP_controller_PID.hpp 
+src/TCP_robot_link.hpp 
+src/TCP_states.hpp
+src/TCP_utils.hpp
+src/TCP_utils.cpp
+src/TinyControlPolicies.cpp 
 src/TinyControlPolicies.hpp
 )
 target_link_libraries (tinycontrolpolicies)
 target_include_directories(tinycontrolpolicies PUBLIC ${PROJECT_SOURCE_DIR}/src)
+target_compile_definitions(tinycontrolpolicies PUBLIC USING_STANDARD_LIBRARY)
 
 
 

src/Serializer.cpp

@@ -1 +1,16 @@
-#include "Serializer.hpp"
+#include "Serializer.hpp"
+
+namespace serializer{
+
+
+uint8_t create_unique_id(){
+    static uint8_t id = 1;
+    return id++;
+    #ifdef USING_STANDARD_LIBRARY
+    if(id == 0){
+        throw std::runtime_error("Too many objects have been serialized. The maximum number of unique objects is 255.");
+    }
+    #endif
+}
+
+}

src/Serializer.hpp

@@ -30,15 +30,7 @@ namespace serializer{
     } // end of compute_checksum
 
 
-    uint8_t create_unique_id(){
-        static uint8_t id = 1;
-        return id++;
-        #ifdef USING_STANDARD_LIBRARY
-        if(id == 0){
-            throw std::runtime_error("Too many objects have been serialized. The maximum number of unique objects is 255.");
-        }
-        #endif
-    }
+    uint8_t create_unique_id();
 
 
 

src/TCP_control_policy.hpp

@@ -1,13 +1,11 @@
 #pragma once
 
 #include "TinyMatrixMath.hpp"
-
-#ifdef USING_STANDARD_LIBRARY
-#include <chrono> // for emulating millis()
-#endif
+#include "TCP_utils.hpp"
 
 namespace tcp{
 
+
     template<tmm::Size num_inputs, tmm::Size num_outputs, typename Scalar = float>
     class ControlPolicy{
         public:
@@ -54,18 +52,5 @@ namespace tcp{
         unsigned long _timestamp = 0;
         /// @brief The time between the last two update() calls in milliseconds
         unsigned long _instantaneous_update_period = 0;
-
-        // Subroutines to emulate the Arduino library with the Standard Library
-        #ifdef USING_STANDARD_LIBRARY
-        // Replicates Arduino's "millis()" if using the Standard Library
-        unsigned long millis(){
-            namespace sc = std::chrono;
-            auto time = sc::system_clock::now();
-            auto since_epoch = time.time_since_epoch();
-            auto millis = sc::duration_cast<sc::milliseconds>(since_epoch);
-            long now = millis.count();
-            return now;
-        }
-        #endif
     };
 }

src/TCP_controller_LTI.hpp

@@ -4,7 +4,7 @@
 
 namespace tcp{
 
-    /// @brief The classic discrete-time linear time-invariant system: X_dot = A X + B U; Y = C X + D <br>
+    /// @brief The classic discrete-time linear time-invariant system: `X_dot = A X + B U; Y = C X + D` <br>
     /// It uses the system's time to compensate for function calls at irregular intervals (units are in seconds).
     /// @tparam Scalar the common datatype to use for all operations.
     /// @tparam num_inputs          the size of u

src/TCP_controller_PID.hpp

@@ -11,7 +11,7 @@ namespace tcp{
     template<tmm::Size num_inputs_and_outputs = 1, typename Scalar = float>
     class Controller_PID: public ControlPolicy<num_inputs_and_outputs, num_inputs_and_outputs, Scalar>{
         public:
-        tmm::Matrix<1, num_inputs_and_outputs, Scalar> Kp, Ki, Kd;
+        tmm::Matrix<num_inputs_and_outputs, 1, Scalar> Kp, Ki, Kd;
         using ControlPolicy<num_inputs_and_outputs, num_inputs_and_outputs, Scalar>::time_since_last_update_seconds;
         using ControlPolicy<num_inputs_and_outputs, num_inputs_and_outputs, Scalar>::update_timestamp;
         virtual void update(){
@@ -24,6 +24,6 @@ namespace tcp{
             update_timestamp();
         }
         private:
-        tmm::Matrix<1, num_inputs_and_outputs, Scalar> _previous, _sum;
+        tmm::Matrix<num_inputs_and_outputs, 1, Scalar> _previous, _sum;
     };
 }

src/TCP_states.hpp

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "TinyMatrixMath.hpp"
+#include "TCP_utils.hpp"
+
+namespace tcp{
+
+    template<tmm::Size num_inputs, typename Scalar = float>
+    class States{
+        public:
+        tmm::Matrix<num_inputs,  1, Scalar> states;
+
+        // Timekeeping
+
+        /// @brief Get the time since the last call to update() in milliseconds
+        /// @return unsigned long
+        unsigned long time_since_last_update_milliseconds(){
+            return millis()-_timestamp;
+        }
+
+        /// @brief Get the time between the last two update() calls in milliseconds
+        /// @return unsigned long
+        unsigned long instantaneous_update_period_milliseconds(){return _instantaneous_update_period;}
+
+        /// @brief Get the time since the last call to update() in seconds
+        /// @return float
+        float time_since_last_update_seconds(){
+            return (float)time_since_last_update_seconds() / 1000.f;
+        }
+
+        /// @brief Get the time between the last two update() calls in seconds
+        /// @return float
+        float instantaneous_update_period_seconds(){
+            return (float)instantaneous_update_period_milliseconds() / 1000.f;
+        }
+
+        /// @brief Call this at the end of your update() function to update the timekeeping variables
+        void update_timestamp(){
+            unsigned long now = millis(); 
+            _instantaneous_update_period = now-_timestamp; 
+            _timestamp = now;
+        }
+
+        private:
+        /// @brief The value of millis() during the last time update() was called
+        unsigned long _timestamp = 0;
+        /// @brief The time between the last two update() calls in milliseconds
+        unsigned long _instantaneous_update_period = 0;
+    };
+}

src/TCP_utils.cpp

@@ -0,0 +1,22 @@
+#include "TCP_utils.hpp"
+
+#ifdef USING_STANDARD_LIBRARY
+
+#include <chrono> // for emulating millis()
+
+namespace tcp{
+
+    // Subroutines to emulate the Arduino library with the Standard Library
+    // Replicates Arduino's "millis()" if using the Standard Library
+    unsigned long millis(){
+        namespace sc = std::chrono;
+        auto time = sc::system_clock::now();
+        auto since_epoch = time.time_since_epoch();
+        auto millis = sc::duration_cast<sc::milliseconds>(since_epoch);
+        long now = millis.count();
+        return now;
+    }
+
+}
+
+#endif

src/TCP_utils.hpp

@@ -0,0 +1,13 @@
+#pragma once
+
+#if USING_STANDARD_LIBRARY
+
+namespace tcp{
+
+    // Subroutines to emulate the Arduino library with the Standard Library
+    // Replicates Arduino's "millis()" if using the Standard Library
+    unsigned long millis();
+
+}
+
+#endif

src/TMM_enable_if.hpp

@@ -2,17 +2,18 @@
 
 #pragma once
 
-
-
-template<bool Cond, class T = void> struct enable_if {};
-template<class T> struct enable_if<true, T> { typedef T type; };
-
-template< bool B, class T = void >
-using enable_if_t = typename enable_if<B,T>::type;
-
-
 // Sample usage: enable_if_t<condition, function_return_type> function_name(){...; return ...;}
 
 // Example:
 //      enable this_function only if n==1 and m==1, and its return type is Scalar
 //      enable_if_t<(n==1&&m==1), insert_return_type_here>::type() this_function(){return insert_return_type_here();}
+
+namespace tmm{
+
+    template<bool Cond, class T = void> struct enable_if {};
+    template<class T> struct enable_if<true, T> { typedef T type; };
+
+    template< bool B, class T = void >
+    using enable_if_t = typename enable_if<B,T>::type;
+
+}

src/TMM_matrix.hpp

@@ -82,11 +82,37 @@ namespace tmm{
         operator[](Size i)
         {return data[i];}
 
-
-        template<typename T = Scalar, typename = enable_if_t<(m==1&&n==1), T>> operator T() { 
+        /// @brief Implicit casting to the Scalar type, enabled only if this matrix is 1x1
+        template<typename T = Scalar, typename = tmm::enable_if_t<(m==1&&n==1), T>> operator T() { 
             return data[0][0]; 
         }
 
+        /// @brief Returns true if all elements of this matrix are equal to the elements of another matrix, within some tolerance
+        /// @tparam Other_Scalar the type of the other matrix
+        /// @param other the matrix to compare to
+        /// @param tolerance the tolerance to use when comparing elements
+        /// @return true if all elements of this matrix are equal to the elements of another matrix, within some tolerance
+        template<typename Other_Scalar>
+        bool equals(const Matrix<n,m,Scalar> &other, Scalar tolerance) const {
+            for(Size i = 0; i < n; i++) 
+            for(Size j = 0; j < m; j++){
+                Scalar comp = data[i][j]-other.data[i][j];
+                if(comp < -tolerance || comp > tolerance) return false;
+            }
+            return true;
+        }
+
+        /// @brief Returns true if all elements of this matrix are identically equal to the elements of another matrix
+        /// @param other the matrix to compare to
+        /// @return true if all elements of this matrix are identically equal to the elements of another matrix
+        /// @note For floating-point matrices, consider using equals() instead. equals() allows for a tolerance.
+        template<typename Other_Scalar>
+        bool operator==(const Matrix<n,m,Other_Scalar> &other) const {
+            return equals<Other_Scalar>(other, 0.f);
+        }
+
+
+
         template<Size q> 
         Matrix<n,m+q,Scalar>
         augmentAfter(const Matrix<n,q,Scalar> &other) const
@@ -332,7 +358,8 @@ namespace tmm{
             return get<n,1>(0, j);
         }
 
-
+        /// @brief Copies the contents of this matrix to another matrix
+        /// @param other the matrix to copy to
         void
         copyTo(Matrix<n,m,Scalar> &other) const
         {
@@ -383,7 +410,7 @@ namespace tmm{
 
         #ifndef TMM_DISABLE_RECURSIVE
         template <typename T = Scalar>
-        enable_if_t<(m==n), T>
+        tmm::enable_if_t<(m==n), T>
         determinant() const
         {
             if(n == 0) { return 1; }
@@ -420,7 +447,7 @@ namespace tmm{
 
 
         template <typename T = Matrix<n,n,Scalar>>
-        enable_if_t<(m==n), T>
+        tmm::enable_if_t<(m==n), T>
         cofactor() const
         {
             Matrix<n,n,Scalar> M;
@@ -470,7 +497,7 @@ namespace tmm{
         /// @warning This method is not implemented correctly yet.
         /// @todo fix and test this method
         template <typename T = Matrix<n,n,Scalar>>
-        enable_if_t<(m==n), T>
+        tmm::enable_if_t<(m==n), T>
         inverse() const
         {
             // The matrix is the adjugate divided by the determinant,