diff --git a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
index 9e988ff..b76acd6 100644
--- a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
+++ b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
@@ -3,83 +3,92 @@
 // being distributed under the MIT liscence as well. Thank you SimpleFOC !
 // --------------------------------------------------------------------------------------
 
-
 #include "MT6835_encoder.h"
 #include "hardware/spi.h"
 #include "hardware/gpio.h"
 #include "pico/stdlib.h"
 
 void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint pin_miso, int32_t baudrate_hz) {
-    // Set GPIO functions to SPI
-    gpio_set_function(pin_sck, GPIO_FUNC_SPI);
-    gpio_set_function(pin_mosi, GPIO_FUNC_SPI);
-    gpio_set_function(pin_miso, GPIO_FUNC_SPI);
-    
-    // SPI format: 8 bits, mode 3 (CPOL=1, CPHA=1)
-    spi_init(spi, baudrate_hz);
-    spi_set_format(spi, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
+  // Set GPIO functions to SPI
+  gpio_set_function(pin_sck, GPIO_FUNC_SPI);
+  gpio_set_function(pin_mosi, GPIO_FUNC_SPI);
+  gpio_set_function(pin_miso, GPIO_FUNC_SPI);
+  
+  // SPI format: 8 bits, mode 3 (CPOL=1, CPHA=1)
+  spi_init(spi, baudrate_hz);
+  spi_set_format(spi, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
 }
 
-
 MT6835Encoder::MT6835Encoder(spi_inst_t* spi, uint cs_pin) : spi(spi), cs_pin(cs_pin) {
-    // nop
+  // nop
 }
 
 MT6835Encoder::~MT6835Encoder() {
-    // nop
+  // nop
 }
 
 void MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
-    if (cs_pin >= 0) {
-        gpio_init(cs_pin);
-        gpio_set_dir(cs_pin, GPIO_OUT);
-        gpio_put(cs_pin, 1);  // CS high
-    }
+  if (cs_pin >= 0) {
+      gpio_init(cs_pin);
+      gpio_set_dir(cs_pin, GPIO_OUT);
+      gpio_put(cs_pin, 1);  // CS high
+  }
 
-    set_bandwidth(bandwidth);
-    set_hysteresis(hysteresis);
+  set_rotation_direction(0);    // needs to be set, otherwise might be random
+  set_bandwidth(bandwidth);
+  set_hysteresis(hysteresis);
 
-    last_raw_angle = 0;
-    abs_raw_angle = 0;
+  last_raw_angle = 0;
+  abs_raw_angle = 0;
 }
 
 void MT6835Encoder::reset_abs_angle(int32_t abs_raw_angle) {
   MT6835Encoder::abs_raw_angle = abs_raw_angle;
 }
 
+void MT6835Encoder::reset_abs_angle_period() {
+  abs_raw_angle %= MT6835_CPR;
+  if (abs_raw_angle < 0)
+    abs_raw_angle += MT6835_CPR;
+}
+
 float MT6835Encoder::read_abs_angle() {
-    int32_t raw_angle = read_abs_angle_raw();
-    return raw_angle * RAW_TO_ANGLE;
+  int32_t raw_angle = read_abs_angle_raw();
+  return raw_angle * RAW_TO_ANGLE;
 }
 
 MT6835Encoder::AbsRawAngleType MT6835Encoder::read_abs_angle_raw() {
-    uint8_t data[6] = {0};
-    data[0] = MT6835_OP_ANGLE << 4;
-    data[1] = MT6835_REG_ANGLE1;
-    // rest zero
+  uint8_t data[6] = {0};
+  data[0] = MT6835_OP_ANGLE << 4;
+  data[1] = MT6835_REG_ANGLE1;
+  // rest zero
 
-    spi_begin_transaction();
-    spi_transfer(data, 6);
-    spi_end_transaction();
+  spi_begin_transaction();
+  spi_transfer(data, 6);
+  spi_end_transaction();
 
-    last_status = data[4] & 0x07;
-    last_crc = data[5];
-    int32_t raw_angle = ((int32_t)data[2] << 13) | ((int32_t)data[3] << 5) | (data[4] >> 3);
-    
-    if (check_crc) {
-        if (last_crc != calc_crc(raw_angle, last_status)) {
-            last_status |= MT6835_CRC_ERROR;
-            return -1.0f; // CRC error indicator
-        }
-    }
+  last_status = data[4] & 0x07;
+  last_crc = data[5];
+  int32_t raw_angle = ((int32_t)data[2] << 13) | ((int32_t)data[3] << 5) | (data[4] >> 3);
+  
+  if (check_crc) {
+      if (last_crc != calc_crc(raw_angle, last_status)) {
+          last_status |= MT6835_CRC_ERROR;
+          return -1.0f; // CRC error indicator
+      }
+  }
 
-    return update_abs_raw_angle(raw_angle);
+  return update_abs_raw_angle(raw_angle);
 }
 
-MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() {
+MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() const {
   return abs_raw_angle;
 }
 
+float MT6835Encoder::get_last_abs_angle() const {
+  return abs_raw_angle * RAW_TO_ANGLE;
+}
+
 int32_t MT6835Encoder::get_rawcounts_per_rev() {
   return MT6835_CPR;
 }
diff --git a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
index 4c13e35..62c6084 100644
--- a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
+++ b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
@@ -147,10 +147,11 @@ class MT6835Encoder {
 
       void init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
       void reset_abs_angle(int32_t abs_raw_angle=0);  // resets the total revolutions of abs angle
-
+      void reset_abs_angle_period();                  // Brings abs angle into [0..2pi)
       float read_abs_angle();                         // returns the absolute angle in radians
+      float get_last_abs_angle() const;               // returns the last read abs angle
       AbsRawAngleType read_abs_angle_raw();           // returns the absolute angle in raw counts
-      AbsRawAngleType get_last_abs_raw_angle();       // returns the last read abs raw angle
+      AbsRawAngleType get_last_abs_raw_angle() const; // returns the last read abs raw angle
 
       int32_t get_rawcounts_per_rev();                // returns the number of raw counts per revolution
 
diff --git a/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp b/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
index 8dc9fb5..3abe215 100644
--- a/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
+++ b/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
@@ -6,12 +6,14 @@
 // --------------------------------------------------------------------------------------
 
 #include "TB6612_motor_driver.h"
+#include "utilities/logging.h"
 
 #include <math.h>
 #include <algorithm>
 #include "hardware/pwm.h"
 #include "hardware/gpio.h"
 #include "hardware/clocks.h"
+#include "pico/time.h"
 
 // Helper rounding function
 int32_t round_int32(float val) {
@@ -33,7 +35,7 @@ TB6612MotorDriver::TB6612MotorDriver(
   pwm_freq(pwm_freq), pwm_resolution(pwm_resolution)
 {
   max_pwm = (1 << pwm_resolution) - 1;
-  amplitude = 0.1f * max_pwm;  // Default to 10% amplitude
+  set_amplitude(0.0f, false);
 }
 
 void init_output_pin(uint8_t pin, bool value) {
@@ -46,17 +48,11 @@ void TB6612MotorDriver::begin() {
   init_output_pin(pin_en_a, false);
   init_output_pin(pin_en_b, false);
 
-  // enable pwm pins, see TB6612 documentation for how the PWM pins work,
-  // for slow decay mode they are constantly enabled
+  // Enable pwm pins, see TB6612 documentation for how the PWM pins work,
+  // for slow decay mode they are constantly enabled.
   init_output_pin(pin_pwm_a, true);
   init_output_pin(pin_pwm_b, true);
 
-  // not needed since pwm pins are configured below
-  // pinMode(pin_pos_a, OUTPUT);
-  // pinMode(pin_neg_a, OUTPUT);
-  // pinMode(pin_pos_b, OUTPUT);
-  // pinMode(pin_neg_b, OUTPUT);
-
   // Read system clock dynamically
   uint32_t sys_clk = clock_get_hz(clk_sys);
   float clkdiv = (float)sys_clk / (pwm_freq * max_pwm);
@@ -76,6 +72,8 @@ void TB6612MotorDriver::begin() {
   setup_pwm_pin(pin_neg_a);
   setup_pwm_pin(pin_pos_b);
   setup_pwm_pin(pin_neg_b);
+
+  disable();
 }
 
 void TB6612MotorDriver::enable() {
@@ -90,18 +88,63 @@ void TB6612MotorDriver::disable() {
 
 void TB6612MotorDriver::set_amplitude(float amplitude, bool immediate_update) {
   amplitude = std::clamp(amplitude, 0.0f, 1.0f);
-  TB6612MotorDriver::amplitude = amplitude * max_pwm;
+  TB6612MotorDriver::amplitude = amplitude;
+  TB6612MotorDriver::amplitude_raw = amplitude * max_pwm;
   if(immediate_update)
     set_field_angle(field_angle);
 }
 
+
+void TB6612MotorDriver::set_amplitude_smooth(float amplitude, int ramp_time_ms) {
+  amplitude = std::clamp(amplitude, 0.0f, 1.0f);
+  float start = TB6612MotorDriver::amplitude;
+  int step_time = 10;
+  int steps = std::max(1, ramp_time_ms / step_time);
+
+  for (int i = 1; i <= steps; ++i) {
+    float t = float(i) / steps;
+    set_amplitude(start + t * (amplitude - start), true);
+    sleep_ms(step_time);
+  }
+}
+
+float TB6612MotorDriver::get_amplitude() const {
+  return amplitude;
+}
+
+void TB6612MotorDriver::rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step) {
+  float start = field_angle;
+  float rad_per_µs = (delta_angle >= 0.0f) ? rad_per_s*1e-6f : -rad_per_s*1e-6f;
+
+  // Determine how long the movement should take (in microseconds)
+  uint64_t duration_us = (uint64_t)(fabs(delta_angle / rad_per_s) * 1e6f);
+  uint64_t start_time_us = time_us_64();
+  while (true) {
+      uint64_t elapsed_us = time_us_64() - start_time_us;
+      if (elapsed_us >= duration_us)
+          break;
+
+      // Update the field
+      set_field_angle(start + rad_per_µs * elapsed_us);
+      
+      if(on_step)
+        on_step();
+
+      sleep_us(500);
+  }
+
+  // Ensure final angle is reached
+  set_field_angle(start + delta_angle);
+}
+
+
 void TB6612MotorDriver::set_field_angle(float angle_rad) {
   field_angle = angle_rad;
   float sin_a = sin(angle_rad);
   float cos_a = cos(angle_rad);
 
-  set_pwm(pin_pos_a, pin_neg_a, round_int32(sin_a * amplitude));
-  set_pwm(pin_pos_b, pin_neg_b, round_int32(cos_a * amplitude));
+  set_pwm(pin_pos_a, pin_neg_a, round_int32(sin_a * amplitude_raw));
+  set_pwm(pin_pos_b, pin_neg_b, round_int32(cos_a * amplitude_raw));
 }
 
 float TB6612MotorDriver::get_field_angle() {
diff --git a/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.h b/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.h
index aa1104c..68d245b 100644
--- a/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.h
+++ b/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.h
@@ -8,6 +8,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <functional>
 // #include <Arduino.h>
 
 class TB6612MotorDriver {
@@ -27,6 +28,12 @@ class TB6612MotorDriver {
     void  set_field_angle(float angle_rad);
     float get_field_angle();
     void  set_amplitude(float amplitude, bool immediate_update);  // Input in range 0.0–1.0
+    void  set_amplitude_smooth(float amplitude, int ramp_time_ms);
+    float get_amplitude() const;
+
+    // Rotate the magnetic field by the given angle delta.
+    // For longer moves, use on_step callback to update encoders.
+    void  rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step);
 
   private:
     void  set_pwm(uint8_t ch_pos, uint8_t ch_neg, int32_t value);
@@ -39,6 +46,8 @@ class TB6612MotorDriver {
     uint16_t pwm_freq;
     uint8_t pwm_resolution;
     uint16_t max_pwm;
-    float amplitude;  // scaled to 0–max_pwm
+    float amplitude_raw=0.0f;  // scaled to 0–max_pwm
+    float amplitude=0.0f;      // scaled to 0–1
     float field_angle;
 };
+
diff --git a/firmware/MotionControllerRP/src/hw_config.h b/firmware/MotionControllerRP/src/hw_config.h
index 181d967..d296c82 100644
--- a/firmware/MotionControllerRP/src/hw_config.h
+++ b/firmware/MotionControllerRP/src/hw_config.h
@@ -5,39 +5,16 @@
 #define MOTOR2_POLE_PAIRS 100
 #define MOTOR3_POLE_PAIRS 100
 
-// #define SINGLE_AXIS_BOARD
-#ifdef SINGLE_AXIS_BOARD
-  // Single Axis Board
-  #define PIN_BUILTIN_LED 16
-  #define PIN_USER_BUTTON 24
+#define CALIBRATION_RANGE 95        // degrees from home position
+#define CALIBRATION_FIELD_VELOCITY 40.0f
 
-  #define PIN_M1_PWM_A_POS  13
-  #define PIN_M1_PWM_A_NEG  12
-  #define PIN_M1_PWM_B_POS  14
-  #define PIN_M1_PWM_B_NEG  15
-
-  #define PIN_M2_PWM_A_POS  9
-  #define PIN_M2_PWM_A_NEG  8
-  #define PIN_M2_PWM_B_POS  10
-  #define PIN_M2_PWM_B_NEG  11
-
-  #define PIN_M3_PWM_A_POS  5
-  #define PIN_M3_PWM_A_NEG  4
-  #define PIN_M3_PWM_B_POS  6
-  #define PIN_M3_PWM_B_NEG  7
+#define HOMING_VELOCITY   1.0f        // rad per s
+#define HOMING_CURRENT    0.15f       // range 0..1
+#define HOMING_FINISH_POS 0.5f        // in rad
 
 
-  #define PIN_MOTOR_EN      18
-  #define PIN_MOTOR_PWMAB   19
-
-  #define PIN_ENCODER1_CS 20
-  #define PIN_ENCODER2_CS 21
-  #define PIN_ENCODER3_CS 22
-  #define PIN_ENCODER_SCK 2
-  #define PIN_ENCODER_MISO 0
-  #define PIN_ENCODER_MOSI 3
-
-#else
+#define SINGLE_AXIS_BOARD
+#ifndef SINGLE_AXIS_BOARD
   // Pins for 3Axis Board
   #define PIN_BUILTIN_LED 23
   #define PIN_USER_BUTTON 24
@@ -68,6 +45,36 @@
   #define PIN_ENCODER_MISO 0
   #define PIN_ENCODER_MOSI 3
 
+#else
+  // Single Axis Board
+  #define PIN_BUILTIN_LED 16
+  #define PIN_USER_BUTTON 24
+
+  #define PIN_M1_PWM_A_POS  13
+  #define PIN_M1_PWM_A_NEG  12
+  #define PIN_M1_PWM_B_POS  14
+  #define PIN_M1_PWM_B_NEG  15
+
+  #define PIN_M2_PWM_A_POS  9
+  #define PIN_M2_PWM_A_NEG  8
+  #define PIN_M2_PWM_B_POS  10
+  #define PIN_M2_PWM_B_NEG  11
+
+  #define PIN_M3_PWM_A_POS  5
+  #define PIN_M3_PWM_A_NEG  4
+  #define PIN_M3_PWM_B_POS  6
+  #define PIN_M3_PWM_B_NEG  7
+
+
+  #define PIN_MOTOR_EN      18
+  #define PIN_MOTOR_PWMAB   19
+
+  #define PIN_ENCODER1_CS 20
+  #define PIN_ENCODER2_CS 21
+  #define PIN_ENCODER3_CS 22
+  #define PIN_ENCODER_SCK 2
+  #define PIN_ENCODER_MISO 0
+  #define PIN_ENCODER_MOSI 3
 #endif
 
 // test setup
diff --git a/firmware/MotionControllerRP/src/kinemtaic_models/kinematic_model_delta3d.cpp b/firmware/MotionControllerRP/src/kinemtaic_models/kinematic_model_delta3d.cpp
index 85e4fe9..4e6c57b 100644
--- a/firmware/MotionControllerRP/src/kinemtaic_models/kinematic_model_delta3d.cpp
+++ b/firmware/MotionControllerRP/src/kinemtaic_models/kinematic_model_delta3d.cpp
@@ -109,10 +109,6 @@ bool KinematicModel_Delta3D::inverse(const Pose6DF& pose, float* joint_positions
     joint_positions[i] = rotor_angle_offset[i] + angle;
   }
 
- // joint_positions[0] = pose.translation.x;
- // joint_positions[1] = pose.translation.y;
- // joint_positions[2] = pose.translation.z;
-
   return true;
 }
 
diff --git a/firmware/MotionControllerRP/src/main.cpp b/firmware/MotionControllerRP/src/main.cpp
index e618a9a..ef10bad 100644
--- a/firmware/MotionControllerRP/src/main.cpp
+++ b/firmware/MotionControllerRP/src/main.cpp
@@ -19,8 +19,9 @@
 #include "utilities/logging.h"
 #include "utilities/frequency_counter.h"
 #include "kinemtaic_models/kinematic_model_delta3d.h"
-
+#include "version.h"
 #include "hw_config.h"
+#include "LittleFS.h"
 
 //*** GLOBALS ***************************************************************************
 
@@ -67,8 +68,9 @@ void overclock() {
 }
 
 void set_led_color(uint8_t r, uint8_t g, uint8_t b) {
+  return;
   strip.neoPixelSetValue(0, r, g, b, false);
-  delayMicroseconds(1000);
+  delayMicroseconds(2000);
   strip.neoPixelShow();
 }
 
@@ -92,7 +94,7 @@ void main_core0() {
 }
 
 void main_core1() {
-  LOG_INFO("starting servo controll loops on core 1...");
+  LOG_INFO("Starting servo controll loops on core 1...");
 
   uint64_t last_time = time_us_64();
   while(true) {
@@ -101,12 +103,16 @@ void main_core1() {
     float dt = float(time_us - last_time)*1e-6f;
     last_time = time_us;
 
+    // limit time delta
+    dt = std::min(dt, 0.0001f);
+
+    // update servo loops
     robot.update_servo_controllers(dt);
   }
 }
 
 void setup() {
-  led_blink(0, 20, 0, 1, 4000/3);
+  led_blink(0, 0, 30, 3, 100);
   // stdio_init_all();  // Initializes USB or UART stdio
   overclock();
   // Serial.begin(921600);
@@ -117,18 +123,29 @@ void setup() {
   // auto* test = new KinematicModel_Delta3D(); test->test(); delete test;
 
   delay(100);  // Allow time for serial monitor to connect
-  Serial.printf("System clock: %i Mhz\n", int32_t(clock_get_hz(clk_sys))/1000/1000);
+  
+  LOG_INFO("Open Micro Stage Firmware: %s", FIRMWARE_VERSION);
+  LOG_INFO("System clock: %i Mhz", int32_t(clock_get_hz(clk_sys))/1000/1000);
 
-  LOG_INFO("initializing robot...");
+  // LittleFS.format();
+  if (!LittleFS.begin()) {
+    LOG_ERROR("Mounting filesystem failed");
+  } else {
+    FSInfo fs_info;
+    LittleFS.info(fs_info);
+    LOG_INFO("Mounting filesystem successfully [%i/%i bytes used]", 
+             (int)fs_info.usedBytes, (int)fs_info.totalBytes);
+  }
+
+  LOG_INFO("Initializing device...");
   robot.init();
 
-  LOG_INFO("homing axes...");
-  robot.home();
-
   multicore_launch_core1(&main_core1);
+  sleep_ms(100);
   
-  set_led_color(0, 10, 0);
-  LOG_INFO("initialization finished...");
+  set_led_color(0, 20, 0);
+  LOG_INFO("Initialization finished");
+  LOG_INFO(" ");
 
   return;
 
@@ -140,193 +157,8 @@ void setup() {
   encoder.init();
   encoder.set_hysteresis(0x4); //0x6);
   */
- /* pinMode(PIN_USER_BUTTON, INPUT_PULLUP);
-
-  // init encoders
-  MT6835Encoder::setup_spi(spi0, PIN_ENCODER_SCK, PIN_ENCODER_MOSI, PIN_ENCODER_MISO, 8000000);
-  encoder1.init(0x5, 0x4);
-  encoder2.init(0x5, 0x4);
-  encoder3.init(0x5, 0x4);
-
-  servo_controller1.init(0.5);
-  servo_controller2.init(0.5);
-  servo_controller3.init(0.5);
-
-  delay(1000); 
-
-  set_led_color(50, 10, 0);
-
-  servo_controller1.home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
-  calibrate_actuator(servo_controller1);
-
-  servo_controller2.home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
-  calibrate_actuator(servo_controller2);
-
-  servo_controller3.home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
-  calibrate_actuator(servo_controller3);
-
-  set_led_color(0, 10, 0);
-  */
 }
 
 void loop() {
   main_core0();
-}
-
-/*
-int it=0;
-float target_angle1 = 50.0f/180.0f*PI;
-float target_angle2 = 50.0f/180.0f*PI;
-float target_angle3 = 50.0f/180.0f*PI;
-int k=0;
-uint64_t last_time = time_us_64();
-uint64_t last_print_time = time_us_64();
-
-
-void loop_encoder_test() {
-  float encoder_angle = encoder3.read_abs_angle();
-  Serial.printf(">angle: %f\n", encoder_angle*360/TWO_PI);
-  delay(10);
-}
-
-void loop_motor_test() {
-  uint64_t time_us = time_us_64();
-  //target_angle = (30.0f+0.07f*((time_us>>19)%2))*DEG_TO_RAD;
-  float target_angle = (1000.05f*sin(float(time_us)*3e-5f))/180*PI;
-  motor_driver1.set_field_angle(target_angle);
-
-  target_angle = (1000.05f*sin(float(time_us)*4e-5f))/180*PI;
-  motor_driver2.set_field_angle(target_angle);
-
-  target_angle = (1000.05f*sin(float(time_us)*5e-5f))/180*PI;
-  motor_driver3.set_field_angle(target_angle);
-}
-
-void loop_old() {
- // return;
- //loop_motor_test(); return;
- // loop_encoder_test(); return;
-
-  // get time and detla time
-  uint64_t time_us = time_us_64();
-  float dt = float(time_us - last_time)*1e-6f;
-  float one_over_dt = 1.0f/dt;
-  last_time = time_us;
-
-  servo_controller1.update(shared_data.joint_positions[0], dt, one_over_dt);
-  servo_controller2.update(shared_data.joint_positions[1], dt, one_over_dt);
-  servo_controller3.update(shared_data.joint_positions[2], dt, one_over_dt);
-  loop_freq_counter.update(dt);
-
-  // motor_servo_update(dt_ms);
-
-  
-  // print info
-  if(time_us-last_print_time > 10000 && true) {
-    //Serial.printf(">angle: %f\n", encoder_angle*360/TWO_PI);
-    //Serial.printf(">field: %f\n", field_angle*360/TWO_PI);
-    Serial.printf(">pos_error [µrad]: %f\n", servo_controller3.get_position_error()*1e6f);
-    Serial.printf(">output [deg]: %f\n", servo_controller3.output*float(RAD_TO_DEG));
-    Serial.printf(">motor_pos [deg]: %f\n", servo_controller3.get_position()*float(RAD_TO_DEG));
-    //Serial.printf(">motor_pos: %f µm\n", servo_controller.get_position()*15.0e6f);
-    //Serial.printf(">update_khz: %f\n", float(loop_freq_counter.get())*0.001);
-    
-    //Serial.printf(">e: %f\n",e*360/TWO_PI);
-
-    last_print_time = time_us;
-  }
-
-  //target_angle2 = (50.0f+0.012f*((time_us>>20)%2))*DEG_TO_RAD;
-  //target_angle3 = (50.0f+0.012f*(1-(time_us>>20)%2))*DEG_TO_RAD;
-
-  //target_angle2 = (50.0f+0.0002f*sin(float(time_us)*5e-6f))/180*PI;
-  //target_angle3 = (50.0f+0.0002f*cos(float(time_us)*5e-6f))/180*PI;
-  
-  // target_angle1 = (50.0f+20.001f*trapezoidal_wave(float(time_us)*5e-6f-PI*0.33f))/180*PI;
-  // target_angle2 = (50.0f+20.001f*trapezoidal_wave(float(time_us)*5e-6f))/180*PI;
-  // target_angle3 = (50.0f+20.001f*trapezoidal_wave(float(time_us)*5e-6f+PI*0.33f))/180*PI;
-  
-  //target_angle = (50.0f+20.005f*triangle_wave(float(time_us)*1.0e-5f))/180*PI;
-  
-
-  return;
-/*
-  it++;
-
-  float ki = 0.0f;
-  if(k<5000 || digitalRead(PIN_USER_BUTTON) == 0)
-    ki = 0.3f;
-  k++;
-
-  // test
-  float max_integral = 0.15f;
-  float e = (target_angle - encoder_angle);
-  //float gain_scale = e<0.25f/360*TWO_PI ? 3.0 : 1.0f;
-  //float d = (e-prev_e) * 10.0f;
-  //d_filtered = d_filtered*0.5f + d*0.5f;
-
-  float velocity_setpoint = 0.2f * e;
-  float velocity = encoder_angle-prev_angle;
-  float velocity_error = velocity_setpoint - velocity;
-
-  integral += std::clamp(1.25f * velocity_error, -max_integral, max_integral);  // velocity PI
-
- // float ig = max(min(e*ki, max_integral), -max_integral);
- // integral += ig - v*0.4f;
-
-  out = integral;// + std::clamp(e*0.0f, -3.415926f*0.05f, 3.415926f*0.05f);// - d_filtered;
-  //float p = max(min(e*3, 1), -1);
-  motor.set_field_angle(target_angle*e2m_scale+out);
-  //motor.set_field_angle(out);
-  prev_e = e;
-  prev_angle = encoder_angle;
-
-  if(it>100) {
-    // Serial.print(rawAngle);
-    Serial.print(">angle: ");
-  //  float angleDegrees = angle_tracker. * 360.0f / 16384.0f;
-    Serial.println(encoder_angle*360/TWO_PI, 9);
-    Serial.print(">out: ");
-    Serial.println(out, 9);
-
-    Serial.printf(">e: %f\n",e*360/TWO_PI);
-   // Serial.printf(">v: %f\n",velocity);
-
-    if(fabs(target_angle-encoder_angle)*360/TWO_PI > 0.05)
-        strip.neoPixelSetValue(0, 10, 0, 0, false);
-    else
-        strip.neoPixelSetValue(0, 0, 10, 0, false);
-
-    delayMicroseconds(10);
-    strip.neoPixelShow();
-  }
-
- // target_angle = (60.0f+20.00f*((k/1000)%2))/180*PI;
-  //  target_angle = (60.0f+0.1f*sin(float(k)*0.01))/180*PI;
- // k++;
-
- // int32_t output = pos_controller.compute(pos_controller.to_fixpoint(target_angle), 
- //                                         pos_controller.to_fixpoint(angle));
-
- // a = pos_controller.from_fixpoint(output);
-  //a += 0.001f;
- // motor.set_field_angle(a);
-
-  // float ma = (angle)*e2m_scale + e2m_offset;
-  // motor.set_field_angle(ma);
-
-/*  static uint16_t hue = 0; // 0-255 for full RGB cycle
-  uint8_t r, g, b;
-  //hsv2rgb(hue, 255, 1, r, g, b); // 50 = brightness (0-255)
-  r = 0; g=0; b=0;
-  g = raw_angle%2 == 0 ? 10 : 0;
-
-  // Set the single pixel to the current color
-  strip.neoPixelSetValue(0, r, g, b, false);
-  strip.neoPixelShow();
-
-  hue = (hue + 1) % 256; // Adjust increment for speed
-  delay(20);             // Adjust delay for smoothness
-  */
- //  strip.neoPixelSetValue(0, 2, 190, 3, false);
-// }
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/main.h b/firmware/MotionControllerRP/src/main.h
index 162ddf8..8b13789 100644
--- a/firmware/MotionControllerRP/src/main.h
+++ b/firmware/MotionControllerRP/src/main.h
@@ -1,6 +1 @@
-// --------------------------------------------------------------------------------------
-// Project: MicroManipulatorStepper
-// License: MIT (see LICENSE file for full description)
-//          All text in here must be included in any redistribution.
-// Author:  M. S. (diffraction limited)
-// --------------------------------------------------------------------------------------
+
diff --git a/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp b/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
index 80934fa..d9ebfa5 100644
--- a/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
+++ b/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
@@ -22,16 +22,17 @@ bool MotionController::update(float dt, float* joint_positions, float* joint_vel
   // increment time counter
   current_time += dt;
 
-  // check if end of current path segment exceeded and if so, fetch next one
+  // check if end of current path segments end time is exceeded and if so, fetch next one
+  bool queue_empty = false;
   float segment_duration = current_path_segment.get_duration();
   while(current_time > segment_duration) {
     // get next path segment from queue
-    bool queue_empty = !path_planner->pop_js_path_segment(current_path_segment);
+    queue_empty = !path_planner->pop_js_path_segment(current_path_segment);
     if(queue_empty) {
       current_time = segment_duration;
       break;
     }
-    
+        
     // update current time and segment duration
     current_time -= segment_duration;
     segment_duration = current_path_segment.get_duration();
@@ -40,6 +41,13 @@ bool MotionController::update(float dt, float* joint_positions, float* joint_vel
   if(!current_path_segment.is_initialized())
     return false;
 
+  // if queue_empty is true the current path segment is finished AND no more
+  // pending segments are in the queue. In that case the current path segment
+  // is disabled for the upcoming iterations. However the current iteration 
+  // will still provide the exact end position to the caller and return true.
+  if(queue_empty) 
+    current_path_segment.initialized = false;
+
   // evaluate path segment
   current_path_segment.evaluate(current_time, joint_positions, joint_velocities);
   return true;
diff --git a/firmware/MotionControllerRP/src/motion_control/motion_controller.h b/firmware/MotionControllerRP/src/motion_control/motion_controller.h
index d86643e..263c55c 100644
--- a/firmware/MotionControllerRP/src/motion_control/motion_controller.h
+++ b/firmware/MotionControllerRP/src/motion_control/motion_controller.h
@@ -30,4 +30,5 @@ class MotionController {
 
     float current_time;
     JointSpacePathSegment current_path_segment;
+    bool current_segment_finished;
 };
diff --git a/firmware/MotionControllerRP/src/robot.cpp b/firmware/MotionControllerRP/src/robot.cpp
index 852dbd7..2bc6075 100644
--- a/firmware/MotionControllerRP/src/robot.cpp
+++ b/firmware/MotionControllerRP/src/robot.cpp
@@ -5,10 +5,19 @@
 // Author:  M. S. (diffraction limited)
 // --------------------------------------------------------------------------------------
 
+#include <LittleFS.h> 
 #include "robot.h"
 #include "hw_config.h"
 #include "utilities/logging.h"
+#include "utilities/utilities.h"
 #include "kinemtaic_models/kinematic_model_delta3d.h"
+#include "servo_control/homing_controller.h"
+#include "servo_control/actuator_calibration.h"
+#include "pico/multicore.h"
+#include "version.h"
+
+constexpr int SPINLOCK_ID_SHARED_DATA = 0;
+constexpr int SPINLOCK_ID_JOINTS = 1;
 
 //*** FUNCTION **************************************************************************
 
@@ -22,8 +31,8 @@ bool startswith(const std::string& str, const std::string& prefix) {
 //--- RobotAxis -------------------------------------------------------------------------
 
 RobotJoint::RobotJoint(MT6835Encoder* encoder, 
-                     TB6612MotorDriver* motor_driver,
-                     int pole_pairs) 
+                       TB6612MotorDriver* motor_driver,
+                       int pole_pairs) 
 {
   RobotJoint::encoder = encoder;
   RobotJoint::motor_driver = motor_driver;
@@ -41,33 +50,47 @@ RobotJoint::~RobotJoint() {
   encoder = nullptr;
 }
 
-void RobotJoint::init() {
+void RobotJoint::init(int joint_idx) {
+  RobotJoint::joint_idx = joint_idx;
+
   encoder->init(0x5, 0x4);
   servo_controller->init(0.5);
+  servo_controller->set_motor_enabled(false, false);
 }
 
-void RobotJoint::home() {
-  servo_controller->home(-1.0f, 100.0f*DEG_TO_RAD, 0.1f);
-  position = servo_controller->get_position();
-  velocity = 0.0f;
-}
+bool RobotJoint::calibrate() {
+  LOG_INFO("Joint-%i: calibrating joint...", joint_idx);
 
-void RobotJoint::calibrate() {
-  LookupTable lut;
-  build_motor_to_enc_angle_lut(lut, *servo_controller, 1.0f*DEG_TO_RAD, 92.0f*DEG_TO_RAD, 256);
-  // lut.print_to_log();
-  LOG_DEBUG("Inverting lookup table...");
-  bool ok = lut.invert(256);
-  if(!ok) {
-    servo_controller->get_motor_driver().disable();
-    lut.print_to_log();
-    while(true);
+  HomingController homing_controller;
+  bool homing_ok = homing_controller.run_blocking(servo_controller, -HOMING_VELOCITY, 
+                                                  360.0f*DEG_TO_RAD, HOMING_CURRENT);
+  if(homing_ok == false) {
+    LOG_ERROR("Joint-%i: Calibration failed due to unsuccessful homing sequence", joint_idx);
+    return false;
   }
-  LOG_DEBUG(">finished");
-  // lut.print_to_log();
-  delay(200);
 
-  servo_controller->set_encoder_lut(lut);
+  // measure lookup tables
+  LookupTable encoder_raw_to_motor_pos_lut;
+  LookupTable motor_pos_to_field_angle_lut;
+  bool ok = measure_calibration_data(encoder_raw_to_motor_pos_lut, 
+                                     motor_pos_to_field_angle_lut, 
+                                     *servo_controller, 
+                                     CALIBRATION_RANGE*DEG_TO_RAD,
+                                     CALIBRATION_FIELD_VELOCITY, 
+                                     256);
+  if(!ok) {
+    LOG_ERROR("Joint-%i: calibrating failed", joint_idx);
+    return false;
+  }
+
+  servo_controller->set_enc_to_pos_lut(encoder_raw_to_motor_pos_lut);
+  servo_controller->set_pos_to_field_lut(motor_pos_to_field_angle_lut);
+  is_calibrated = true;
+  is_homed = true;
+
+  LOG_INFO("Joint-%i: calibrating joint successful.", joint_idx);
+
+  return true;
 }
 
 void RobotJoint::update(float dt, float one_over_dt) {
@@ -79,18 +102,61 @@ void RobotJoint::update_target(float p, float v) {
   velocity = v;
 }
 
+bool RobotJoint::load_calibration() {
+  std::string fn1 = calib_data_filename("enc_to_pos_lut").c_str();
+  std::string fn2 = calib_data_filename("pos_to_field_lut").c_str();
+  if(!LittleFS.exists(fn1.c_str()) || !LittleFS.exists(fn2.c_str())) {
+    LOG_WARNING("Joint-%i: Not all calibration files found. Run joint calibration with M56.", joint_idx);
+    return false;
+  }
+
+  LookupTable enc_to_pos_lut;
+  LookupTable pos_to_field_lut;
+  bool res = true; 
+  res &= load_lut_from_file(enc_to_pos_lut, fn1.c_str());
+  res &= load_lut_from_file(pos_to_field_lut, fn2.c_str());
+  if(res == false)
+    return false;
+
+  servo_controller->set_enc_to_pos_lut(enc_to_pos_lut);
+  servo_controller->set_pos_to_field_lut(pos_to_field_lut);
+
+  is_calibrated = true;
+  LOG_INFO("Joint-%i: Encoder lookup tables loaded (size=%i,%i)", 
+           joint_idx, enc_to_pos_lut.size(), pos_to_field_lut.size());
+
+  return true;
+}
+
+bool RobotJoint::store_calibration() {
+  bool res = true;
+
+  res &= save_lut_to_file(servo_controller->get_enc_to_pos_lut(), 
+                          calib_data_filename("enc_to_pos_lut").c_str());
+  res &= save_lut_to_file(servo_controller->get_pos_to_field_lut(), 
+                          calib_data_filename("pos_to_field_lut").c_str());
+
+  return res;
+}
+
+std::string RobotJoint::calib_data_filename(std::string data_name) const {
+  return std::string("joint")+std::to_string(joint_idx)+"_"+data_name+".dat";
+}
+
 //--- Robot -----------------------------------------------------------------------------
 
 Robot::Robot(float path_segment_time_step) : 
   path_planner(nullptr, path_segment_time_step), 
   motion_controller(&path_planner),
   servo_loop_frequency_counter(10000),
-  motion_controller_frequency_counter(1000)
+  motion_controller_frequency_counter(1000),
+  shared_data(SPINLOCK_ID_SHARED_DATA),
+  joints_spin_lock(spin_lock_instance(SPINLOCK_ID_JOINTS))
 {
   kinematic_model = new KinematicModel_Delta3D();
   path_planner.set_kinematic_model(kinematic_model);
 
-  for(int i=0; i<3; i++)
+  for(int i=0; i<NUM_JOINTS; i++)
     joints[i] = nullptr;
 
   command_parser.set_command_processor(this);
@@ -106,7 +172,7 @@ Robot::~Robot() {
   if(kinematic_model != nullptr)
     delete kinematic_model;
 
-  for(int i=0; i<3; i++) {
+  for(int i=0; i<NUM_JOINTS; i++) {
     if(joints[i] != nullptr)
       delete joints[i];
     joints[i] = nullptr;
@@ -147,8 +213,9 @@ void Robot::init() {
   }
 
   // initialize axes
-  for(int i=0; i<3; i++) {
-    joints[i]->init();
+  for(int i=0; i<NUM_JOINTS; i++) {
+    joints[i]->init(i);
+    joints[i]->load_calibration();
   }
 
   // setup timer for updating the motion controller (which evaluates joint space path
@@ -160,26 +227,6 @@ void Robot::init() {
                          &motion_controller_update_timer);
 }
 
-void Robot::calibrate() {
-  for(int i=0; i<3; i++) {
-    joints[i]->calibrate();
-  }
-}
-
-void Robot::home() {
-  for(int i=0; i<3; i++) {
-    joints[i]->home();
-    joints[i]->calibrate();
-
-    // set start angle
-    float start_angle = 20*Constants::DEG2RAD;
-    joints[i]->servo_controller->move_to_open_loop(start_angle, 1.0f);
-    if (spin_try_lock_unsafe(shared_data.lock)) {
-      shared_data.joint_positions[i] = start_angle;
-      spin_unlock_unsafe(shared_data.lock);
-    }
-  }
-}
 
 void Robot::update_command_parser() {
   // process serial input
@@ -198,6 +245,7 @@ void Robot::update_command_parser() {
   //LOG_INFO(">pos_error [µrad]: %f\n", pos_error*1e6);
   //Serial.printf(">pos_error [µrad]: %f\n", pos_error*1e6);#
   //LOG_INFO(">pos_x [mm]: %f", joints[1]->position);
+  //update_servo_controllers(0.01f);
 }
 
 /**
@@ -252,8 +300,8 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
   // Attempt to acquire spinlock non-blocking and set new target data for the servo loops
   if (update_ok && spin_try_lock_unsafe(robot->shared_data.lock)) {
     for (int i = 0; i < NUM_JOINTS; i++) {
-      robot->shared_data.joint_positions[i] = joint_positions[i];
-      robot->shared_data.joint_velocities[i] = joint_velocities[i];
+      robot->shared_data.joint_target_positions[i] = joint_positions[i];
+      robot->shared_data.joint_target_velocities[i] = joint_velocities[i];
     }
     spin_unlock_unsafe(robot->shared_data.lock);
   }
@@ -272,19 +320,193 @@ void Robot::update_servo_controllers(float dt) {
 
   // update axis target position and velocity from shared data
   spin_lock_unsafe_blocking(shared_data.lock);
-  for(int i=0; i<3; i++)
-    joints[i]->update_target(shared_data.joint_positions[i], shared_data.joint_velocities[i]);
+  for(int i=0; i<3; i++) {
+    joints[i]->update_target(shared_data.joint_target_positions[i], 
+                             shared_data.joint_target_velocities[i]);
+  }
   spin_unlock_unsafe(shared_data.lock);
 
   // update servo loop for each axis
-  for(int i=0; i<3; i++) {
+  spin_lock_unsafe_blocking(joints_spin_lock);
+  for(int i=0; i<NUM_JOINTS; i++) {
     joints[i]->update(dt, one_over_dt);
   }
+  spin_unlock_unsafe(joints_spin_lock);
 
   // update frequency counter
   servo_loop_frequency_counter.update(dt);
 }
 
+void Robot::enable_servo_control(bool enable) {
+  // LOG_DEBUG(enable ? "Enable servo control" : "Disable servo sontrol");
+
+  // update servo loop for each axis
+  spin_lock_unsafe_blocking(joints_spin_lock);
+
+  for(int i=0; i<NUM_JOINTS; i++) {
+    bool en = joints[i]->is_homed && joints[i]->is_calibrated && enable;
+    LOG_DEBUG(en ? "Joint-%i: servo control enabled" : "Joint-%i: servo control disabled", i);
+    joints[i]->servo_controller->set_motor_update_enabled(en);
+  }
+
+  spin_unlock_unsafe(joints_spin_lock);
+}
+
+void Robot::set_pose(const Pose6DF& pose) {
+  // run inverse kinematic and compute joint positions
+  float joint_positions[NUM_JOINTS];
+  kinematic_model->inverse(pose, joint_positions);
+
+  while(true) {
+    // Attempt to acquire spinlock non-blocking and set new target data for the servo loops
+    if (spin_try_lock_unsafe(shared_data.lock)) {
+      for (int i = 0; i < NUM_JOINTS; i++) {
+        shared_data.joint_target_positions[i] = joint_positions[i];
+        shared_data.joint_target_velocities[i] = 0.0f;
+        // LOG_DEBUG("Joint-%i: set pose -> angle %f", i, joint_positions[i]);
+      }
+      spin_unlock_unsafe(shared_data.lock);
+      break;
+    }
+  }
+
+  current_pose = pose;
+}
+
+Pose6DF Robot::pose_from_joint_angles() {
+  // read joint positions from encoders
+  float joint_pos[NUM_JOINTS];
+
+  spin_lock_unsafe_blocking(joints_spin_lock);
+  for (int i = 0; i < NUM_JOINTS; i++) {
+    joint_pos[i] = joints[i]->servo_controller->read_position(); 
+  }
+  spin_unlock_unsafe(joints_spin_lock);
+
+  // run foreward kinematic model to retrieve pose from joint positions
+  Pose6DF pose;
+  bool ok = kinematic_model->foreward(joint_pos, pose);
+  if(ok == false)
+    LOG_ERROR("Foreward kinematic failed");
+
+  return pose;
+}
+
+bool Robot::check_all_joints_ready() {
+  bool all_ready = true;
+  for(int i=0; i<NUM_JOINTS; i++) {
+    all_ready &= joints[i]->is_calibrated && joints[i]->is_homed;
+  }
+
+  return all_ready;
+}
+
+bool Robot::home(uint8_t joint_mask) {
+  HomingController homing_controller[NUM_JOINTS];
+  LOG_INFO("homing...");
+  enable_servo_control(false);
+
+  // prevent servo loop updates from running during homing
+  spin_lock_unsafe_blocking(joints_spin_lock);
+
+  // initialize homing controllers
+  for(int i=0; i<NUM_JOINTS; i++) {
+    // only start requested joints
+    if(((joint_mask>>i)&1) == 0) continue;
+    LOG_DEBUG("start homing axis %i", i);
+    homing_controller[i].start(joints[i]->servo_controller, 
+                               -HOMING_VELOCITY, 360.0f*DEG_TO_RAD, HOMING_CURRENT);
+  }
+
+  // run homing controllers
+  bool all_finished = false;
+  while(all_finished == false) {
+    all_finished = true;
+    for(int i=0; i<NUM_JOINTS; i++) {
+      // only update requested joints
+      if(((joint_mask>>i)&1) == 0) continue;
+
+      // uddate
+      homing_controller[i].update();
+      all_finished &= homing_controller[i].is_finished();  
+    }
+  }
+
+  // finalize homing controllers
+  bool homing_successful = true;
+  for(int i=0; i<NUM_JOINTS; i++) {
+    // only check requested joints
+    if(((joint_mask>>i)&1) == 0) continue;
+
+    homing_controller[i].finalize();
+
+    if(homing_controller[i].is_successful()) {
+      joints[i]->is_homed = true;
+    } else {
+      LOG_ERROR("homing joint %i failed", i);
+      homing_successful = false;
+    }
+
+    // set joint angles
+    spin_lock_unsafe_blocking(shared_data.lock);
+    shared_data.joint_target_positions[i] = joints[i]->servo_controller->read_position();
+    spin_unlock_unsafe(shared_data.lock);
+  }
+
+  // servo updates may continue here
+  spin_unlock_unsafe(joints_spin_lock);
+
+  // get pose from joint angles
+  set_pose(pose_from_joint_angles());
+
+  // enable servo loops if all joints are initialized
+  enable_servo_control(true);
+
+  // check if all joints are ready
+  all_joints_ready = check_all_joints_ready();
+
+  return homing_successful;
+}
+
+bool Robot::calibrate_joint(int joint_idx, bool store_calibration) {
+  if(joint_idx<0 || joint_idx >= NUM_JOINTS)
+    return false;
+
+  RobotJoint* joint = joints[joint_idx];
+
+  // prevent servo loop updates from running during homing
+  enable_servo_control(false);
+  spin_lock_unsafe_blocking(joints_spin_lock);
+
+  bool calibration_ok = joint->calibrate();
+  if(!calibration_ok) {
+    spin_unlock_unsafe(joints_spin_lock);
+    return false;
+  }
+
+  // joint->servo_controller->move_to_open_loop(0.05f, 1.0);
+  shared_data.joint_target_positions[joint_idx] = 0; // joint->servo_controller->read_position();
+
+  if(store_calibration)
+    joint->store_calibration();
+
+  // servo updates may continue here
+  spin_unlock_unsafe(joints_spin_lock);
+
+  // recover pose from joint angles
+  set_pose(pose_from_joint_angles());
+
+  // enable servo loops if all joints are initialized
+  enable_servo_control(true);
+
+  // check if all joints are ready
+  all_joints_ready = check_all_joints_ready();
+
+  return true;
+}
+
+//--- G-Code Commands -------------------------------------------------------------------
+
 bool Robot::can_process_command(const GCodeCommand& cmd) {
   if(cmd.get_command() == "G0" || 
      cmd.get_command() == "G4")
@@ -303,15 +525,137 @@ void Robot::process_command(const GCodeCommand& cmd, std::string& reply) {
   if(cmd.get_command() == "G0") process_motion_command(cmd, reply);
   else if(cmd.get_command() == "G1") process_motion_command(cmd, reply);
   else if(cmd.get_command() == "G4") process_dwell_command(cmd, reply);
+  else if(cmd.get_command() == "G24") process_set_pose_command(cmd, reply);
+  else if(cmd.get_command() == "G28") process_home_command(cmd, reply);
   else if(startswith(cmd.get_command(), "M")) process_machine_command(cmd, reply);
   else reply="error: unknown command\n";
 }
 
+void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
+  reply = "";
+
+   // enable motors
+  if(cmd.get_command() == "M17") {
+    // read current pose from HW and set it as current pose
+    set_pose(pose_from_joint_angles());
+
+    // enable motors
+    spin_lock_unsafe_blocking(joints_spin_lock);
+    for(int i=0; i<NUM_JOINTS; i++) {
+      joints[i]->servo_controller->set_motor_enabled(true, true);
+    }
+    spin_unlock_unsafe(joints_spin_lock);
+
+    reply = "ok\n";
+  }
+
+  // disable motors
+  if(cmd.get_command() == "M18") {     
+    spin_lock_unsafe_blocking(joints_spin_lock);
+    for(int i=0; i<NUM_JOINTS; i++)
+      joints[i]->servo_controller->set_motor_enabled(false, false);
+    spin_unlock_unsafe(joints_spin_lock);
+    
+    reply = "ok\n";
+  }
+
+  // get current internal position (not using encoders to read physical position)
+  if(cmd.get_command() == "M50") {
+    reply += std::string("X") + std::to_string(current_pose.translation.x);
+    reply += std::string(" Y") + std::to_string(current_pose.translation.y);
+    reply += std::string(" Z") + std::to_string(current_pose.translation.z);
+    reply += "\nok\n";
+  }
+
+    // get current internal position (not using encoders to read physical position)
+  if(cmd.get_command() == "M51") {
+    for(int i=0; i<NUM_JOINTS; i++) {
+      float raw_angle = joints[i]->encoder->get_last_abs_raw_angle();
+      float angle = joints[i]->encoder->get_last_abs_angle()*Constants::RAD2DEG;
+      reply += std::string("Joint ")+std::to_string(i)+":  " + 
+               std::to_string(angle) + " deg  (raw="+std::to_string(raw_angle)+")\n";
+    }
+    reply += "ok\n";
+  }
+
+  // get planner queue size
+  if(cmd.get_command() == "M52") {
+    int s = path_planner.input_queue_size();
+    reply += std::string("Queue Size: ") + std::to_string(s) + "\n";
+    reply += "ok\n";
+  }
+
+  // check if all planned motions are finished executing
+  if(cmd.get_command() == "M53") {
+    bool f = path_planner.all_finished();
+    reply += f ? "1\n" : "0\n";
+    reply += "ok\n";
+  }
+
+  // set servo loop parameters
+  if(cmd.get_command() == "M55") {
+    process_set_servo_parameter_command(cmd, reply);
+  }
+
+  // calibrate joint
+  if(cmd.get_command() == "M56") {
+    process_calibrate_joint_command(cmd, reply);
+  }
+
+  // get info
+  if(cmd.get_command() == "M57") {
+    uint32_t servo_loop_freq = servo_loop_frequency_counter.get();
+    uint32_t mcontroler_freq = motion_controller_frequency_counter.get();
+
+    spin_lock_unsafe_blocking(joints_spin_lock);
+    for(int i=0; i<NUM_JOINTS; i++) {
+      float angle = joints[i]->encoder->read_abs_angle()*Constants::RAD2DEG;
+
+      reply += std::string("Joint ") + std::to_string(i)+":";
+      reply += std::string("  is_homed=") + std::to_string(joints[i]->is_homed);
+      reply += std::string("  is_calibrated=") + std::to_string(joints[i]->is_calibrated);
+      reply += std::string("  encoder_angle=") + std::to_string(angle) + " deg\n";
+    }
+    spin_unlock_unsafe(joints_spin_lock);
+
+    reply += std::string("Servo Loop: ") + std::to_string(servo_loop_freq/1000) + " kHz\n";
+    reply += std::string("Motion Controler: ") + std::to_string(mcontroler_freq) + " Hz\n";
+
+    // file list
+    reply += std::string("Files on flash: \n");
+    auto file_list = get_file_list("/", true);
+    for(auto& f : file_list) reply += std::string("  ")+f+"\n";
+    reply += "ok\n";
+  }
+
+  // get firmware version
+  if(cmd.get_command() == "M58") {
+    reply = std::string(FIRMWARE_VERSION)+"\n";
+    reply += "ok\n";
+  }
+
+  if(cmd.get_command() == "M59") {
+    int idx = (int)cmd.get_value('J', 0);
+    joints[idx]->servo_controller->get_enc_to_pos_lut().print_to_log();
+  }
+
+  // set linear and angular acceleration
+  if(cmd.get_command() == "M204") {
+    if(cmd.has_word('L')) max_acceleration.linear = cmd.get_value('L');
+    if(cmd.has_word('A')) max_acceleration.angular = cmd.get_value('A');
+    reply += "ok\n";
+  }
+}
+
 void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
   Pose6DF end_pose;
-
+  
+  if(!all_joints_ready) {
+    reply = "error: not all joints calibrated and homed\n";
+    return;
+  }
   if(path_planner.input_queue_full()) {
-    reply = "error: input queue full\n";
+    reply = "busy\n";
     return;
   }
 
@@ -350,45 +694,38 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
   }
 }
 
-void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
-  reply = "";
-  
-  if(cmd.get_command() == "M50") {
-    reply += "Current Position: ";
-    reply += std::string(" X") + std::to_string(current_pose.translation.x);
-    reply += std::string(" Y") + std::to_string(current_pose.translation.y);
-    reply += std::string(" Z") + std::to_string(current_pose.translation.z);
-    reply += "\n";
-    reply = "ok\n";
+void Robot::process_set_pose_command(const GCodeCommand& cmd, std::string& reply) {
+  Pose6DF pose;
+
+  if(!all_joints_ready) {
+    reply = "error: not all joints calibrated and homed\n";
+    return;
   }
-  if(cmd.get_command() == "M51") {
-    uint32_t servo_loop_freq = servo_loop_frequency_counter.get();
-    uint32_t mcontroler_freq = motion_controller_frequency_counter.get();
-    reply += std::string("Servo Loop: ") + std::to_string(servo_loop_freq/1000) + "kHz\n";
-    reply += std::string("Motion Controler: ") + std::to_string(mcontroler_freq/1000) + "kHz\n";
-    reply += "ok\n";
-  }
-  if(cmd.get_command() == "M52") {
-    int s = path_planner.input_queue_size();
-    reply += std::string("Queue Size: ") + std::to_string(s) + "\n";
-    reply += "ok\n";
-  }
-  if(cmd.get_command() == "M53") {
-    bool f = path_planner.all_finished();
-    reply += f ? "1\n" : "0\n";
-    reply += "ok\n";
-  }
-  if(cmd.get_command() == "M55") {
-    process_set_servo_parameter_command(cmd, reply);
-  }
-  if(cmd.get_command() == "M204") {
-    if(cmd.has_word('L')) max_acceleration.linear = cmd.get_value('L');
-    if(cmd.has_word('A')) max_acceleration.angular = cmd.get_value('A');
-    reply += "ok\n";
+
+  // read translation
+  pose.translation.x = cmd.get_value('X', current_pose.translation.x);
+  pose.translation.y = cmd.get_value('Y', current_pose.translation.y);
+  pose.translation.z = cmd.get_value('Z', current_pose.translation.z);
+
+  // read rotation (all elements must be present)
+  if(cmd.has_word('A') && cmd.has_word('B') && cmd.has_word('C')) {
+    Vec3F rot_vec(cmd.get_value('A'), cmd.get_value('B'), cmd.get_value('C'));
+    pose.rotation = QuaternionF::from_rot_vec(rot_vec);
+  } else {
+    pose.rotation = current_pose.rotation;
   }
+
+  // set the current pose und update target angles for servo loops
+  set_pose(pose);
+  reply = "ok\n";
 }
 
 void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
+  if(!all_joints_ready) {
+    reply = "error: not all joints calibrated and homed\n";
+    return;
+  }
+
   // get dwell time
   float dwell_time = 1.0f;
   if(cmd.has_word('S')) dwell_time = cmd.get_value('S');          // time given in seconds
@@ -420,3 +757,26 @@ void Robot::process_set_servo_parameter_command(const GCodeCommand& cmd, std::st
   reply = "ok\n";
 }
 
+void Robot::process_home_command(const GCodeCommand& cmd, std::string& reply) {
+  // TODO: check parameter and build joint mask
+  uint8_t joint_mask = 0;
+  for(int i=0; i<NUM_JOINTS; i++) {
+    if(cmd.has_word('A'+i))
+      joint_mask |= 1<<i;
+  }
+
+  if(joint_mask == 0)
+    joint_mask = 255;
+
+  bool ok = home(joint_mask);
+
+  reply = ok ? "ok\n" : "error\n";
+}
+
+void Robot::process_calibrate_joint_command(const GCodeCommand& cmd, std::string& reply) {
+  int idx = cmd.get_value('J', 0);
+  bool store_calibration = cmd.has_word('S');
+
+  bool ok = calibrate_joint(idx, store_calibration);
+  reply = ok ? "ok\n" : "error\n";
+}
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/robot.h b/firmware/MotionControllerRP/src/robot.h
index f42141c..fd2abfe 100644
--- a/firmware/MotionControllerRP/src/robot.h
+++ b/firmware/MotionControllerRP/src/robot.h
@@ -13,17 +13,25 @@
 #include "hardware/MT6835_encoder.h"
 #include "hardware/TB6612_motor_driver.h"
 #include "servo_control/servo_controller.h"
-#include "servo_control/encoder_lut.h"
+#include "utilities/lookup_table.h"
 #include "utilities/math_constants.h"
 
 #include "motion_control/path_planner.h"
 #include "motion_control/motion_controller.h"
 #include "command_parser/command_parser.h"
 
+constexpr int ENCODER_LUT_SIZE = 256; 
+
 //*** CALSS *****************************************************************************
 
 class Robot;
 
+//--- PersistentData --------------------------------------------------------------------
+
+struct PersistentRobotData {
+  float encoder_lut[NUM_JOINTS][ENCODER_LUT_SIZE];
+};
+
 //--- SharedData ------------------------------------------------------------------------
 
 enum class ERobotState {
@@ -35,13 +43,14 @@ enum class ERobotState {
 
 //--- SharedData ------------------------------------------------------------------------
 
+// shared data used to communicte between CPU cores
 struct SharedData {
   SharedData(int hw_spinlock_id=0){
     lock = spin_lock_instance(hw_spinlock_id);
   };
 
-  volatile float joint_positions[NUM_JOINTS];
-  volatile float joint_velocities[NUM_JOINTS];
+  volatile float joint_target_positions[NUM_JOINTS];
+  volatile float joint_target_velocities[NUM_JOINTS];
   spin_lock_t* lock = nullptr;
 };
 
@@ -52,14 +61,21 @@ class RobotJoint {
     RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
     ~RobotJoint();
 
-    void init();
-    void home();
-    void calibrate();
+    void init(int joint_idx);
+    bool calibrate();
     void update(float dt, float one_over_dt);
-
     void update_target(float p, float v);
+    bool load_calibration();
+    bool store_calibration();
+
+  private:
+    std::string calib_data_filename(std::string data_name) const;
 
   public:
+    int joint_idx = 0;
+    bool is_homed = false;
+    bool is_calibrated = false;
+
     float position;
     float velocity;
 
@@ -77,24 +93,32 @@ class Robot : public ICommandProcessor {
 
     void init();
     void calibrate();
-    void home();
+    bool home(uint8_t joint_mask=255);
+    bool calibrate_joint(int joint_idx, bool store_calibration);
+    void enable_servo_control(bool enable);  // enables joint servo controll if homed and calibrated
 
     void update_command_parser();            // called from main loop
     void update_path_planner();              // called from main loop
     void update_servo_controllers(float dt); // called from seperate cpu-core
 
+    void set_pose(const Pose6DF& pos);
+    Pose6DF pose_from_joint_angles();
+
   public:
     void send_reply(const char* str) override;
     bool can_process_command(const GCodeCommand& cmd) override;
     void process_command(const GCodeCommand& cmd, std::string& reply) override;
 
     void process_motion_command(const GCodeCommand& cmd, std::string& reply);
+    void process_set_pose_command(const GCodeCommand& cmd, std::string& reply);
     void process_dwell_command(const GCodeCommand& cmd, std::string& reply);
     void process_machine_command(const GCodeCommand& cmd, std::string& reply);
-    void process_set_pose_command(const GCodeCommand& cmd, std::string& reply);
     void process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply);
+    void process_home_command(const GCodeCommand& cmd, std::string& reply);
+    void process_calibrate_joint_command(const GCodeCommand& cmd, std::string& reply);
 
   protected:
+    bool check_all_joints_ready();   // checks if all joints are homed and calibrated
     static bool update_motion_controller_isr(repeating_timer_t* timer); // called from update timer
 
   private:
@@ -102,16 +126,17 @@ class Robot : public ICommandProcessor {
     uint32_t path_buffering_time_us;
     uint64_t path_buffering_start_time;
 
-    int motor_pole_pairs;
+    bool all_joints_ready;
     RobotJoint* volatile joints[NUM_JOINTS];
+    spin_lock_t* joints_spin_lock = nullptr;
 
     IKinemtaicModel* kinematic_model;
     PathPlanner path_planner;
     MotionController motion_controller;
     CommandParser command_parser;
 
-    LinearAngular max_acceleration;
     Pose6DF current_pose;
+    LinearAngular max_acceleration;
     LinearAngular current_feedrate;
 
     SharedData shared_data;
diff --git a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp
new file mode 100644
index 0000000..d681f4b
--- /dev/null
+++ b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp
@@ -0,0 +1,84 @@
+// --------------------------------------------------------------------------------------
+// Project: MicroManipulatorStepper
+// License: MIT (see LICENSE file for full description)
+//          All text in here must be included in any redistribution.
+// Author:  M. S. (diffraction limited)
+// --------------------------------------------------------------------------------------
+
+#include <algorithm>
+
+#include "servo_controller.h"
+#include "utilities/logging.h"
+#include "utilities/math_constants.h"
+
+#include "actuator_calibration.h"
+
+
+//*** FUNCTION ***********************************************************************************/
+
+bool measure_calibration_data(
+            LookupTable& encoder_raw_to_motor_pos_lut,
+            LookupTable& motor_pos_to_field_angle_lut,
+            ServoController& servo_controller,
+            float calibration_range,
+            float field_velocity,
+            size_t table_size)
+{
+  LOG_INFO("Measuring motor to encoder angle lookup table...");
+  int sample_count = table_size*4;
+
+  // get required values
+  std::vector<std::pair<float, float>> motor_pos_and_field_angle;
+  std::vector<std::pair<float, float>> encoder_angle_and_motor_pos;
+
+  auto& motor_driver = servo_controller.get_motor_driver();
+  float pole_pair_count = servo_controller.get_pole_pair_count();
+  float start_field_angle = motor_driver.get_field_angle();
+  float field_angle_step = calibration_range*pole_pair_count/(sample_count-1);
+
+  auto run_measurement = [&](int sample_count, float field_angle_step) {
+    // Measure in increasing direction
+    for (size_t i = 0; i < sample_count; ++i) {
+      if(i>0)
+        motor_driver.rotate_field(field_angle_step, field_velocity, nullptr);
+
+      float encoder_angle_raw = servo_controller.get_encoder().read_abs_angle_raw();
+      float field_angle = motor_driver.get_field_angle();
+      float motor_pos = (field_angle-start_field_angle)/pole_pair_count;
+      // TODO: read motor_pos from precise reference encoder
+
+      encoder_angle_and_motor_pos.push_back({encoder_angle_raw, motor_pos});
+      motor_pos_and_field_angle.push_back({motor_pos, field_angle});
+    }
+  };
+
+  // Measure in increasing direction
+  LOG_DEBUG("Running foreward pass...");
+  run_measurement(sample_count, field_angle_step);
+  LOG_DEBUG("Running backward pass...");
+  run_measurement(sample_count, -field_angle_step);
+
+  // rotate back to start position
+  motor_driver.rotate_field(start_field_angle-motor_driver.get_field_angle(), 
+    Constants::TWO_PI_F*40.0f, [&servo_controller]() {
+      servo_controller.get_encoder().read_abs_angle_raw();
+  });
+
+  // build lookup tables
+  bool ok = encoder_raw_to_motor_pos_lut.init_interpolating(encoder_angle_and_motor_pos, table_size, true);
+  encoder_raw_to_motor_pos_lut.optimize_lut(encoder_angle_and_motor_pos);
+  if(ok == false) {
+    LOG_ERROR("Creating lookup table encoder_raw_angle -> motor_pos failed.");
+    return false;
+  }
+
+  ok = motor_pos_to_field_angle_lut.init_interpolating(motor_pos_and_field_angle, table_size/2, true);
+  motor_pos_to_field_angle_lut.optimize_lut(motor_pos_and_field_angle);
+  if(ok == false) {
+    LOG_ERROR("Creating lookup table motor_pos -> field_angle failed.");
+    return false;
+  }
+
+  LOG_INFO("finished");
+  return true;
+}
diff --git a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.h b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.h
new file mode 100644
index 0000000..912be5c
--- /dev/null
+++ b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.h
@@ -0,0 +1,22 @@
+// --------------------------------------------------------------------------------------
+// Project: MicroManipulatorStepper
+// License: MIT (see LICENSE file for full description)
+//          All text in here must be included in any redistribution.
+// Author:  M. S. (diffraction limited)
+// --------------------------------------------------------------------------------------
+
+#pragma once
+
+#include "servo_controller.h"
+#include "utilities/lookup_table.h"
+
+//*** FUNCTIONS *************************************************************************
+
+bool measure_calibration_data(
+  LookupTable& encoder_raw_to_motor_pos_lut,
+  LookupTable& motor_pos_to_field_angle_lut,
+  ServoController& servo_controller,
+  float field_angle_range,
+  float field_velocity,
+  size_t size);
+
diff --git a/firmware/MotionControllerRP/src/servo_control/encoder_lut.cpp b/firmware/MotionControllerRP/src/servo_control/encoder_lut.cpp
deleted file mode 100644
index 5a72b7d..0000000
--- a/firmware/MotionControllerRP/src/servo_control/encoder_lut.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-// --------------------------------------------------------------------------------------
-// Project: MicroManipulatorStepper
-// License: MIT (see LICENSE file for full description)
-//          All text in here must be included in any redistribution.
-// Author:  M. S. (diffraction limited)
-// --------------------------------------------------------------------------------------
-
-#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);
-  }
-}
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp b/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp
new file mode 100644
index 0000000..e3ecc7c
--- /dev/null
+++ b/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp
@@ -0,0 +1,137 @@
+#include "homing_controller.h"
+#include "utilities/math_constants.h"
+#include "utilities/logging.h"
+#include "pico/time.h"
+
+HomingController::HomingController() {
+  retract_field_velocity = 30.0f; // rad per second
+}
+
+bool HomingController::run_blocking(ServoController* servo_controller, float motor_velocity, float search_range, float current) {
+  start(servo_controller, motor_velocity, search_range, current);
+  while(is_finished() == false) {
+    update();
+  }
+  finalize();
+  return is_successful();
+}
+
+void HomingController::start(ServoController* servo_controller, float velocity, float range, float current) {
+  float pole_pair_count = servo_controller->get_pole_pair_count();
+  float field_angle_to_encoder_angle = Constants::TWO_PI_F*30.0f/3.0f*0.5f / pole_pair_count;
+  
+  eval_field_angle_delta = Constants::TWO_PI_F*0.1f;
+  expected_encoder_delta = eval_field_angle_delta * field_angle_to_encoder_angle;
+  
+  servo_ctrl = servo_controller;
+  field_velocity = velocity * pole_pair_count;
+  field_angle_search_range = range * pole_pair_count;
+  homing_current = current;
+
+  auto& motor_driver = servo_ctrl->get_motor_driver();
+  auto& encoder = servo_ctrl->get_encoder();
+
+  // perform 'soft start'
+  servo_ctrl->set_motor_enabled(true, false);
+
+  initial_current = servo_ctrl->get_motor_driver().get_amplitude();
+  servo_ctrl->get_motor_driver().set_amplitude_smooth(homing_current, 100);
+  search_failed = false;
+
+  // motor_driver.rotate_field(Constants::TWO_PI_F*0.5f * (field_velocity>0.0f ? -1.0f : 1.0f), 12.0f);
+  start_field_angle = fmodf(motor_driver.get_field_angle(), Constants::TWO_PI_F);
+
+  last_eval_encoder_angle = encoder.read_abs_angle();
+  last_time = 0;
+
+  state = State::Homing;
+}
+
+void HomingController::update() {
+  if (state != State::Homing)
+    return;
+
+  auto& motor_driver = servo_ctrl->get_motor_driver();
+  auto& encoder = servo_ctrl->get_encoder();
+
+  uint64_t time_us = time_us_64();
+  if(last_time == 0) last_time = time_us;
+  float dt = float(time_us - last_time) * 1e-6f;
+  last_time = time_us;
+
+  // Move motor and read encoder
+  field_angle_offset += field_velocity * dt;
+  motor_driver.set_field_angle(start_field_angle+field_angle_offset);
+
+  float encoder_angle = encoder.read_abs_angle();
+
+  if (fabs(last_eval_field_angle_offset - field_angle_offset) > eval_field_angle_delta) {
+    float encoder_delta = encoder_angle - last_eval_encoder_angle;
+    float encoder_velocity_ratio = encoder_delta / expected_encoder_delta;
+
+    // LOG_DEBUG("encoder_delta=%f/ %f", encoder_delta, expected_encoder_delta);
+    // LOG_DEBUG("encoder_velocity_ratio=%f", encoder_velocity_ratio);
+
+    if (fabsf(encoder_velocity_ratio) < 0.05f) {
+      LOG_DEBUG("End stop detected");
+      on_endstop_detected();
+      return;
+    }
+
+    last_eval_encoder_angle = encoder_angle;
+    last_eval_field_angle_offset = field_angle_offset;
+  }
+
+  if (fabs(field_angle_offset) > field_angle_search_range) {
+    LOG_INFO("End stop not detected");
+    search_failed = true;
+    finalize();
+  }
+}
+
+void HomingController::on_endstop_detected() {
+  state = State::Done;
+  auto& motor_driver = servo_ctrl->get_motor_driver();
+  auto& encoder = servo_ctrl->get_encoder();
+
+  if(search_failed) {
+    servo_ctrl->set_motor_enabled(false, false);
+    return;
+  }
+
+  // reset encoder period, the remainder will provide a very repeatable position reference
+  encoder.read_abs_angle();
+  servo_ctrl->get_encoder().reset_abs_angle_period();
+
+  // the motor is currently held against the end stop by the field, defining a geometric reference
+  home_encoder_angle = encoder.read_abs_angle();
+  LOG_DEBUG("home_encoder_angle=%f deg", home_encoder_angle*Constants::RAD2DEG);
+  if(home_encoder_angle < Constants::TWO_PI_F*0.01 || home_encoder_angle > Constants::TWO_PI_F*0.99)
+    LOG_WARNING("encoder angle at home position close to wrap around point !");
+}
+
+void HomingController::finalize() {
+   auto& motor_driver = servo_ctrl->get_motor_driver();
+
+  // back off from home position
+  float backoff_field_angle = Constants::TWO_PI_F*0.25f;
+  motor_driver.rotate_field(backoff_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f), 
+                            retract_field_velocity, nullptr);
+
+  // restore previous motor current
+  motor_driver.set_amplitude_smooth(initial_current, 100);
+}
+
+bool HomingController::is_finished() const {
+  return state == State::Done;
+}
+
+bool HomingController::is_successful() const {
+  return state == State::Done && !search_failed;
+}
+
+float HomingController::get_home_encoder_angle() const {
+  return home_encoder_angle;
+}
+
+
diff --git a/firmware/MotionControllerRP/src/servo_control/homing_controller.h b/firmware/MotionControllerRP/src/servo_control/homing_controller.h
new file mode 100644
index 0000000..32ae186
--- /dev/null
+++ b/firmware/MotionControllerRP/src/servo_control/homing_controller.h
@@ -0,0 +1,76 @@
+// --------------------------------------------------------------------------------------
+// Project: MicroManipulatorStepper
+// License: MIT (see LICENSE file for full description)
+//          All text in here must be included in any redistribution.
+// Author:  M. S. (diffraction limited)
+// --------------------------------------------------------------------------------------
+
+#pragma once
+
+#include "servo_controller.h"
+
+//*** CLASS *****************************************************************************
+
+/**
+ * This class implements the homing procedure for a single actuator. To allow for parallel 
+ * homing the class is stateful and has an update() function, that can be called togeather
+ * with the updates of other homing controllers inside a loop.
+ * 
+ * Homing procedure:
+ *   1. move axis in negative direction until a physical hard stop is reached
+ *   2. reset encoder period
+ *   3. back off slightly from the hard stop
+ */
+class HomingController {
+  public:
+    HomingController();
+
+    // Starts the homing cycle, motor_velocity can be negative and defines the homing direction.
+    // WARNING: Servo loop updates (including encoder reads) must be completely disabled during homing.
+    bool run_blocking(ServoController* servo_controller, float motor_velocity, float search_range, float current);
+    void start(ServoController* servo_controller, float motor_velocity, float search_range, float current);
+    void update();
+    void finalize();
+
+    bool is_finished() const;
+    bool is_successful() const;
+    float get_home_encoder_angle() const;
+
+  private:
+    void on_endstop_detected();
+    float compute_eval_pos_delta(float pos, float field_angle_delta);
+
+  private:
+    enum class State {
+      Idle,
+      Initializing,
+      Homing,
+      Done
+    };
+
+    ServoController* servo_ctrl;
+
+    // Configuration params
+    float field_velocity = 0.0f; // defines homing direction
+    float field_angle_search_range = 0.0f;
+    float homing_current = 0.0f;
+    float initial_current = 0.0f;
+    float retract_field_velocity = 0.0f;
+
+    // State machine
+    State state = State::Idle;
+    bool search_failed = false;
+
+    // Timing
+    uint64_t last_time = 0;
+
+    // Offsets and tracking
+    float start_field_angle = 0.0f;
+    float field_angle_offset = 0.0f;
+    float last_eval_field_angle_offset = 0.0f;
+    float last_eval_encoder_angle = 0.0f;
+    float eval_field_angle_delta = 0.0f;
+    float expected_encoder_delta = 0.0f;
+
+    float home_encoder_angle = 0.0f;
+};
diff --git a/firmware/MotionControllerRP/src/servo_control/pid.cpp b/firmware/MotionControllerRP/src/servo_control/pid.cpp
index 8042b6e..56706b2 100644
--- a/firmware/MotionControllerRP/src/servo_control/pid.cpp
+++ b/firmware/MotionControllerRP/src/servo_control/pid.cpp
@@ -20,37 +20,37 @@ void PIDController::set_parameter(float kP, float kI, float kD, float output_lim
 
 // PID controller function
 float PIDController::compute(float error, float dt, float one_over_dt) {
-    // Proportional component
-    float proportional = kP * error;
-    float output = proportional;
+  // 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;
-    }
+  // 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;
-    }
+  // 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;
+  // 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;
+  integral_prev = 0.0f;
+  error_prev = 0.0f;
 }
 
 //--- LowpassFilter -----------------------------------------------------------
@@ -67,4 +67,8 @@ float LowpassFilter::update(float value, float dt) {
   float v = value_prev*alpha + (1.0f - alpha)*value;
   value_prev = v;
   return v;
-}
\ No newline at end of file
+}
+
+void LowpassFilter::reset(float value) {
+  value_prev = value;
+}
diff --git a/firmware/MotionControllerRP/src/servo_control/pid.h b/firmware/MotionControllerRP/src/servo_control/pid.h
index 098aa3f..cb48cf1 100644
--- a/firmware/MotionControllerRP/src/servo_control/pid.h
+++ b/firmware/MotionControllerRP/src/servo_control/pid.h
@@ -8,6 +8,7 @@ class LowpassFilter {
 
     void  set_time_constant(float time_constant);
     float update(float value, float dt);
+    void  reset(float value);
 
   private:
     float value_prev;
diff --git a/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp b/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
index 67bec01..39a6b82 100644
--- a/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
+++ b/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
@@ -6,7 +6,7 @@
 // --------------------------------------------------------------------------------------
 
 #include "hardware/timer.h"
-#include "Arduino.h"
+#include "pico/stdlib.h"
 
 #include "servo_controller.h"
 #include "utilities/logging.h"
@@ -19,11 +19,20 @@ ServoController::ServoController(
   ENCODER_TYPE& encoder, 
   int32_t motor_pole_pair_count) :
     motor_driver(motor_driver),
-    encoder(encoder), 
-    motorpos_to_field_angle(motor_pole_pair_count) 
+    encoder(encoder)
 {
-  motor_pos = 0.0f;
-  pos_error = 0.0f;
+  ServoController::motor_pole_pair_count = motor_pole_pair_count;
+  ServoController::motor_update_enabled = false;
+  ServoController::encoder_update_enabled = true;
+  ServoController::motor_pos = 0.0f;
+  ServoController::pos_error = 0.0f;
+
+  // set default encoder lut
+  using namespace Constants;
+  float magnet_array_radius = 30.0f; // mm
+  float magnet_pitch = 3.0f;         // mm
+  float g = float(encoder.get_rawcounts_per_rev())*(TWO_PI_F*magnet_array_radius/magnet_pitch)*0.5f;
+  build_linear_lut(encoder_raw_to_motor_pos_lut, -g, g, -TWO_PI_F, TWO_PI_F);
 }
 
 void ServoController::init(float max_motor_amplitude) {
@@ -31,26 +40,41 @@ void ServoController::init(float max_motor_amplitude) {
 
   // setup motor driver
   motor_driver.begin();
-  motor_driver.set_amplitude(0.0f, true); 
+  motor_driver.set_amplitude(0.0f, true); // correct amplitude will be set by 'set_motor_enabled()' 
   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.004f);
   pos_controller.set_parameter(75.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
   velocity_controller.set_parameter(0.2f, 150.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
+
+ // pos_controller.set_parameter(75.0f, 2000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
+ // velocity_controller.set_parameter(0.2f, 0.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::set_enc_to_pos_lut(LookupTable& lut) {
+  ServoController::encoder_raw_to_motor_pos_lut = lut;
 }
 
-void ServoController::update(float target_motor_pos, float dt, float one_over_dt) { 
+// get the motor position to field angle lookup table
+const LookupTable& ServoController::get_enc_to_pos_lut() const {
+  return encoder_raw_to_motor_pos_lut;
+}
+
+void ServoController::set_pos_to_field_lut(LookupTable& lut) {
+  ServoController::motor_pos_to_field_angle_lut = lut;
+}
+
+// get the motor position to field angle lookup table
+const LookupTable& ServoController::get_pos_to_field_lut() const {
+  return motor_pos_to_field_angle_lut;
+}
+
+
+void ServoController::update(float target_motor_pos, float dt, float one_over_dt) {
+  if(encoder_update_enabled == false)
+    return;
+
   // read encoder
   int32_t encoder_angle_raw = encoder.read_abs_angle_raw();
 
@@ -72,7 +96,9 @@ void ServoController::update(float target_motor_pos, float dt, float one_over_dt
 
   // 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); 
+  if(motor_update_enabled) {
+    motor_driver.set_field_angle(field_angle + output);
+  } 
 
   // store values for next update
   motor_pos_prev = motor_pos;
@@ -125,93 +151,33 @@ bool ServoController::move_to(float target_motor_pos, float at_pos_eps, float se
   return false;
 }
 
-void ServoController::move_to_open_loop(float target_motor_pos, float motor_angular_velocity) {
+void ServoController::move_to_open_loop(float delta_motor_pos, float motor_angular_velocity) {
   // Determine direction of movement at the start
-  const bool moving_forward = target_motor_pos > motor_pos;
+  const bool moving_forward = delta_motor_pos > 0.0f;
 
   uint64_t last_time = time_us_64();
-  while ((moving_forward && motor_pos < target_motor_pos) ||
-         (!moving_forward && motor_pos > target_motor_pos))
+  float pos = 0.0f;
+  while (fabs(pos) < delta_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();
+    if(encoder_update_enabled)
+      encoder.read_abs_angle_raw();
 
     // update motor position
-    motor_pos += moving_forward ? motor_angular_velocity * dt : -motor_angular_velocity * dt;
+    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));
+    float clamped_motor_pos = moving_forward ? std::min(pos, delta_motor_pos) : 
+                                               std::max(pos, -delta_motor_pos);
+    motor_driver.set_field_angle(clamped_motor_pos*motor_pole_pair_count);
     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);
+  motor_pos += delta_motor_pos;
 }
 
 ServoController::ENCODER_TYPE& ServoController::get_encoder() {
@@ -222,69 +188,49 @@ 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);
+float ServoController::get_pole_pair_count() {
+  return motor_pole_pair_count;
+}
+
+void ServoController::set_motor_enabled(bool enable, bool synchronize_field_angle) {
+  if(enable) {
+    // synchronize field angle to motor_pos
+    if(synchronize_field_angle) {
+      float start_field_angle = motor_pos_to_field_angle(motor_pos);
+      motor_driver.set_field_angle(start_field_angle);
+    }
+
+    motor_driver.set_amplitude_smooth(motor_current_amplitude, 100);
+    pos_controller.reset();
+    velocity_controller.reset();
+    velocity_lowpass.reset(0.0f);
+    motor_pos_prev = motor_pos;
+
   } else {
-    return enc_to_pos_lut.evaluate(encoder_angle_raw);
+    motor_driver.set_amplitude_smooth(0.0f, 100);
   }
 }
 
+// enable or disable servo loop update and encoder reads
+void ServoController::set_motor_update_enabled(bool enable) {
+  pos_controller.reset();
+  velocity_controller.reset();
+  velocity_lowpass.reset(0.0f);
+  motor_pos_prev = motor_pos;
+  motor_update_enabled = enable;
+}
+
+void ServoController::set_encoder_update_enabled(bool enable) {
+  pos_controller.reset();
+  velocity_controller.reset();
+  motor_pos_prev = motor_pos;
+  encoder_update_enabled = enable;
+}
+
+float ServoController::encoder_angle_to_motor_pos(int32_t encoder_angle_raw) {
+  return encoder_raw_to_motor_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;
-}
+  return motor_pos_to_field_angle_lut.evaluate(motor_pos);
+}
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/servo_control/servo_controller.h b/firmware/MotionControllerRP/src/servo_control/servo_controller.h
index 2411942..d0bd2d7 100644
--- a/firmware/MotionControllerRP/src/servo_control/servo_controller.h
+++ b/firmware/MotionControllerRP/src/servo_control/servo_controller.h
@@ -9,9 +9,11 @@
 
 #include "hardware/MT6835_encoder.h"
 #include "hardware/TB6612_motor_driver.h"
-#include "encoder_lut.h"
+#include "utilities/lookup_table.h"
 #include "pid.h"
 
+//*** CLASS *****************************************************************************
+
 class ServoController {
   public:
     // use defines instead of virtual functions for speed
@@ -22,40 +24,70 @@ class ServoController {
   public:
     ServoController(MOTOR_DRIVER_TYPE& motor_driver, ENCODER_TYPE& encoder, int32_t motor_pole_pairs);
 
+    // initialize the servo controller hardware
     void init(float max_motor_amplitude);
 
-    void set_encoder_lut(LookupTable& enc_to_pos_lut);
+    // set the encoder raw angle to motor position lookup table
+    void set_enc_to_pos_lut(LookupTable& lut);
+    // get the encoder raw angle to motor position lookup table
+    const LookupTable& get_enc_to_pos_lut() const;
 
+    // set the motor position to field angle lookup table
+    void set_pos_to_field_lut(LookupTable& lut);
+    // get the motor position to field angle lookup table
+    const LookupTable& get_pos_to_field_lut() const;
+
+    // Updates the servo loop. 
     void update(float target_motor_pos,
                 float dt,
                 float one_over_dt);
 
+    // Checks if the motor is at position (uses values from previous update() call).
     bool at_position(float motor_pos_eps);
 
+    // Reads the current motor position from the encoder.
     float read_position();
 
+    // Returns the motor position.
     float get_position();
 
+    // Returns the current position error from the last servo loop update.
     float get_position_error();
 
+    // Moves to a new motor position using closed loop control (blocking).
     bool move_to(float target_motor_angle, 
                  float at_pos_motor_angle_eps, 
                  float settle_time_ms,
                  float timeout_us);
 
+    // Moves to a new motor position using open loop controll (blocking).
+    // Motor updates must be disabled if servo loop is running in background.
     void move_to_open_loop(float target_motor_angle, 
                            float angular_velocity);
-
-    void home(float motor_velocity, float search_range, float current=0.2f);
     
+    // Returns the encoder object.
     ENCODER_TYPE& get_encoder();
-    MOTOR_DRIVER_TYPE& get_motor_driver();
-    float output;
 
-  private:
+    // Returns the motor driver object.
+    MOTOR_DRIVER_TYPE& get_motor_driver();
+
+    // Returns number of motor pole pairs.
+    // Can be used to approximate conversion of motor position to field angle.
+    float get_pole_pair_count();
+
+    // enable or disable motor 
+    void set_motor_enabled(bool enable, bool synchronize_field_angle);
+
+    // enable or disable motor updates
+    void set_motor_update_enabled(bool enable);
+
+    // enable or disable encoder reads
+    void set_encoder_update_enabled(bool enable);
+
+  public:
     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);
+    float motor_pos_to_field_angle_derivative(float motor_pos);
 
   public:
     LowpassFilter velocity_lowpass;
@@ -65,22 +97,16 @@ class ServoController {
   private:
     ENCODER_TYPE& encoder;
     MOTOR_DRIVER_TYPE& motor_driver;
-    LookupTable enc_to_pos_lut;
+    LookupTable encoder_raw_to_motor_pos_lut;
+    LookupTable motor_pos_to_field_angle_lut;
 
+    float motor_pole_pair_count = 0.0f;   // number as motor pole pairs (as float to avoid repeated conversion)
+    float motor_current_amplitude = 0.5f; // motor current in range [0..1]
     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);
\ No newline at end of file
+    float output = 0.0f;                  // servo loop output (field angle offset)
+    bool motor_update_enabled = false;    // enables mootor field updates
+    bool encoder_update_enabled = true;   // enables encoder reads
+};
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/utilities/logging.cpp b/firmware/MotionControllerRP/src/utilities/logging.cpp
index c8d346b..47c42fa 100644
--- a/firmware/MotionControllerRP/src/utilities/logging.cpp
+++ b/firmware/MotionControllerRP/src/utilities/logging.cpp
@@ -70,10 +70,10 @@ void Logger::log(ELogLevel level, const char* fmt, va_list args) {
 
 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] ";
+    case ELogLevel::DEBUG: return "D)";
+    case ELogLevel::INFO:  return "I)";
+    case ELogLevel::WARN:  return "W)";
+    case ELogLevel::ERROR: return "E)";
     default:               return "";
   }
 }
diff --git a/firmware/MotionControllerRP/src/utilities/lookup_table.cpp b/firmware/MotionControllerRP/src/utilities/lookup_table.cpp
new file mode 100644
index 0000000..253cc5c
--- /dev/null
+++ b/firmware/MotionControllerRP/src/utilities/lookup_table.cpp
@@ -0,0 +1,464 @@
+// --------------------------------------------------------------------------------------
+// Project: MicroManipulatorStepper
+// License: MIT (see LICENSE file for full description)
+//          All text in here must be included in any redistribution.
+// Author:  M. S. (diffraction limited)
+// --------------------------------------------------------------------------------------
+
+#include <pico/stdlib.h>
+#include <LittleFS.h> 
+#include <algorithm> 
+
+#include "lookup_table.h"
+#include "utilities/logging.h"
+#include "utilities/math_constants.h" 
+
+//*** FUNCTIONS *************************************************************************
+
+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);
+}
+
+//*** CLASS *****************************************************************************
+
+bool LookupTable::init(int32_t table_size, float input_min, float input_max) {
+  lookup_table.clear();
+  lookup_table.resize(table_size, 0.0f);
+  if(input_min>=input_max) {
+    LOG_ERROR("LookupTable: input_max must be larger than input_min");
+    return false;
+  }
+
+  LookupTable::input_min = input_min;
+  LookupTable::input_max = input_max;
+  LookupTable::one_over_input_range = 1.0f/(input_max-input_min);
+
+  return true;
+}
+
+/*
+bool LookupTable::init_approximating(std::vector<std::pair<float, float>> in_out_pairs, int table_size, float sigma) {
+    const int N = int(in_out_pairs.size());
+    if (N < 2 || table_size < 2)
+        return false;
+
+    // Sort by x ascending
+    std::sort(in_out_pairs.begin(), in_out_pairs.end(),
+              [](const auto& a, const auto& b) { return a.first < b.first; });
+
+    input_min = in_out_pairs.front().first;
+    input_max = in_out_pairs.back().first;
+    if (input_min >= input_max)
+        return false;
+
+    one_over_input_range = 1.0f / (input_max - input_min);
+    lookup_table.resize(table_size, 0.0f);
+
+    int start = 0;
+    int end = 0;
+    const float hw_size = (sigma*3.0f)*0.5f;
+    const float two_sigma_sq = 2.0f * powf(sigma, 2.0f);
+
+    for (int i = 0; i < table_size; ++i) {
+      // compute current evaluation position
+      float t = static_cast<float>(i) / (table_size - 1);
+      float x = input_min + t * (input_max - input_min);
+
+      // Advance window start and end index
+      float x_min = x - hw_size;
+      while (start < N - 1 && in_out_pairs[start].first < x_min) start++;
+      float x_max = x + hw_size;
+      while (end < N - 1 && in_out_pairs[end].first < x_max) end++;
+
+      // compute weighted average in window range
+      float sum = 0.0f;
+      float wcount = 0;
+      for (int idx = start; idx <= end; idx++) {
+        float dx = in_out_pairs[idx].first - x;
+        float w = std::exp(-(dx * dx) / two_sigma_sq);
+        sum += in_out_pairs[idx].second * w;
+        wcount += w;
+      }
+
+      // store weighted average value
+      lookup_table[i] = sum / wcount;
+    }
+
+    return true;
+}
+*/
+
+bool LookupTable::init_interpolating(std::vector<std::pair<float, float>> in_out_pairs, 
+                                     int table_size, bool sort_input)
+{
+  if (in_out_pairs.size() < 2)
+    return false;
+
+  // If input is in descending order, reverse it
+  if(sort_input) {
+    std::sort(in_out_pairs.begin(), in_out_pairs.end(), 
+              [](const auto& a, const auto& b) { return a.first < b.first; });
+  }
+  else if (in_out_pairs.front().first > in_out_pairs.back().first) {
+    std::reverse(in_out_pairs.begin(), in_out_pairs.end());
+  }
+
+  // check if values are now ascending (i.e the original input values where monotonic)
+  bool is_ascending = std::is_sorted(in_out_pairs.begin(), in_out_pairs.end(),
+                                     [](const auto& a, const auto& b) { return a.first < b.first; });
+  if(is_ascending == false) {
+    LOG_ERROR("LookupTable could not be initialized, input values must be monotonic");
+    return false;
+  }
+
+  // get input range
+  input_min = in_out_pairs.front().first;
+  input_max = in_out_pairs.back().first;
+
+  // check for invalid input range
+  if (input_min >= input_max)
+    return false; 
+  one_over_input_range = 1.0f / (input_max - input_min);
+
+  // define lookup table size
+  lookup_table.resize(table_size, 0.0f);
+
+  int j = 0; // index in in_out_pairs
+  for (int i = 0; i < table_size; ++i) {
+    float t = static_cast<float>(i) / (table_size - 1); // normalized [0,1]
+    float x = input_min + t * (input_max - input_min);
+
+    // advance j until x is between in_out_pairs[j] and in_out_pairs[j + 1]
+    while (j + 1 < in_out_pairs.size() && x > in_out_pairs[j + 1].first) j++;
+
+    float x0 = in_out_pairs[j].first;
+    float y0 = in_out_pairs[j].second;
+    float x1 = in_out_pairs[j + 1].first;
+    float y1 = in_out_pairs[j + 1].second;
+
+    if (std::fabs(x1 - x0) < std::numeric_limits<float>::epsilon()) {
+      lookup_table[i] = y0;
+    } else {
+      float alpha = (x - x0) / (x1 - x0);
+      lookup_table[i] = y0 + alpha * (y1 - y0);
+    }
+  }
+
+  return true;
+}
+
+bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs) {
+  if (lookup_table.empty() || in_out_pairs.empty())
+    return false;
+
+  LOG_DEBUG("Optimizing lookup table...");
+
+  const float learning_rate = 0.02f;
+  const int max_iterations = 1000;
+  const int N = static_cast<int>(lookup_table.size());
+
+  // Gradient descent loop
+  std::vector<float> gradients(N, 0.0f);
+  float total_loss = 0.0f;
+  for (int iter = 0; iter < max_iterations; iter++) {
+    // Accumulate gradients for each pair
+    total_loss = 0.0f;
+    for (const auto &pair : in_out_pairs) {
+      float x = pair.first;
+      float y_target = pair.second;
+
+      // Compute interpolation indices and weights
+      int idx_a, idx_b;
+      float w_a, w_b;
+      linear_interpolate(x, idx_a, idx_b, w_a, w_b);
+
+      // Current output from LUT
+      float y_pred = w_a * lookup_table[idx_a] + w_b * lookup_table[idx_b];
+      float error = y_pred - y_target;
+
+      total_loss += error * error;
+
+      // Gradient of loss wrt y_pred = 2 * error
+      float grad_loss = 2.0f * error;
+
+      // Distribute gradient to LUT entries weighted by interpolation weights
+      gradients[idx_a] += grad_loss * w_a;
+      gradients[idx_b] += grad_loss * w_b;
+
+      //if(iter == max_iterations-1)
+      //  LOG_DEBUG("error=%f", iter, error);
+    }
+
+    // Update LUT entries
+    for (int i = 0; i < N; i++) {
+      lookup_table[i] -= learning_rate * gradients[i];
+      gradients[i] = 0.0f;
+    }
+
+    //if(iter%100 == 0)
+    //  LOG_DEBUG("iteration %04i: rms=%f", iter, sqrtf(total_loss));
+  }
+  LOG_DEBUG("Optimizing lookup table finished: rms=%f", total_loss);
+
+  return true;
+}
+
+void LookupTable::clear() {
+  lookup_table.clear();
+}
+
+// returns the size of the lookup table
+uint32_t LookupTable::size() const {
+  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) const {
+  return lookup_table[idx];
+}
+
+float LookupTable::evaluate(float x) const {
+  if (lookup_table.size() < 2)
+    return 0.0f;
+
+  int idx_a, idx_b; 
+  float weight_a, weight_b;
+  linear_interpolate(x, idx_a, idx_b, weight_a, weight_b);
+
+  float a = lookup_table[idx_a];
+  float b = lookup_table[idx_b];
+  return a + weight_b * (b - a);
+}
+
+void LookupTable::linear_interpolate(float x, int& idx_a, int& idx_b, 
+                                     float& weight_a, float& weight_b) const 
+{
+  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) {
+    idx_a = idx_b = 0;
+    weight_a = 1.0f;
+    weight_b = 0.0f;
+  } else if (t >= 1.0f) {
+    idx_a = idx_b = table_size-1;
+    weight_a = 0.0f;
+    weight_b = 1.0f;
+  } else {
+    index = static_cast<size_t>(std::floor(pos));  
+    idx_a = index;
+    idx_b = index + 1;
+    weight_b = pos - index;
+    weight_a = (1.0f-weight_b);
+  }
+}
+
+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 LookupTable::in_input_range(float x) const {
+  return x >= input_min && x <= input_max;
+}
+
+bool LookupTable::in_output_range(float x) const {
+  if(lookup_table.size() < 2) return false;
+  float a = lookup_table.front();
+  float b = lookup_table.back();
+  return x >= std::min(a,b) && x <= std::max(a,b);
+}
+
+void LookupTable::get_intput_range(float& input_min, float& input_max) const {
+  input_min = LookupTable::input_min;
+  input_max = LookupTable::input_max;
+}
+
+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);
+  }
+}
+
+//*** FUNCTION ***********************************************************************************/
+
+void build_linear_lut(LookupTable& lut, float in_min, float in_max, float out_min, float out_max) {
+  lut.init(2, in_min, in_max);
+  lut.set_entry(0, out_min);
+  lut.set_entry(1, out_max);
+}
+
+bool save_lut_to_file(const LookupTable& lut, const char* filename) {
+  LOG_DEBUG("Saving Lookup table to file '%s'...", filename);
+
+  File file = LittleFS.open(filename, "w");
+  if (!file) {
+    LOG_ERROR("Failed to open file '%s' for writing", filename);
+    return false;
+  }
+
+  // Save metadata
+  uint32_t size = lut.size();
+  float input_min, input_max;
+  lut.get_intput_range(input_min, input_max);
+
+  // Write metadata (size, input_min, input_max)
+  if (file.write((uint8_t*)&size, sizeof(size)) != sizeof(size)) return false;
+  if (file.write((uint8_t*)&input_min, sizeof(input_min)) != sizeof(input_min)) return false;
+  if (file.write((uint8_t*)&input_max, sizeof(input_max)) != sizeof(input_max)) return false;
+
+  // Write all LUT entries
+  for (uint32_t i = 0; i < size; i++) {
+    float v = lut.get_entry(i);
+    if (file.write((uint8_t*)&v, sizeof(v)) != sizeof(v)) return false;
+  }
+
+  file.close();
+
+  // LOG_DEBUG("Saving Lookup table to file successful");
+  return true;
+}
+
+bool load_lut_from_file(LookupTable& lut, const char* filename) {
+  LOG_DEBUG("Loading Lookup table from file '%s'...", filename);
+  File file = LittleFS.open(filename, "r");
+  if (!file) {
+    LOG_ERROR("Failed to open file '%s' for reading", filename);
+    return false;
+  }
+
+  // Read metadata
+  uint32_t size = 0;
+  float input_min = 0.0f, input_max = 0.0f;
+
+  if (file.read((uint8_t*)&size, sizeof(size)) != sizeof(size)) return false;
+  if (file.read((uint8_t*)&input_min, sizeof(input_min)) != sizeof(input_min)) return false;
+  if (file.read((uint8_t*)&input_max, sizeof(input_max)) != sizeof(input_max)) return false;
+
+  if (!lut.init(size, input_min, input_max)) {
+    LOG_ERROR("Failed to create LUT from file");
+    file.close();
+    return false;
+  }
+
+  // Read LUT entries
+  for (uint32_t i = 0; i < size; i++) {
+    float v = 0.0f;
+    if (file.read((uint8_t*)&v, sizeof(v)) != sizeof(v)) {
+      LOG_ERROR("Reading LUT data failed");
+      file.close();
+      return false;
+    }
+    lut.set_entry(i, v);
+  }
+
+  file.close();
+  // LOG_DEBUG("Loading Lookup table from file successful");
+
+  return true;
+}
diff --git a/firmware/MotionControllerRP/src/servo_control/encoder_lut.h b/firmware/MotionControllerRP/src/utilities/lookup_table.h
similarity index 51%
rename from firmware/MotionControllerRP/src/servo_control/encoder_lut.h
rename to firmware/MotionControllerRP/src/utilities/lookup_table.h
index 6e5d5c4..456c4a1 100644
--- a/firmware/MotionControllerRP/src/servo_control/encoder_lut.h
+++ b/firmware/MotionControllerRP/src/utilities/lookup_table.h
@@ -11,25 +11,35 @@
 #include <cstdint>
 #include <cmath>
 
+//*** CLASS *****************************************************************************
 
 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);
+    bool init(int32_t table_size, float input_min, float input_max);
+
+    // initializes the lookup table from a list of input output value pairs
+    // approximation/interpolation will be used to sample the input range in
+    // equidistant steps. 
+    bool init_interpolating(std::vector<std::pair<float, float>> in_out_pairs, 
+                            int table_size, bool sort_input);
+    
+    // optimizes the lookup table using gradient descen to minimize error to provided data
+    bool optimize_lut(std::vector<std::pair<float, float>> in_out_pairs);
 
     // clear the lookup table, use init to use it again
-    void  clear();
+    void clear();
 
     // returns the size of the lookup table
-    uint32_t size();
+    uint32_t size() const;
 
     // set an entry of the lookup table
-    void  set_entry(int32_t idx, float v);
+    void set_entry(int32_t idx, float v);
 
     // set an entry of the lookup table
-    float  get_entry(int32_t idx);
+    float get_entry(int32_t idx) const;
 
     // evaluate the lookup table at a given position with linear interpolation
     float evaluate(float x) const;
@@ -43,9 +53,21 @@ class LookupTable {
     // check if the lookup table is monotonic
     bool is_monotonic() const;
 
+    // check if the given value is inside the input range
+    bool in_input_range(float x) const;
+
+    // check if the given value is inside the output range
+    bool in_output_range(float x) const;
+
+    // get input range
+    void get_intput_range(float& input_min, float& input_max) const;
+
     // prints the lookup table using the logger
     void print_to_log() const;
 
+  private:
+    void linear_interpolate(float x, int& idx_a, int& idx_b, float& weight_a, float& weight_b) const;
+
   private:
     float input_min = 0.0f;
     float input_max = 0.0f;
@@ -55,3 +77,7 @@ class LookupTable {
 
 //*** FUNCTION ***********************************************************************************/
 
+void build_linear_lut(LookupTable& lut, float in_min, float in_max, float out_min, float out_max);
+bool save_lut_to_file(const LookupTable& lut, const char* filename);
+bool load_lut_from_file(LookupTable& lut, const char* filename);
+
diff --git a/firmware/MotionControllerRP/src/utilities/utilities.cpp b/firmware/MotionControllerRP/src/utilities/utilities.cpp
new file mode 100644
index 0000000..f3e0fd3
--- /dev/null
+++ b/firmware/MotionControllerRP/src/utilities/utilities.cpp
@@ -0,0 +1,36 @@
+// --------------------------------------------------------------------------------------
+// Project: MicroManipulatorStepper
+// License: MIT (see LICENSE file for full description)
+//          All text in here must be included in any redistribution.
+// Author:  M. S. (diffraction limited)
+// --------------------------------------------------------------------------------------
+
+#include <LittleFS.h>
+#include <string>
+
+#include "logging.h"
+#include "utilities.h"
+
+//*** FUNCTIONS *************************************************************************
+
+std::vector<std::string> get_file_list(const char* dirname, bool include_dirs) {
+  std::vector<std::string> file_list;
+
+  File root = LittleFS.open(dirname, "r");
+  if (!root || !root.isDirectory()) {
+    LOG_ERROR("Failed to open directory %s", dirname);
+    return file_list; // return empty vector
+  }
+
+  File file = root.openNextFile();
+  while (file) {
+    if (!file.isDirectory()) {
+      file_list.emplace_back(file.name());
+    } else if(include_dirs) {
+      file_list.emplace_back(std::string("DIR ") + file.name());
+    }
+    file = root.openNextFile();
+  }
+
+  return file_list;
+}
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/utilities/utilities.h b/firmware/MotionControllerRP/src/utilities/utilities.h
new file mode 100644
index 0000000..3416710
--- /dev/null
+++ b/firmware/MotionControllerRP/src/utilities/utilities.h
@@ -0,0 +1,13 @@
+// --------------------------------------------------------------------------------------
+// Project: MicroManipulatorStepper
+// License: MIT (see LICENSE file for full description)
+//          All text in here must be included in any redistribution.
+// Author:  M. S. (diffraction limited)
+// --------------------------------------------------------------------------------------
+
+#include <string>
+#include <vector>
+
+//*** FUNCTIONS *************************************************************************
+
+std::vector<std::string> get_file_list(const char* dirname, bool include_dirs);
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/version.h b/firmware/MotionControllerRP/src/version.h
new file mode 100644
index 0000000..b5f5346
--- /dev/null
+++ b/firmware/MotionControllerRP/src/version.h
@@ -0,0 +1 @@
+static const char* FIRMWARE_VERSION = "v1.0.1";