added files

firmware/MotionControllerRP/src/utilities/fp_math.h

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <stdint.h>
+#include <limits>
+#include "math_constants.h"
+
+// #define CHECK_FP_MATH_ERRORS
+#ifdef CHECK_FP_MATH_ERRORS
+  #include "Arduino.h"
+#endif
+
+/**
+ * @brief A header-only utility class for fixed-point arithmetic operations.
+ *
+ * This class provides methods to convert between floating-point and fixed-point,
+ * and perform common fixed-point arithmetic (multiplication, division, reciprocal).
+ * It uses a specified Q-format (number of fractional bits) for all operations.
+ */
+class FPMath {
+  public:
+    // constants
+    const int32_t FP_ONE; // Represents the value 1.0 in fixed-point (1 << q)
+    const int32_t FP_PI;
+    const int32_t FP_TWO_PI;
+
+  public:
+    /**
+     * @brief Constructor for FPMath.
+     * @param q_format The number of fractional bits for fixed-point representation.
+     * A Q-format of 'q' means numbers are stored as N * 2^q.
+     * For example, q=16 means 16 fractional bits.
+     */
+    explicit FPMath(uint8_t q_format)
+        : q(q_format),
+          FP_ONE(int32_t(1) << q_format),
+          US_TO_MSFP(FP_ONE/1000),
+          FP_TO_FLOAT(1.0f/(int32_t(1)<<q_format)),
+          FP_PI(to_fixpoint(Constants::PI_F)),
+          FP_TWO_PI(to_fixpoint(Constants::TWO_PI_F))
+          {
+
+          }
+
+    inline uint8_t get_qformat() {
+      return q;
+    }
+
+    /**
+     * @brief Converts a floating-point value to its fixed-point representation.
+     * @param v The floating-point value to convert.
+     * @return The fixed-point representation of 'v'.
+     */
+    inline int32_t to_fixpoint(float v) const {
+        return static_cast<int32_t>(v * FP_ONE);
+    }
+
+    /**
+     * @brief Converts a fixed-point value back to its floating-point representation.
+     * @param v The fixed-point value to convert.
+     * @return The floating-point representation of 'v'.
+     */
+    inline float from_fixpoint(int32_t v) const {
+        return static_cast<float>(v) * FP_TO_FLOAT;
+    }
+
+    /**
+     * @brief Converts a duration in microseconds (uint32_t) to a fixed-point
+     * representation in milliseconds.
+     * @param dt_us The duration in microseconds, in normal integer format.
+     * @return The duration in milliseconds, in fixed-point format.
+     */
+    inline int32_t duration_us_to_ms(uint32_t dt_us) const {
+        return static_cast<int32_t>(dt_us * US_TO_MSFP);
+    }
+
+    /**
+     * @brief Performs fixed-point multiplication.
+     * @param a First operand in fixed-point (Q-format).
+     * @param b Second operand in fixed-point (Q-format).
+     * @return Result of a * b in fixed-point (Q-format maintained).
+     * Uses 64-bit intermediate multiplication to prevent overflow.
+     */
+    inline int32_t mul(int32_t a, int32_t b) const {
+        // Multiplying two Qx numbers results in a Q(2x) number.
+        // Shifting right by 'q' converts it back to Qx.
+        // Use int64_t for intermediate product to prevent overflow.
+        // be aware that the compiler needs to keep the sign bit untouched
+        int64_t product = static_cast<int64_t>(a) * b;
+        int64_t shifted = product >> q;
+        #ifdef CHECK_FP_MATH_ERRORS
+          if (shifted > INT32_MAX) on_overflow_error();
+          if (shifted < INT32_MIN) on_overflow_error();
+        #endif
+        return static_cast<int32_t>(shifted);
+    }
+
+    /**
+     * @brief Performs fixed-point division.
+     * @param a Numerator in fixed-point (Q-format).
+     * @param b Denominator in fixed-point (Q-format).
+     * @return Result of a / b in fixed-point (Q-format maintained).
+     * Handles division by zero by saturating the result.
+     */
+    inline int32_t div(int32_t a, int32_t b) const {
+        if (b == 0) {
+            // Handle division by zero: return saturation value.
+            return (a >= 0) ? std::numeric_limits<int32_t>::max() : std::numeric_limits<int32_t>::min();
+        }
+        // To maintain Qx precision after division, multiply numerator by FP_ONE (2^q) before dividing.
+        // Use int64_t for intermediate product to prevent overflow.
+        return static_cast<int32_t>((static_cast<int64_t>(a) * FP_ONE) / b);
+    }
+
+    /**
+     * @brief Calculates the reciprocal (1 / a) in fixed-point.
+     * @param a The operand in fixed-point (Q-format).
+     * @return The reciprocal of 'a' in fixed-point (Q-format).
+     */
+    inline int32_t one_over(int32_t a) const {
+        return div(FP_ONE, a);
+    }
+
+    #ifdef CHECK_FP_MATH_ERRORS
+    inline void on_overflow_error() const {
+      Serial.printf("fp_math overflow error:");
+    }
+    #endif
+
+  private:
+    const uint8_t q;      // Number of fractional bits for fixed-point
+    const int32_t US_TO_MSFP; // Pre-calculated fixed-point value for 1/1000 for us to ms conversion
+    const float   FP_TO_FLOAT;
+};

firmware/MotionControllerRP/src/utilities/frequency_counter.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <stdint.h>
+
+class FrequencyCounter {
+  public:
+      FrequencyCounter(uint32_t num_samples) : num_samples(num_samples), sum(0), count(0), freq(0) {}
+
+      void update(float dt) {
+          sum += dt;
+          if (++count >= num_samples && sum) {
+              freq = num_samples / sum;
+              sum = 0;
+              count = 0;
+          }
+      }
+
+      uint32_t get() const { return freq; }
+
+  private:
+      float num_samples, count;
+      float sum;
+      float freq;
+};

firmware/MotionControllerRP/src/utilities/logging.cpp

@@ -0,0 +1,80 @@
+#include "logging.h"
+#include "Arduino.h"
+#include <stdarg.h>
+
+Logger& Logger::instance() {
+  static Logger logger;
+  return logger;
+}
+
+void Logger::begin(unsigned long baudrate, bool wait_for_connection) {
+  Serial.begin(baudrate);
+  if(wait_for_connection)
+    while(!Serial);
+}
+
+void Logger::set_level(ELogLevel level) {
+  current_level = level;
+}
+
+void Logger::debug(const char* fmt, ...) {
+  if (current_level <= ELogLevel::DEBUG) {
+      va_list args;
+      va_start(args, fmt);
+      log(ELogLevel::DEBUG, fmt, args);
+      va_end(args);
+  }
+}
+
+void Logger::info(const char* fmt, ...) {
+  if (current_level <= ELogLevel::INFO) {
+      va_list args;
+      va_start(args, fmt);
+      log(ELogLevel::INFO, fmt, args);
+      va_end(args);
+  }
+}
+
+void Logger::warn(const char* fmt, ...) {
+  if (current_level <= ELogLevel::WARN) {
+      va_list args;
+      va_start(args, fmt);
+      log(ELogLevel::WARN, fmt, args);
+      va_end(args);
+  }
+}
+
+void Logger::error(const char* fmt, ...) {
+  if (current_level <= ELogLevel::ERROR) {
+      va_list args;
+      va_start(args, fmt);
+      log(ELogLevel::ERROR, fmt, args);
+      va_end(args);
+  }
+}
+
+void Logger::log(ELogLevel level, const char* fmt, va_list args) {
+  char buf[128];  // Adjust size as needed
+  vsnprintf(buf, sizeof(buf), fmt, args);
+
+  Serial.print(log_prefix(level));
+  Serial.println(buf);
+}
+
+const char* Logger::log_prefix(ELogLevel level) {
+  switch (level) {
+    case ELogLevel::DEBUG: return "[DEBUG] ";
+    case ELogLevel::INFO:  return "";
+    case ELogLevel::WARN:  return "[WARNING] ";
+    case ELogLevel::ERROR: return "[ERROR] ";
+    default:               return "";
+  }
+}
+
+void error_trap(const char* message) {
+  while(true) {
+    sleep_ms(1000);
+    if(message != nullptr)
+      LOG_ERROR(message);
+  }
+}

firmware/MotionControllerRP/src/utilities/logging.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <stdarg.h>
+
+//*** MACRO *****************************************************************************
+
+#define LOG_DEBUG(...) Logger::instance().debug(__VA_ARGS__)
+#define LOG_INFO(...) Logger::instance().info(__VA_ARGS__)
+#define LOG_WARNING(...) Logger::instance().warn(__VA_ARGS__)
+#define LOG_ERROR(...) Logger::instance().error(__VA_ARGS__)
+
+//*** ENUM ******************************************************************************
+
+enum class ELogLevel {
+  DEBUG,
+  INFO,
+  WARN,
+  ERROR,
+  NONE
+};
+
+//*** CLASS *****************************************************************************
+
+class Logger {
+  public:
+    static Logger& instance();
+
+    void begin(unsigned long baudrate = 115200, bool wait_for_connection=false);
+    void set_level(ELogLevel level);
+
+    void debug(const char* fmt, ...);
+    void info(const char* fmt, ...);
+    void warn(const char* fmt, ...);
+    void error(const char* fmt, ...);
+
+  private:
+    Logger() = default;
+    Logger(const Logger&) = delete;
+    Logger& operator=(const Logger&) = delete;
+
+    void log(ELogLevel level, const char* fmt, va_list args);
+    const char* log_prefix(ELogLevel level);
+
+    ELogLevel current_level = ELogLevel::DEBUG;
+};
+
+//*** FUNCTION **************************************************************************
+
+void error_trap(const char* message="");
+

firmware/MotionControllerRP/src/utilities/math3d.h

@@ -0,0 +1,208 @@
+#pragma once
+#include <cmath>
+
+//--- Vec3F -----------------------------------------------------------------------------
+
+class Vec3F {
+  public:
+    Vec3F() : x(0.0f), y(0.0f), z(0.0f) {}
+    Vec3F(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {}
+
+    Vec3F operator+(const Vec3F& v) const { return Vec3F(x + v.x, y + v.y, z + v.z); }
+    Vec3F operator-(const Vec3F& v) const { return Vec3F(x - v.x, y - v.y, z - v.z); }
+    Vec3F operator*(float s) const { return Vec3F(x * s, y * s, z * s); }
+    Vec3F operator/(float s) const { float t = 1.0f/s; return Vec3F(x*t, y*t, z*t); }
+
+    float dot(const Vec3F& v) const { return x * v.x + y * v.y + z * v.z; }
+    float length() const { return std::sqrt(x * x + y * y + z * z); }
+    float sqr_length() const { return x * x + y * y + z * z; }
+    Vec3F cross(const Vec3F& v) const {
+        return Vec3F(
+            y * v.z - z * v.y,
+            z * v.x - x * v.z,
+            x * v.y - y * v.x
+        );
+    }
+
+    Vec3F normalized() const {
+        float len = length();
+        return len > 0.0f ? (*this / len) : Vec3F(0, 0, 0);
+    }
+
+  public:
+    float x, y, z;
+};
+
+//--- QuaternionF -----------------------------------------------------------------------
+
+class QuaternionF {
+  public:
+    QuaternionF() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {}
+    QuaternionF(float w_, float x_, float y_, float z_) : w(w_), x(x_), y(y_), z(z_) {}
+
+    static QuaternionF from_axis_angle(const Vec3F& axis, float angle_rad) {
+        Vec3F naxis = axis.normalized();
+        float half_angle = 0.5f * angle_rad;
+        float s = std::sin(half_angle);
+        return QuaternionF(std::cos(half_angle), naxis.x * s, naxis.y * s, naxis.z * s);
+    }
+
+    static QuaternionF from_rot_vec(const Vec3F& rot_vec) {
+        float length = rot_vec.length();
+        Vec3F naxis = rot_vec/length;
+        float half_angle = 0.5f * length;
+        float s = std::sin(half_angle);
+        return QuaternionF(std::cos(half_angle), naxis.x * s, naxis.y * s, naxis.z * s);
+    }
+
+    void to_axis_angle(Vec3F& axis, float& angle) const {
+        angle = 2.0f * std::acos(std::fmax(-1.0f, std::fmin(1.0f, w)));
+
+        float sin_half_angle = std::sqrt(1.0f - w * w);
+
+        if (sin_half_angle < 1e-6f)
+          axis = Vec3F(1.0f, 0.0f, 0.0f);
+        else
+          axis = Vec3F(x, y, z) / sin_half_angle;
+    }
+
+    float angle() {
+      // Clamp to avoid domain errors due to floating point
+      float angle = 2.0f * std::acos(std::fmax(-1.0f, std::fmin(1.0f, w)));
+      return angle;  // In radians, range: [0, π]
+    }
+
+    // return inverse of a normalized quaternion
+    QuaternionF normalized_inverse() const {
+      return QuaternionF(w, -x, -y, -z);
+    }
+
+    QuaternionF operator*(const QuaternionF& q) const {
+        return QuaternionF(
+            w * q.w - x * q.x - y * q.y - z * q.z,
+            w * q.x + x * q.w + y * q.z - z * q.y,
+            w * q.y - x * q.z + y * q.w + z * q.x,
+            w * q.z + x * q.y - y * q.x + z * q.w
+        );
+    }
+
+    Vec3F rotate(const Vec3F& v) const {
+      Vec3F qvec(x, y, z);
+      Vec3F t = qvec.cross(v) * 2.0f;
+      return v + t * w + qvec.cross(t);
+    }
+
+    QuaternionF normalized() const {
+        float norm = std::sqrt(w * w + x * x + y * y + z * z);
+        return norm > 0.0f ? QuaternionF(w / norm, x / norm, y / norm, z / norm) : QuaternionF();
+    }
+
+    // spherical linear interpolation, both input quaternions must be normalized
+    QuaternionF slerp(const QuaternionF& other, float t) const {
+      float dot = w * other.w + x * other.x + y * other.y + z * other.z;
+      QuaternionF q2 = other;
+
+      if (dot < 0.0f) {
+          dot = -dot;
+          q2 = QuaternionF(-q2.w, -q2.x, -q2.y, -q2.z);
+      }
+
+      const float DOT_THRESHOLD = 0.9995f;
+      if (dot > DOT_THRESHOLD) {
+          // LERP + normalize for nearly identical quaternions
+          QuaternionF result(
+              w + t * (q2.w - w),
+              x + t * (q2.x - x),
+              y + t * (q2.y - y),
+              z + t * (q2.z - z)
+          );
+          return result.normalized();  // still necessary for lerp path
+      }
+
+      float theta_0 = std::acos(dot);
+      float theta = theta_0 * t;
+      float sin_theta = std::sin(theta);
+      float sin_theta_0 = std::sin(theta_0);
+
+      float s1 = std::cos(theta) - dot * sin_theta / sin_theta_0;
+      float s2 = sin_theta / sin_theta_0;
+
+      return QuaternionF(
+          s1 * w + s2 * q2.w,
+          s1 * x + s2 * q2.x,
+          s1 * y + s2 * q2.y,
+          s1 * z + s2 * q2.z
+      );
+    }
+
+  public:
+    float w, x, y, z;
+};
+
+//--- Pose6DF ---------------------------------------------------------------------------
+
+class Pose6DF {
+  public:
+    Pose6DF() : translation(), rotation() {}
+    Pose6DF(const Vec3F& t, const QuaternionF& r) : translation(t), rotation(r.normalized()) {}
+
+    /// Transform a point from local to world space
+    Vec3F transformPoint(const Vec3F& localPoint) const {
+        return rotation.rotate(localPoint) + translation;
+    }
+
+    /// Combine with another pose (this * other)
+    Pose6DF operator*(const Pose6DF& other) const {
+        Vec3F newTranslation = transformPoint(other.translation);
+        QuaternionF newRotation = (rotation * other.rotation).normalized();
+        return Pose6DF(newTranslation, newRotation);
+    }
+
+    /// Invert this pose
+    Pose6DF inverse() const {
+        QuaternionF inv_rot = rotation.normalized_inverse();
+        Vec3F inv_trans = inv_rot.rotate(translation * -1.0f);
+        return Pose6DF(inv_trans, inv_rot);
+    }
+
+    // Linearly interpolate between two poses
+    static Pose6DF lerp(const Pose6DF& a, const Pose6DF& b, float t) {
+        // Linear interpolation for translation
+        Vec3F translation = a.translation * (1.0f - t) + b.translation * t;
+
+        // Spherical linear interpolation for rotation
+        QuaternionF rotation = a.rotation.slerp(b.rotation, t).normalized();
+
+        return Pose6DF(translation, rotation);
+    }
+    
+  public:
+    Vec3F translation;
+    QuaternionF rotation;
+};
+
+//--- LinearAngular ---------------------------------------------------------------------
+
+class LinearAngular {
+  public:
+    LinearAngular(float l = 1.0f, float a = 1.0f) : linear(l), angular(a) {};
+
+    LinearAngular operator+(const LinearAngular& other) const {
+      return { linear + other.linear, angular + other.angular };
+    }
+    LinearAngular operator-(const LinearAngular& other) const {
+      return { linear - other.linear, angular - other.angular };
+    }
+    LinearAngular operator*(float scalar) const {
+      return { linear * scalar, angular * scalar };
+    }
+    LinearAngular operator*(const LinearAngular& other) const {
+      return { linear * other.linear, angular * other.angular };
+    }
+
+  public:
+    float linear = 1.0f;     // mm/s
+    float angular = 1.0f;    // rad/s
+};
+
+

firmware/MotionControllerRP/src/utilities/math_constants.h

@@ -0,0 +1,8 @@
+#pragma once
+
+namespace Constants {
+  constexpr float PI_F = 3.1415927f;
+  constexpr float TWO_PI_F = 6.2831855f;
+  constexpr float RAD2DEG = 57.29577951308232f;
+  constexpr float DEG2RAD = 0.017453292519943295f;
+}

firmware/MotionControllerRP/src/utilities/ringbuffer.h

@@ -0,0 +1,46 @@
+
+template<typename T, int N>
+class RingBuffer {
+  public:
+    RingBuffer() : head(0), tail(0), count(0) {}
+
+    T* push(const T& value) {
+        if (count == N) return nullptr; // Full
+        buffer[head] = value;
+        T* ptr = &buffer[head];
+        head = (head + 1) % N;
+        ++count;
+        return ptr;
+    }
+
+    bool pop(T& value) {
+        if (count == 0) return false; // Empty
+        value = buffer[tail];
+        tail = (tail + 1) % N;
+        --count;
+        return true;
+    }
+
+    bool pop() {
+        if (count == 0) return false; // Empty
+        tail = (tail + 1) % N;
+        --count;
+        return true;
+    }
+
+    T* peek() {
+      if (count == 0) return nullptr;
+      return &(buffer[tail]);
+    }
+
+    bool empty() const { return count == 0; }
+    bool full() const { return count == N; }
+    int size() const { return count; }
+    int free_item_count() const { return N-count; }
+
+  private:
+    T buffer[N];
+    volatile int head;
+    volatile int tail;
+    volatile int count;
+};

firmware/MotionControllerRP/src/utilities/waveforms.cpp

@@ -0,0 +1,71 @@
+
+#include "waveforms.h"
+#include "math_constants.h"
+#include <algorithm>
+#include <cmath>
+
+float triangle_with_plateau(float x) {
+    const float period = Constants::TWO_PI_F;
+    const float segment = period * 0.16666666666f;
+    float t = fmodf(x, period);  // wrap x to [0, 2π)
+
+    if (t < segment) {
+        return t / segment;  // 0 → +1
+    } else if (t < 2 * segment) {
+        return 1.0f - (t - segment) / segment;  // +1 → 0
+    } else if (t < 3 * segment) {
+        return 0.0f;  // plateau
+    } else if (t < 4 * segment) {
+        return -(t - 3 * segment) / segment;  // 0 → -1
+    } else if (t < 5 * segment) {
+        return -1.0f + (t - 4 * segment) / segment;  // -1 → 0
+    } else {
+        return 0.0f;  // plateau
+    }
+}
+
+float triangle_wave(float x) {
+    const float period = Constants::TWO_PI_F;
+    float t = fmodf(x, period);
+
+    // Normalize t to [0, 1)
+    float phase = t / period;
+
+    // Scale to triangle shape in [-1, 1]
+    if (phase < 0.25f)
+        return 4.0f * phase;             // 0 → +1
+    else if (phase < 0.75f)
+        return 2.0f - 4.0f * phase;      // +1 → -1
+    else
+        return -4.0f + 4.0f * phase;     // -1 → 0
+}
+
+float trapezoidal_wave(float x, float plateau_fraction) {
+    const float two_pi = 2.0f * M_PI;
+
+    // Clamp plateau_fraction to valid range
+    plateau_fraction = std::clamp(plateau_fraction, 0.0f, 0.4999f);
+
+    // Calculate segment widths
+    float plateau_width = plateau_fraction * two_pi;
+    float ramp_width = (two_pi - 2 * plateau_width) / 2.0f;
+
+    // Wrap x into [0, 2π)
+    float phase = fmodf(x, two_pi);
+    if (phase < 0.0f) phase += two_pi;
+
+    if (phase < plateau_width) {
+        // Bottom plateau
+        return -1.0f;
+    } else if (phase < plateau_width + ramp_width) {
+        // Rising edge
+        return -1.0f + 2.0f * (phase - plateau_width) / ramp_width;
+    } else if (phase < plateau_width + ramp_width + plateau_width) {
+        // Top plateau
+        return 1.0f;
+    } else {
+        // Falling edge
+        return 1.0f - 2.0f * (phase - (2 * plateau_width + ramp_width)) / ramp_width;
+    }
+}
+

firmware/MotionControllerRP/src/utilities/waveforms.h

@@ -0,0 +1,4 @@
+
+float triangle_with_plateau(float x);
+float triangle_wave(float x);
+float trapezoidal_wave(float x, float plateau_fraction = 0.2f);