added files

firmware/MotionControllerRP/src/servo_control/encoder_lut.cpp

@@ -0,0 +1,176 @@
+#include "encoder_lut.h"
+#include "pico/stdlib.h"
+#include "utilities/logging.h"
+#include "utilities/math_constants.h"
+
+void LookupTable::init(int32_t size, float input_min, float input_max) {
+  lookup_table.clear();
+  lookup_table.resize(size, 0.0f);
+  LookupTable::input_min = input_min;
+  LookupTable::input_max = input_max;
+  LookupTable::one_over_input_range = 1.0f/(input_max-input_min);
+}
+
+void LookupTable::clear() {
+  lookup_table.clear();
+}
+
+// returns the size of the lookup table
+uint32_t LookupTable::size() {
+  return (uint32_t)lookup_table.size();
+}
+
+void LookupTable::set_entry(int32_t idx, float v) {
+  lookup_table[idx] = v;
+}
+
+// set an entry of the lookup table
+float LookupTable::get_entry(int32_t idx) {
+  return lookup_table[idx];
+}
+
+float LookupTable::evaluate(float x) const {
+  if (lookup_table.empty() || lookup_table.size() < 2)
+    return 0.0f;
+
+  int32_t table_size = lookup_table.size();
+  float t = (x - input_min) * one_over_input_range;
+  float pos = t * (table_size - 1);
+  float frac;
+  size_t index;
+
+  if (t < 0.0f) {
+    return lookup_table.front();
+    // Extrapolate to the left using first two points
+    index = 0;
+    frac = pos;  // pos is negative
+  } else if (t >= 1.0f) {
+    return lookup_table.back();
+    // Extrapolate to the right using last two points
+    index = table_size - 2;
+    frac = pos - (table_size - 2);
+  } else {
+    // Interpolate normally
+    index = static_cast<size_t>(std::floor(pos));
+    frac = pos - index;
+  }
+
+  float a = lookup_table[index];
+  float b = lookup_table[index + 1];
+
+  return a + frac * (b - a);  // Linear interpolation or extrapolation
+}
+
+
+bool LookupTable::is_monotonic() const {
+  if (lookup_table.size() < 2)
+    return true;
+
+  bool increasing = true, decreasing = true;
+  for (size_t i = 1; i < lookup_table.size(); ++i) {
+    float b = lookup_table[i - 1];
+    float a = lookup_table[i];
+
+    if (a < b) increasing = false;
+    if (a > b) decreasing = false;
+  }
+
+  return increasing || decreasing;
+}
+
+bool almost_equal(float a, float b, float rel_tol = 1e-6f, float abs_tol = 1e-6f) {
+    return std::fabs(a - b) <= std::max(rel_tol * std::max(std::fabs(a), std::fabs(b)), abs_tol);
+}
+
+float LookupTable::evaluate_inverse(float y) const {
+  int size = static_cast<int>(lookup_table.size());
+  if (size < 2) return input_min;
+
+  int low = 0;
+  int high = size - 1;
+  bool increasing = lookup_table.front() < lookup_table.back();
+
+  // Clamp y outside the range
+// Clamp y outside the range
+if ((increasing && y <= lookup_table.front()) || 
+    (!increasing && y >= lookup_table.front()))
+  return input_min;
+if ((increasing && y >= lookup_table.back()) || 
+    (!increasing && y <= lookup_table.back()))
+  return input_max;
+
+  // Binary search to find the interval
+  while (high - low > 1) {
+    int mid = (low + high) / 2;
+    float val = lookup_table[mid];
+
+    if ((increasing && val < y) || (!increasing && val > y))
+      low = mid;
+    else
+      high = mid;
+  }
+
+  // Interpolate between low and high
+  float y0 = lookup_table[low];
+  float y1 = lookup_table[high];
+
+  if (std::fabs(y1 - y0) < std::numeric_limits<float>::epsilon()) {
+    // Avoid division by zero if both entries are equal
+    float t = float(low) / (size - 1);
+    return input_min + t * (input_max - input_min);
+  }
+
+  float t = (y - y0) / (y1 - y0);
+  float pos = (float(low) + t) / (size - 1);
+
+  return input_min + pos * (input_max - input_min);
+}
+
+// inverts the lookup table so it represents the funcion x = fi(y) given y = f(x) 
+bool LookupTable::invert(int new_size) {
+  if (lookup_table.empty() || new_size <= 0) {
+    LOG_ERROR("invert_lut(): lut size is zero");
+    return false;
+  }
+
+  if (!is_monotonic()) {
+    LOG_ERROR("invert_lut(): lut is not monotonic");
+    return false;
+  }
+
+  // Find the output (y) range of the current LUT
+  float output_min = lookup_table.front();
+  float output_max = lookup_table.back();
+  if (output_max < output_min) {
+      std::swap(output_min, output_max);
+  }
+
+  // Prepare new LUT data
+  std::vector<float> new_lut(new_size);
+  float delta_y = (output_max - output_min) / (new_size - 1);
+
+  for (int i = 0; i < new_size; ++i) {
+      float y = output_min + i * delta_y;
+      new_lut[i] = evaluate_inverse(y);  // find x for given y
+  }
+
+  // Replace old LUT with the inverted LUT
+  lookup_table = std::move(new_lut);
+  input_min = output_min;
+  input_max = output_max;
+  one_over_input_range = 1.0f/(input_max-input_min);
+
+  return true;
+}
+
+void LookupTable::print_to_log() const {
+  int size = lookup_table.size();
+  if (size == 0) return;
+
+  float step = (input_max - input_min) / (size - 1);
+  for (int i = 0; i < size; ++i) {
+    float x = input_min + i * step;
+    float y = lookup_table[i];
+    LOG_INFO("%.6f;%.6f", x, y);
+  }
+}

firmware/MotionControllerRP/src/servo_control/encoder_lut.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <vector>
+#include <cstdint>
+#include <cmath>
+
+
+class LookupTable {
+  public:
+    LookupTable() {}
+
+    // initializes the lookup table to a given size and input range
+    void  init(int32_t size, float input_min, float input_max);
+
+    // clear the lookup table, use init to use it again
+    void  clear();
+
+    // returns the size of the lookup table
+    uint32_t size();
+
+    // set an entry of the lookup table
+    void  set_entry(int32_t idx, float v);
+
+    // set an entry of the lookup table
+    float  get_entry(int32_t idx);
+
+    // evaluate the lookup table at a given position with linear interpolation
+    float evaluate(float x) const;
+
+    // evaluate the inverse of the lookup table function (very slow), the LUT must be monotonic
+    float evaluate_inverse(float y) const;
+
+    // inverts the lookup table so it represents the funcion x = fi(y) given y = f(x) 
+    bool invert(int new_size);
+
+    // check if the lookup table is monotonic
+    bool is_monotonic() const;
+
+    // prints the lookup table using the logger
+    void print_to_log() const;
+
+  private:
+    float input_min = 0.0f;
+    float input_max = 0.0f;
+    float one_over_input_range = 1.0f;
+    std::vector<float> lookup_table;
+};
+
+//*** FUNCTION ***********************************************************************************/
+

firmware/MotionControllerRP/src/servo_control/pid.cpp

@@ -0,0 +1,70 @@
+#include "pid.h"
+#include <algorithm>
+
+PIDController::PIDController()
+    : kP(0.0f), kI(0.0f), kD(0.0f), kI_half(0.0f)
+    , output_limit(0.0f), windup_limit(0.0f)
+    , error_prev(0.0f), integral_prev(0.0f)
+{
+}
+
+void PIDController::set_parameter(float kP, float kI, float kD, float output_limit, float windup_limit) {
+  PIDController::kP = kP;
+  PIDController::kI = kI;
+  PIDController::kD = kD;
+  PIDController::output_limit = output_limit;
+  PIDController::windup_limit = windup_limit;
+
+  PIDController::kI_half = kI*0.5f;
+}
+
+// PID controller function
+float PIDController::compute(float error, float dt, float one_over_dt) {
+    // Proportional component
+    float proportional = kP * error;
+    float output = proportional;
+
+    // Integral component
+    if(kI != 0.0f) {
+      // Tustin transform of the integral part
+      // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
+      float integral = integral_prev + kI_half*dt*(error + error_prev);
+      integral = std::clamp(integral, -windup_limit, windup_limit);
+      output += integral;
+      integral_prev = integral;
+    }
+
+    // Derivative component
+    if(kD != 0.0f) {
+      // u_dk = D(ek - ek_1)/Ts
+      float derivative = kD*(error - error_prev)*one_over_dt;
+      output += derivative;
+    }
+
+    // clamp output and store error
+    output = std::clamp(output, -output_limit, output_limit);
+    error_prev = error;
+    
+    return output;
+}
+
+void PIDController::reset(){
+    integral_prev = 0.0f;
+    error_prev = 0.0f;
+}
+
+//--- LowpassFilter -----------------------------------------------------------
+
+LowpassFilter::LowpassFilter(): value_prev(0.0f), time_constant(1.0f) {
+}
+
+void LowpassFilter::set_time_constant(float time_constant) {
+  LowpassFilter::time_constant = time_constant;
+}
+
+float LowpassFilter::update(float value, float dt) {
+  float alpha = time_constant/(time_constant + dt);
+  float v = value_prev*alpha + (1.0f - alpha)*value;
+  value_prev = v;
+  return v;
+}

firmware/MotionControllerRP/src/servo_control/pid.h

@@ -0,0 +1,40 @@
+#pragma once
+
+//--- LowpassFilter -----------------------------------------------------------
+
+class LowpassFilter {
+  public:
+    LowpassFilter();
+
+    void  set_time_constant(float time_constant);
+    float update(float value, float dt);
+
+  private:
+    float value_prev;
+    float time_constant;
+};
+
+//--- PIDController -----------------------------------------------------------
+
+class PIDController {
+  public:
+    PIDController();
+    ~PIDController() = default;
+
+    void  set_parameter(float kP, float kI, float kD, float output_limit, float windup_limit);
+    float compute(float error, float dt, float one_over_dt);
+    void  reset();
+
+  protected:
+    float output_limit; // Maximum output value
+    float windup_limit; // Maximum output value
+
+    float kP; // Proportional gain
+    float kI; // Integral gain
+    float kD; // Derivative gain
+
+    float error_prev; // last tracking error value
+    float integral_prev; // last integral component value
+
+    float kI_half; // to avoid multiply
+};

firmware/MotionControllerRP/src/servo_control/servo_controller.cpp

@@ -0,0 +1,283 @@
+#include "hardware/timer.h"
+#include "Arduino.h"
+
+#include "servo_controller.h"
+#include "utilities/logging.h"
+#include "utilities/math_constants.h"
+
+#include <algorithm>
+
+ServoController::ServoController(
+  MOTOR_DRIVER_TYPE& motor_driver, 
+  ENCODER_TYPE& encoder, 
+  int32_t motor_pole_pair_count) :
+    motor_driver(motor_driver),
+    encoder(encoder), 
+    motorpos_to_field_angle(motor_pole_pair_count) 
+{
+  motor_pos = 0.0f;
+  pos_error = 0.0f;
+}
+
+void ServoController::init(float max_motor_amplitude) {
+  ServoController::motor_current_amplitude = max_motor_amplitude;
+
+  // setup motor driver
+  motor_driver.begin();
+  motor_driver.set_amplitude(0.0f, true); 
+  motor_driver.enable();
+  motor_driver.set_field_angle(0.0f);
+
+  // soft start
+  for(int i=0; i<100; i++) {
+    motor_driver.set_amplitude(motor_current_amplitude*float(i)/(100-1), true);
+    sleep_ms(1);
+  }
+
+  velocity_lowpass.set_time_constant(0.0025f);
+  pos_controller.set_parameter(150.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
+  velocity_controller.set_parameter(0.2f, 100.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
+}
+
+void ServoController::set_encoder_lut(LookupTable& enc_to_pos_lut) {
+  ServoController::enc_to_pos_lut = enc_to_pos_lut;
+}
+
+void ServoController::update(float target_motor_pos, float dt, float one_over_dt) { 
+  // read encoder
+  int32_t encoder_angle_raw = encoder.read_abs_angle_raw();
+
+  // convert encoder angle to motor pos using LUT and compute field angle
+  motor_pos = encoder_angle_to_motor_pos(encoder_angle_raw);
+  float field_angle = motor_pos_to_field_angle(motor_pos);
+
+  // position controll loop
+  pos_error = target_motor_pos-motor_pos;
+  float velocity_target = pos_controller.compute(pos_error, dt, one_over_dt);
+
+  // velocity controll loop
+  float velocity_unfiltered = (motor_pos - motor_pos_prev)*one_over_dt;
+  velocity = velocity_lowpass.update(velocity_unfiltered, dt);
+  float torque_target = velocity_controller.compute(velocity_target-velocity, dt, one_over_dt);
+
+  // torque controll loop
+  output = torque_target;
+
+  // set new field direction
+  // motor_driver.set_amplitude(std::clamp(abs(output*10.0f), 0.1f, 0.5f), false);
+  motor_driver.set_field_angle(field_angle + output); 
+
+  // store values for next update
+  motor_pos_prev = motor_pos;
+}
+
+bool ServoController::at_position(float motor_pos_eps) {
+  return fabs(pos_error) < motor_pos_eps;
+}
+
+float ServoController::read_position() {
+  return encoder_angle_to_motor_pos(encoder.read_abs_angle_raw());
+}
+
+float ServoController::get_position() {
+  return motor_pos;
+}
+
+float ServoController::get_position_error() {
+  return pos_error;
+}
+
+bool ServoController::move_to(float target_motor_pos, float at_pos_eps, float settle_time_s, float timeout_s) {
+  uint64_t start_time_us = time_us_64();
+  uint64_t time_us = start_time_us;
+  uint64_t pos_reached_time_us = 0;
+  uint64_t last_time = time_us;
+  uint32_t settle_time_us = settle_time_s*1e6f;
+  uint32_t timeout_us = timeout_s*1e6f;
+
+  do {
+    // get time and detla time
+    time_us = time_us_64();
+    float dt = float(time_us - last_time)*1e-6f;
+    last_time = time_us;
+
+    float pos_error;
+    update(target_motor_pos, dt, 1.0f/dt);
+
+    // check if traget position reached
+    if(pos_reached_time_us == 0) {
+      if(at_position(at_pos_eps))
+        pos_reached_time_us = time_us;
+    } else {
+      if(time_us-pos_reached_time_us > settle_time_us)
+        return true;
+    }
+
+  } while(time_us-start_time_us < timeout_us);
+
+  return false;
+}
+
+void ServoController::move_to_open_loop(float target_motor_pos, float motor_angular_velocity) {
+  // Determine direction of movement at the start
+  const bool moving_forward = target_motor_pos > motor_pos;
+
+  uint64_t last_time = time_us_64();
+  while ((moving_forward && motor_pos < target_motor_pos) ||
+         (!moving_forward && motor_pos > target_motor_pos))
+  {
+    uint64_t time_us = time_us_64();
+    float dt = float(time_us - last_time) * 1e-6f;
+    last_time = time_us;
+
+    // update encoder regularly
+    encoder.read_abs_angle_raw();
+
+    // update motor position
+    motor_pos += moving_forward ? motor_angular_velocity * dt : -motor_angular_velocity * dt;
+
+    // set field ange to new position
+    float clamped_motor_pos = moving_forward ? std::min(motor_pos, target_motor_pos) : 
+                                               std::max(motor_pos, target_motor_pos);
+    motor_driver.set_field_angle(motor_pos_to_field_angle(clamped_motor_pos));
+    sleep_us(100);
+  }
+  
+  motor_pos = target_motor_pos;
+}
+
+void ServoController::home(float motor_velocity, float search_range, float current) {
+  bool search_failed = false;
+  float pos_offset = 0.0f;
+  motor_driver.set_amplitude(current, true);
+  float eval_pos_delta = (Constants::TWO_PI_F*0.1)/motorpos_to_field_angle;
+
+  // determine expected encoder angle delta for motion of eval_pos_delta
+  motor_driver.set_field_angle(motor_pos_to_field_angle(motor_pos+eval_pos_delta));
+  sleep_ms(200);
+  float angle1 = encoder.read_abs_angle();
+
+  motor_driver.set_field_angle(motor_pos_to_field_angle(motor_pos));
+  sleep_ms(200);
+  float angle2 = encoder.read_abs_angle();
+  float expected_encoder_delta = (angle2-angle1);
+  
+  // start homing search
+  uint64_t last_time = time_us_64();
+  float encoder_angle_prev = encoder.read_abs_angle();
+  float last_eval_offset = 0.0f;
+
+  while(true) {
+    // compute time delta
+    uint64_t time_us = time_us_64();
+    float dt = float(time_us - last_time) * 1e-6f;
+    last_time = time_us;
+
+    // move motor and read encoder
+    pos_offset += motor_velocity * dt;
+    motor_driver.set_field_angle(motor_pos_to_field_angle(motor_pos+pos_offset));
+    float encoder_angle = encoder.read_abs_angle();
+
+    // check ratio of measured encoder delta to expected delta to determine motor stop
+    if(fabs(last_eval_offset-pos_offset) > eval_pos_delta) {
+      float encoder_delta = (encoder_angle - encoder_angle_prev);
+      float encoder_velocity_ratio = encoder_delta/expected_encoder_delta;
+      // Serial.printf(">encoder_velocity_ratio: %f\n", encoder_velocity_ratio);
+      // Serial.printf(">encoder_velocity: %f\n", encoder_velocity);
+      if(encoder_velocity_ratio < 0.05f)
+        break;
+      encoder_angle_prev = encoder_angle;
+      last_eval_offset = pos_offset;
+    }
+
+    // check if search range exeeded
+    if(fabs(pos_offset) > search_range) {
+      search_failed = true;
+      break;
+    }
+  }
+
+  // reset positions
+  motor_pos = 0;
+  motor_driver.set_field_angle(0);
+  sleep_ms(200);
+  encoder.reset_abs_angle();
+ 
+  // set normal motor current
+  motor_driver.set_amplitude(motor_current_amplitude, true);
+}
+
+ServoController::ENCODER_TYPE& ServoController::get_encoder() {
+  return encoder;
+}
+
+ServoController::MOTOR_DRIVER_TYPE& ServoController::get_motor_driver() {
+  return motor_driver;
+}
+
+float ServoController::encoder_angle_to_motor_pos(int32_t encoder_angle_raw) {
+  // TODO: use lut here
+  if(enc_to_pos_lut.size() == 0) {
+    int32_t encoder_cpr = encoder.get_rawcounts_per_rev();
+    return encoder_angle_raw*Constants::TWO_PI_F/encoder_cpr/(7.5f*4);
+  } else {
+    return enc_to_pos_lut.evaluate(encoder_angle_raw);
+  }
+}
+
+float ServoController::motor_pos_to_field_angle(float motor_pos) {
+  return motor_pos*motorpos_to_field_angle;
+}
+
+float ServoController::motor_velocity_to_field_velocity(float v) {
+  return v*motorpos_to_field_angle;
+}
+
+//*** FUNCTION ***********************************************************************************/
+
+bool build_motor_to_enc_angle_lut(
+            LookupTable& lut,
+            ServoController& servo_controller,
+            float min_motor_angle,
+            float max_motor_angle,
+            size_t size)
+{
+  LOG_INFO("Measuring motor to encoder angle lookup table...");
+  float speed = 1.0f;
+  float input_min = min_motor_angle;
+  float input_max = max_motor_angle;
+
+  lut.init(size, input_min, input_max);
+  // float initial_pos = servo_controller.read_position();
+  // move to starting position
+  servo_controller.move_to_open_loop(min_motor_angle, 2.0f);
+  servo_controller.get_encoder().reset_abs_angle(0); // Reset encoder to 0 at min_motor_angle
+
+  float step = float(input_max - input_min) / (size - 1);
+
+  // Measure in increasing direction
+  for (size_t i = 0; i < size; ++i) {
+    float target_motor_angle = input_min + step * i;
+    servo_controller.move_to_open_loop(target_motor_angle, speed);
+    // sleep_ms(0);
+    float encoder_angle_raw = servo_controller.get_encoder().read_abs_angle_raw();
+    lut.set_entry(i, encoder_angle_raw);
+  }
+
+  // Measure in decreasing direction (average with increasing direction)
+  for (size_t i = 0; i < size; ++i) {
+    float target_motor_angle = input_max - step * i; // Start from max and go down
+    servo_controller.move_to_open_loop(target_motor_angle, speed);
+    // sleep_ms(0);
+    float encoder_angle_raw = servo_controller.get_encoder().read_abs_angle_raw();
+    // Average with the previously recorded value
+    int idx = size-1-i;
+    lut.set_entry(idx, (lut.get_entry(idx) + encoder_angle_raw) / 2.0f);
+  }
+
+  // move to starting position
+  servo_controller.move_to_open_loop(min_motor_angle, 2.0f);
+
+  LOG_INFO(">finished");
+  return true;
+}

firmware/MotionControllerRP/src/servo_control/servo_controller.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include "hardware/MT6835_encoder.h"
+#include "hardware/TB6612_motor_driver.h"
+#include "encoder_lut.h"
+#include "pid.h"
+
+class ServoController {
+  public:
+    // use defines instead of virtual functions for speed
+    // TODO: check if this makes any difference and change accordingly
+    typedef TB6612MotorDriver MOTOR_DRIVER_TYPE;
+    typedef MT6835Encoder ENCODER_TYPE;
+
+  public:
+    ServoController(MOTOR_DRIVER_TYPE& motor_driver, ENCODER_TYPE& encoder, int32_t motor_pole_pairs);
+
+    void init(float max_motor_amplitude);
+
+    void set_encoder_lut(LookupTable& enc_to_pos_lut);
+
+    void update(float target_motor_pos,
+                float dt,
+                float one_over_dt);
+
+    bool at_position(float motor_pos_eps);
+
+    float read_position();
+
+    float get_position();
+
+    float get_position_error();
+
+    bool move_to(float target_motor_angle, 
+                 float at_pos_motor_angle_eps, 
+                 float settle_time_ms,
+                 float timeout_us);
+
+    void move_to_open_loop(float target_motor_angle, 
+                           float angular_velocity);
+
+    void home(float motor_velocity, float search_range, float current=0.2f);
+    
+    ENCODER_TYPE& get_encoder();
+    MOTOR_DRIVER_TYPE& get_motor_driver();
+    float output;
+
+  private:
+    float encoder_angle_to_motor_pos(int32_t encoder_angle_raw);
+    float motor_pos_to_field_angle(float motor_pos);
+    float motor_velocity_to_field_velocity(float v);
+
+  private:
+    ENCODER_TYPE& encoder;
+    MOTOR_DRIVER_TYPE& motor_driver;
+    LookupTable enc_to_pos_lut;
+
+    LowpassFilter velocity_lowpass;
+    PIDController pos_controller;
+    PIDController velocity_controller;
+
+    float motor_pos = 0;                  // current motor position
+    float motor_pos_prev = 0;             // previous motor position
+    float pos_error = 0;                  // current position error as computed by upate()
+    float velocity = 0;                   // current velocity estimate
+
+    float motorpos_to_field_angle = 0;    // conversion factor derived from pole pair count
+    float motor_current_amplitude = 0.5f;
+};
+
+//*** FUNCTIONS **************************************************************/
+
+bool build_motor_to_enc_angle_lut(
+  LookupTable& lut,
+  ServoController& servo_controller,
+  float min_motor_angle,
+  float max_motor_angle,
+  size_t size);