diff --git a/firmware/MotionControllerRP/Rp2040-Pico_Pinout.png b/firmware/MotionControllerRP/Rp2350-Pico_Pinout.png
similarity index 100%
rename from firmware/MotionControllerRP/Rp2040-Pico_Pinout.png
rename to firmware/MotionControllerRP/Rp2350-Pico_Pinout.png
diff --git a/firmware/MotionControllerRP/src/command_parser/command_parser.cpp b/firmware/MotionControllerRP/src/command_parser/command_parser.cpp
index ab1a002..ed185a9 100644
--- a/firmware/MotionControllerRP/src/command_parser/command_parser.cpp
+++ b/firmware/MotionControllerRP/src/command_parser/command_parser.cpp
@@ -24,6 +24,38 @@ const std::string& GCodeCommand::get_command() const {
   return command;
 }
 
+GCodeCommand::EParseStatus GCodeCommand::from_command_str(const char* gcode_command) {
+  // Copy line into a mutable buffer
+  char line_copy[256];
+  strncpy(line_copy, gcode_command, sizeof(line_copy));
+  line_copy[sizeof(line_copy) - 1] = '\0';
+
+  // Tokenize the first word (e.g., G0, M3, T1)
+  char* saveptr = nullptr;
+  char* token = strtok_r(line_copy, " ", &saveptr);
+  if (!token || token[0] < 'A' || token[0] > 'Z') {
+    return EParseStatus::MALFORMED_COMMAND;
+  }
+
+  reset();
+  set_command(token);
+
+  // Parse remaining words (e.g., X1.0, Y2.5, F200)
+  while ((token = strtok_r(nullptr, " ", &saveptr))) {
+    if (token[0] >= 'A' && token[0] <= 'Z') {
+      if(token[1] == '\0')
+        set_value(token[0], 0.0f);
+      else
+        set_value(token[0], strtof(token + 1, nullptr));
+    } else {
+      return EParseStatus::INVALID_PARAMETER;
+    }
+  }
+
+  return EParseStatus::OK;
+}
+
+
 void GCodeCommand::reset() {
   // Initialize all word values to NaN to represent "not set"
   for (float& value : word_values) {
@@ -116,6 +148,10 @@ void CommandParser::update() {
   }
 }
 
+bool CommandParser::is_command_ready() {
+  return command_ready;
+}
+
 // Feed input chars one by one
 void CommandParser::add_input_character(char c) {
   if (c == '\n' || c == '\r') {
@@ -132,41 +168,19 @@ void CommandParser::add_input_character(char c) {
 bool CommandParser::parse_line(const char* line) {
   command_ready = false;
 
-  // Copy line into a mutable buffer
-  char line_copy[256];
-  strncpy(line_copy, line, sizeof(line_copy));
-  line_copy[sizeof(line_copy) - 1] = '\0';
+  // reset and parse gcode command from string
+  GCodeCommand::EParseStatus ret = command.from_command_str(line);
 
-  // Tokenize the first word (e.g., G0, M3, T1)
-  char* saveptr = nullptr;
-  char* token = strtok_r(line_copy, " ", &saveptr);
-  if (!token || token[0] < 'A' || token[0] > 'Z') {
+  if(ret == GCodeCommand::EParseStatus::MALFORMED_COMMAND) {
     command_processor->send_reply("error: malformed command\n");
     return false;
   }
 
-  command.reset();
-  command.set_command(token);
-
-  // Parse remaining words (e.g., X1.0, Y2.5, F200)
-  while ((token = strtok_r(nullptr, " ", &saveptr))) {
-    if (token[0] >= 'A' && token[0] <= 'Z') {
-      if(token[1] == '\0')
-        command.set_value(token[0], 0.0f);
-      else
-      command.set_value(token[0], strtof(token + 1, nullptr));
-    } else {
-      command_processor->send_reply("error: invalid parameter\n");
-      return false;
-    }
+  if(ret == GCodeCommand::EParseStatus::INVALID_PARAMETER) {
+    command_processor->send_reply("error: invalid parameter\n");
+    return false;
   }
 
   command_ready = true;
   return true;
-}
-
-// returns false if the command could not yet be processed.
-// The function will be called witht the same command later to try again.
-bool CommandParser::handle_gcode_command(const GCodeCommand& cmd) {
-  return false;
-}
+}
\ No newline at end of file
diff --git a/firmware/MotionControllerRP/src/command_parser/command_parser.h b/firmware/MotionControllerRP/src/command_parser/command_parser.h
index 7eea46a..407d149 100644
--- a/firmware/MotionControllerRP/src/command_parser/command_parser.h
+++ b/firmware/MotionControllerRP/src/command_parser/command_parser.h
@@ -16,8 +16,16 @@ static constexpr int LETTER_COUNT = 26;
 
 class GCodeCommand {
   public:
+    enum class EParseStatus {
+      OK=1,
+      MALFORMED_COMMAND=-1,
+      INVALID_PARAMETER=-2
+    };
+
     GCodeCommand();
 
+    EParseStatus        from_command_str(const char* cmd);
+
     void                reset();
     void                set_command(const char* cmd);
     const std::string&  get_command() const;
@@ -52,11 +60,9 @@ class CommandParser {
 
     void set_command_processor(ICommandProcessor* cp);
     void add_input_character(char c);  // Feed input chars one by one
-    void update();  // Feed input chars one by one
-
-  protected:
+    void update();                     
+    bool is_command_ready();  // true if command was parsed and is waiting to be executed
     bool parse_line(const char* line);
-    bool handle_gcode_command(const GCodeCommand& cmd);
 
   private:
     char buffer[255];
diff --git a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
index b76acd6..19dda1d 100644
--- a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
+++ b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
@@ -7,6 +7,7 @@
 #include "hardware/spi.h"
 #include "hardware/gpio.h"
 #include "pico/stdlib.h"
+#include "utilities/logging.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
@@ -17,22 +18,27 @@ void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint
   // 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);
+
+  sleep_ms(10);
 }
 
 MT6835Encoder::MT6835Encoder(spi_inst_t* spi, uint cs_pin) : spi(spi), cs_pin(cs_pin) {
-  // nop
+  if (cs_pin >= 0) {
+      gpio_init(cs_pin);
+      gpio_set_dir(cs_pin, GPIO_OUT);
+      gpio_put(cs_pin, 1);  // CS high
+  }
 }
 
 MT6835Encoder::~MT6835Encoder() {
   // 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
-  }
+bool MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
+  sleep_ms(100);
+
+  if(is_connected() == false)
+    return false;
 
   set_rotation_direction(0);    // needs to be set, otherwise might be random
   set_bandwidth(bandwidth);
@@ -40,6 +46,25 @@ void MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
 
   last_raw_angle = 0;
   abs_raw_angle = 0;
+  initialized = true;
+
+  return true;
+}
+
+bool MT6835Encoder::is_connected() {
+  uint8_t check_bytes[] = {37, 109, 179, 251, 1};
+  for(int i=0; i<sizeof(check_bytes); i++) {
+    write_register(MT6835_REG_USERID, check_bytes[i]);
+    uint8_t user_id = read_register(MT6835_REG_USERID);
+    if(user_id != check_bytes[i])
+      return false;
+  }
+
+  return true;
+}
+
+bool MT6835Encoder::is_initialized() {
+  return initialized;
 }
 
 void MT6835Encoder::reset_abs_angle(int32_t abs_raw_angle) {
diff --git a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
index 62c6084..2a0fed8 100644
--- a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
+++ b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
@@ -145,7 +145,10 @@ class MT6835Encoder {
       MT6835Encoder(spi_inst_t *spi, uint cs_pin);
       virtual ~MT6835Encoder();
 
-      void init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
+      bool init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
+      bool is_connected();
+      bool is_initialized();
+
       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
@@ -198,6 +201,7 @@ class MT6835Encoder {
       bool check_crc = false;
 
   private:
+      bool initialized=false;
       spi_inst_t *spi;
       uint cs_pin;
       uint8_t last_status = 0;
diff --git a/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp b/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
index 3abe215..0457670 100644
--- a/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
+++ b/firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp
@@ -36,6 +36,7 @@ TB6612MotorDriver::TB6612MotorDriver(
 {
   max_pwm = (1 << pwm_resolution) - 1;
   set_amplitude(0.0f, false);
+  field_angle = 0.0f;
 }
 
 void init_output_pin(uint8_t pin, bool value) {
@@ -113,6 +114,9 @@ float TB6612MotorDriver::get_amplitude() const {
 }
 
 void TB6612MotorDriver::rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step) {
+  if(delta_angle == 0.0f)
+    return;
+    
   float start = field_angle;
   float rad_per_µs = (delta_angle >= 0.0f) ? rad_per_s*1e-6f : -rad_per_s*1e-6f;
 
diff --git a/firmware/MotionControllerRP/src/hardware/spi_interface.cpp b/firmware/MotionControllerRP/src/hardware/spi_interface.cpp
new file mode 100644
index 0000000..258b568
--- /dev/null
+++ b/firmware/MotionControllerRP/src/hardware/spi_interface.cpp
@@ -0,0 +1,111 @@
+#include "spi_interface.h"
+
+//*** CLASS *****************************************************************************
+
+SpiDma::SpiDma(spi_inst_t* spi, uint sck, uint mosi, uint miso, uint cs)
+        : spi(spi), sck(sck), mosi(mosi), miso(miso), cs(cs) 
+{
+}
+
+void SpiDma::begin(uint32_t baudrate_hz) {
+  // SPI init
+  spi_init(spi, baudrate_hz);
+  spi_set_format(spi, 8,
+                  SPI_CPOL_0,
+                  SPI_CPHA_0,
+                  SPI_MSB_FIRST);
+
+  gpio_set_function(sck, GPIO_FUNC_SPI);
+  gpio_set_function(mosi, GPIO_FUNC_SPI);
+  gpio_set_function(miso, GPIO_FUNC_SPI);
+
+  // Manual CS
+  gpio_init(cs);
+  gpio_set_dir(cs, GPIO_OUT);
+  gpio_put(cs, 1);
+
+  // DMA TX channel
+  dma_tx = dma_claim_unused_channel(true);
+  cfg_tx = dma_channel_get_default_config(dma_tx);
+  channel_config_set_transfer_data_size(&cfg_tx, DMA_SIZE_8);
+  channel_config_set_dreq(&cfg_tx, spi_get_dreq(spi, true));
+  channel_config_set_read_increment(&cfg_tx, true);
+  channel_config_set_write_increment(&cfg_tx, false);
+
+  // --- DMA RX channel ---
+  dma_rx = dma_claim_unused_channel(true);
+  cfg_rx = dma_channel_get_default_config(dma_rx);
+  channel_config_set_transfer_data_size(&cfg_rx, DMA_SIZE_8);
+  channel_config_set_dreq(&cfg_rx, spi_get_dreq(spi, false));
+  channel_config_set_read_increment(&cfg_rx, false);
+  channel_config_set_write_increment(&cfg_rx, true);
+
+  // Configure but don't start yet
+  dma_channel_configure(
+      dma_tx, &cfg_tx,
+      &spi_get_hw(spi)->dr,
+      nullptr, 0, false);
+
+  dma_channel_configure(
+      dma_rx, &cfg_rx,
+      nullptr,
+      &spi_get_hw(spi)->dr,
+      0, false);
+}
+
+bool SpiDma::is_busy() const {
+    return dma_channel_is_busy(dma_tx) ||
+           dma_channel_is_busy(dma_rx);
+}
+
+void SpiDma::transfer(const void* tx_buf, void* rx_buf, size_t bytes) {
+    while (is_busy()) {}
+
+    gpio_put(cs, 0);
+
+    // Ensure minimum CS low time (50 ns) - Approx 14 cycles at 250 MHz
+    for (int i = 0; i < 14; i++) __asm volatile("nop");
+
+    // Setup RX DMA - must be set first!
+    if (rx_buf != nullptr) {
+        dma_channel_set_write_addr(dma_rx, rx_buf, false);
+        dma_channel_set_trans_count(dma_rx, bytes, false);
+    }
+
+    // Setup TX DMA
+    if (tx_buf != nullptr) {
+        dma_channel_set_read_addr(dma_tx, tx_buf, false);
+        dma_channel_set_trans_count(dma_tx, bytes, true);
+    }
+}
+
+void SpiDma::wait_for_finish() {
+  while (is_busy()) {}
+  gpio_put(cs, 1);
+
+  // Ensure minimum CS low time (50 ns) - Approx 14 cycles at 250 MHz
+  for (int i = 0; i < 14; i++) __asm volatile("nop");
+}
+
+void SpiDma::abort() {
+    // Stop TX DMA if running
+    if (dma_channel_is_busy(dma_tx)) {
+        dma_channel_abort(dma_tx);
+    }
+
+    // Stop RX DMA if running
+    if (dma_channel_is_busy(dma_rx)) {
+        dma_channel_abort(dma_rx);
+    }
+
+    // Flush the SPI FIFOs to prevent leftover data
+    spi_get_hw(spi)->dr = 0;       // clear DR (TX FIFO)
+    spi_get_hw(spi)->sr;           // read SR to clear flags
+
+    // Deassert CS pin to leave SPI slave in idle state
+    gpio_put(cs, 1);
+}
+
+
+
+
diff --git a/firmware/MotionControllerRP/src/hardware/spi_interface.h b/firmware/MotionControllerRP/src/hardware/spi_interface.h
new file mode 100644
index 0000000..5fb8686
--- /dev/null
+++ b/firmware/MotionControllerRP/src/hardware/spi_interface.h
@@ -0,0 +1,37 @@
+// --------------------------------------------------------------------------------------
+// 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 "hardware/spi.h"
+#include "hardware/dma.h"
+#include "hardware/gpio.h"
+
+//*** CLASS *****************************************************************************
+
+class SpiDma {
+  public:
+      SpiDma(spi_inst_t* spi, uint sck, uint mosi, uint miso, uint cs);
+
+      void begin(uint32_t baudrate_hz);
+      bool is_busy() const;
+      void transfer(const void* tx_buf, void* rx_buf, size_t bytes);
+      void wait_for_finish();
+      void abort();
+
+  private:
+      spi_inst_t* spi;
+      uint sck, mosi, miso, cs;
+
+      int dma_tx;
+      int dma_rx;
+      dma_channel_config cfg_tx;
+      dma_channel_config cfg_rx;
+};
+
+
+
diff --git a/firmware/MotionControllerRP/src/hw_config.h b/firmware/MotionControllerRP/src/hw_config.h
index 6869428..c396748 100644
--- a/firmware/MotionControllerRP/src/hw_config.h
+++ b/firmware/MotionControllerRP/src/hw_config.h
@@ -1,14 +1,20 @@
 #pragma once
 #include "utilities/math_constants.h"
 
+// #define DEMO_MODE
+
 //--- MOTORS ------------------------------------------------------------------
 
 // motor pole pair count
 //  * 100 for 0.9deg stepper motors
 //  * 50  for 1.8deg stepper motors
-constexpr float MOTOR1_POLE_PAIRS = 100;
-constexpr float MOTOR2_POLE_PAIRS = 100;
-constexpr float MOTOR3_POLE_PAIRS = 100;
+constexpr float MOTOR1_POLE_PAIRS = 50;
+constexpr float MOTOR2_POLE_PAIRS = 50;
+constexpr float MOTOR3_POLE_PAIRS = 50;
+
+// max current factor in range [0..1]. Lower values reduce pwm resolution so a
+// value above 0.4 is recommended.
+constexpr float MOTOR_MAX_CURRENT_FACTOR = 0.6f;
 
 //--- ENCODERS ----------------------------------------------------------------
 
@@ -28,18 +34,44 @@ constexpr float HOMING_FINISH_POS = 0.5f;        // in rad
 //--- CALIBRATION -------------------------------------------------------------
 
 // degrees from home position
-constexpr float CALIBRATION_RANGE = 95; 
+constexpr float CALIBRATION_RANGE = 83; 
 
 // velocity of the magnetic field during calibration (lower is more accurate)
-constexpr float CALIBRATION_FIELD_VELOCITY = 40.0f; 
+constexpr float CALIBRATION_FIELD_VELOCITY = 20.0f; 
+
+// size of the calibration lookup table
+constexpr int ENCODER_LUT_SIZE = 256;
+
+//--- CLOSED LOOP CONTROL -----------------------------------------------------
+
+// position controller 
+constexpr float POS_KP = 60.0f;
+constexpr float POS_KI = 30000.0f;
+
+// velocity controller
+constexpr float VEL_LOWPASS_TC = 0.004f;
+constexpr float VEL_KP = 0.2f;
+constexpr float VEL_KI = 90.0f;
+
+//--- KINEMATIC ---------------------------------------------------------------
+
+// Kinematic Parameters are defined kinematic_modes/kinematic_model_delta3d.cpp
+
+// NUM_JOINTS and NUM_TOOLS are defined in 'path_segment.h'. Note that changing
+// the number of joints requires changing the kinematic model accordingly and
+// also requires the initialization of the correct number of 'RobotJoint' objects
+// in the Robtos init method.
 
 //--- PINS --------------------------------------------------------------------
 
+#define JOINT_READY_OVERRIDE
+
 // #define SINGLE_AXIS_BOARD
 #ifndef SINGLE_AXIS_BOARD
   // Pins for 3Axis Board
-  #define PIN_BUILTIN_LED 23
-  #define PIN_USER_BUTTON 24
+  // #define PIN_BUILTIN_LED 23 // RP2040 pico clone
+  // #define PIN_USER_BUTTON 24 // RP2040 pico clone
+  #define PIN_BUILTIN_LED 25
 
   #define PIN_M1_PWM_A_POS  13
   #define PIN_M1_PWM_A_NEG  12
@@ -67,6 +99,9 @@ constexpr float CALIBRATION_FIELD_VELOCITY = 40.0f;
   #define PIN_ENCODER_MISO 0
   #define PIN_ENCODER_MOSI 3
 
+  #define PIN_TOOL1 16
+  #define PIN_TOOL2 17
+
 #else
   // Single Axis Board
   #define PIN_BUILTIN_LED 16
diff --git a/firmware/MotionControllerRP/src/kinematic_models/kinematic_model_delta3d.cpp b/firmware/MotionControllerRP/src/kinematic_models/kinematic_model_delta3d.cpp
index 4e6c57b..d86cfd4 100644
--- a/firmware/MotionControllerRP/src/kinematic_models/kinematic_model_delta3d.cpp
+++ b/firmware/MotionControllerRP/src/kinematic_models/kinematic_model_delta3d.cpp
@@ -14,11 +14,60 @@
 
 //*** CLASS *****************************************************************************
 
+#define HW_VERSION4 // remove this if you are using HW-v3 (Note geometry parameters might be slightly off for HW-v3.0!)
+
+#ifdef HW_VERSION4
+
+/**
+ * Initializes the kinematic model and its geometric parameters for hardware version v4.0.
+ */
+KinematicModel_Delta3D::KinematicModel_Delta3D() {
+  const float D2R = Constants::DEG2RAD;
+
+  // location of base origin in endeffector coordinate system (ee in neutral position)
+  Vec3F base_offset(-47.5f, -47.5f, -47.5f);
+  
+  // distance between the two spehere centers of the linkage rod (arms)
+  arm_length = 36.25f*2;
+
+  // distance from arm attachment points to rotor axis
+  rotor_radius = 15.0f;
+
+  // midpoint between the two attachment spheres on the endeffector in EE coordinate system
+  ee_attachment_points[0] = Vec3F(3.54f, -11.5f, 4.5f);
+  ee_attachment_points[1] = Vec3F(4.5f, 3.54f, -11.5f);
+  ee_attachment_points[2] = Vec3F(-11.5f, 4.5f, 3.54f);
+
+  // set actuator transformations based on CAD model
+  actuator_to_base[0].rotation = QuaternionF::from_axis_angle(Vec3F(0.0f, 0.0f, 1.0f), 90.0f*D2R);
+  actuator_to_base[0].translation = Vec3F(-21.5f, 21.0f, 52.0f)+base_offset;
+
+  actuator_to_base[1].rotation = QuaternionF::from_axis_angle(Vec3F(1.0f, 0.0f, 1.0f), 180.0f*D2R);
+  actuator_to_base[1].translation = Vec3F(52.0f, -21.5f, 21.0f)+base_offset;
+
+  actuator_to_base[2].rotation = QuaternionF::from_axis_angle(Vec3F(-1.0f, 0.0f, 0.0f), 90.0f*D2R);
+  actuator_to_base[2].translation = Vec3F(21.0f, 52.0f, -21.5f)+base_offset;
+  
+  // angle offset from home positionto center position of the rotor
+  rotor_angle_offset[0] = 42.0f*Constants::DEG2RAD;
+  rotor_angle_offset[1] = 42.0f*Constants::DEG2RAD;
+  rotor_angle_offset[2] = 42.0f*Constants::DEG2RAD;
+
+  for(int i=0; i<3; i++)
+    base_to_actuator[i] = actuator_to_base[i].inverse();
+}
+
+#else
+
+// Below is the old code for HW-v3.0 for compatibility, some values might be wrong and do not reflect the actual cad model.
+// If you use them please check them against your build. All these problems where fixed in HW-v4 and the new function above
+// uses the correct values.
 KinematicModel_Delta3D::KinematicModel_Delta3D() {
   const float D2R = Constants::DEG2RAD;
 
   // offset to move base origin defined in CAD to endeffector origin near neutral position
-  // real device
+  
+  // REAL DEVICE <--- These might be slightly wrong
   Vec3F base_offset(-32.5f, -32.5f, -32.5f);
   arm_length = 73.8f;
   rotor_radius = 15.0f;
@@ -26,8 +75,7 @@ KinematicModel_Delta3D::KinematicModel_Delta3D() {
   ee_attachment_points[1] = Vec3F(2.0f, -0.5f, -14.5f);
   ee_attachment_points[2] = Vec3F(-14.5f, 2.0f, -0.5f);
 
-  // endeffector attachment points
-  // CAD
+  // CAD <--- These might be be more correct but not sure
   /*
   Vec3F base_offset(-30.5f, -30.5f, -30.5f);
   arm_length = 2*36.5;
@@ -55,6 +103,8 @@ KinematicModel_Delta3D::KinematicModel_Delta3D() {
     base_to_actuator[i] = actuator_to_base[i].inverse();
 }
 
+#endif
+
 int KinematicModel_Delta3D::get_joint_count() {
   return 3;
 }
@@ -65,7 +115,7 @@ bool KinematicModel_Delta3D::foreward(const float* joint_positions, Pose6DF& pos
     Vec3F p = arm_attachment_point(i, joint_positions[i]);
     p = actuator_to_base[i].transformPoint(p);
 
-    // apply ee attachment point offsets offset so that three sphere intersection can be used 
+    // apply ee attachment point offsets so that three sphere intersection can be used 
     // to find ee position. This only works if there is no ee rotation.
     arm_attachment_points[i] = p-ee_attachment_points[i];
   }
diff --git a/firmware/MotionControllerRP/src/main.cpp b/firmware/MotionControllerRP/src/main.cpp
index 5e2a6ec..b55139c 100644
--- a/firmware/MotionControllerRP/src/main.cpp
+++ b/firmware/MotionControllerRP/src/main.cpp
@@ -11,7 +11,6 @@
 #include "hardware/pll.h"
 #include "hardware/vreg.h"
 
-#include <NeoPixelConnect.h>
 #include <Wire.h>
 #include <algorithm>
 
@@ -23,9 +22,11 @@
 #include "hw_config.h"
 #include "LittleFS.h"
 
+#include "demo_gcode_generator.h"
+
 //*** GLOBALS ***************************************************************************
 
-NeoPixelConnect strip(PIN_BUILTIN_LED, 1);
+// NeoPixelConnect strip(PIN_BUILTIN_LED, 1);
 Robot robot(0.01f);
 
 /*
@@ -68,16 +69,18 @@ void overclock() {
 }
 
 void set_led_color(uint8_t r, uint8_t g, uint8_t b) {
-  strip.neoPixelSetValue(0, r, g, b, false);
+ /* strip.neoPixelSetValue(0, r, g, b, false);
   delayMicroseconds(2000);
-  strip.neoPixelShow();
+  strip.neoPixelShow(); */
 }
 
 void led_blink(uint8_t r, uint8_t g, uint8_t b, int count, int period_time_ms) {
   for(int i=0; i<count; i++) {
-    set_led_color(r, g, b);
+    gpio_put(PIN_BUILTIN_LED, 1);
+    // set_led_color(r, g, b);
     sleep_ms(period_time_ms/2);
-    set_led_color(0, 0, 0);
+    // set_led_color(0, 0, 0);
+    gpio_put(PIN_BUILTIN_LED, 0);
     sleep_ms(period_time_ms/2);
   }
 }
@@ -85,6 +88,11 @@ void led_blink(uint8_t r, uint8_t g, uint8_t b, int count, int period_time_ms) {
 void main_core0() {
   uint64_t last_time = time_us_64();
 
+  #ifdef DEMO_MODE
+    auto* demo_gcode_generator = new DemoGcodeGenerator(&robot);
+    demo_gcode_generator->run();  // blocks forever
+  #endif
+
   while(true) {
     // update motion controller
     robot.update_command_parser();
@@ -111,12 +119,17 @@ void main_core1() {
 }
 
 void setup() {
+  gpio_init(PIN_BUILTIN_LED);
+  gpio_set_dir(PIN_BUILTIN_LED, GPIO_OUT);
+  
   led_blink(0, 0, 30, 3, 100);
   // stdio_init_all();  // Initializes USB or UART stdio
   overclock();
   // Serial.begin(921600);
   Logger::instance().begin(921600, false);
+  #ifndef DEMO_MODE
   while(!Serial);
+  #endif
 
   set_led_color(50, 10, 0);
   // auto* test = new KinematicModel_Delta3D(); test->test(); delete test;
@@ -146,6 +159,8 @@ void setup() {
   LOG_INFO("Initialization finished");
   LOG_INFO(" ");
 
+  gpio_put(PIN_BUILTIN_LED, 1);
+
   return;
 
   /*
diff --git a/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp b/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
index d9ebfa5..abfe634 100644
--- a/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
+++ b/firmware/MotionControllerRP/src/motion_control/motion_controller.cpp
@@ -18,7 +18,10 @@ MotionController::MotionController(PathPlanner* path_planner) {
   current_time = 0.0f;
 }
 
-bool MotionController::update(float dt, float* joint_positions, float* joint_velocities) {
+bool MotionController::update(float dt, 
+                              float* joint_positions,
+                              float* joint_velocities,
+                              float* tool_outputs) {
   // increment time counter
   current_time += dt;
 
@@ -49,6 +52,6 @@ bool MotionController::update(float dt, float* joint_positions, float* joint_vel
     current_path_segment.initialized = false;
 
   // evaluate path segment
-  current_path_segment.evaluate(current_time, joint_positions, joint_velocities);
+  current_path_segment.evaluate(current_time, joint_positions, joint_velocities, tool_outputs);
   return true;
 }
diff --git a/firmware/MotionControllerRP/src/motion_control/motion_controller.h b/firmware/MotionControllerRP/src/motion_control/motion_controller.h
index 263c55c..d046d7b 100644
--- a/firmware/MotionControllerRP/src/motion_control/motion_controller.h
+++ b/firmware/MotionControllerRP/src/motion_control/motion_controller.h
@@ -23,7 +23,7 @@ class MotionController {
 
     // updates the motion controller and computes new joint positions and velocities
     // after dt has passed. Ouput array must hav space for 'NUM_JOINTS' entries.
-    bool update(float dt, float* joint_positions, float* joint_velocities);
+    bool update(float dt, float* joint_positions, float* joint_velocities, float* tool_outputs);
 
   private:
     PathPlanner* path_planner;
diff --git a/firmware/MotionControllerRP/src/motion_control/path_segment.cpp b/firmware/MotionControllerRP/src/motion_control/path_segment.cpp
index f9adcf0..74fbe82 100644
--- a/firmware/MotionControllerRP/src/motion_control/path_segment.cpp
+++ b/firmware/MotionControllerRP/src/motion_control/path_segment.cpp
@@ -112,12 +112,15 @@ float MotionProfileConstAcc::evaluate(float time) const {
 
 CartesianPathSegment::CartesianPathSegment() {
   dwell_time = 0.0f;
+  for(int i=0; i<NUM_TOOLS; i++)
+    tool_outputs[i] = 0.0f;
 }
 
 CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
                                            const Pose6DF& end_pose,
                                            const LinearAngular& target_velocity,
-                                           const LinearAngular& max_acceleration)
+                                           const LinearAngular& max_acceleration,
+                                           const float tool_outputs[NUM_TOOLS])
 {
   CartesianPathSegment::dwell_time = 0.0f;
   CartesianPathSegment::start_pose = start_pose;
@@ -134,6 +137,8 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
 
   translation_delta_normalized = translation_delta.normalized();
 
+  for(int i=0; i<NUM_TOOLS; i++)
+    CartesianPathSegment::tool_outputs[i] = tool_outputs[i];
 
   /*
   QuaternionF rotation_delta = (end_pose.rotation * start_pose.rotation.normalized_inverse());
@@ -143,7 +148,9 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
   rotation_delta_axis = axis; */
 }
 
-CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
+CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose,
+                                           const float tool_outputs[NUM_TOOLS], 
+                                           float dwell_time)
 {
   CartesianPathSegment::dwell_time = dwell_time;
   CartesianPathSegment::start_pose = pose;
@@ -156,6 +163,9 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time
 
   travel_distance.linear = 0.0f;
   travel_distance.angular = 0.0f;
+
+  for(int i=0; i<NUM_TOOLS; i++)
+    CartesianPathSegment::tool_outputs[i] = tool_outputs[i];
 }
 
 
@@ -209,6 +219,7 @@ JointSpacePathSegment::JointSpacePathSegment() {
 JointSpacePathSegment::JointSpacePathSegment(
   const float start_pos[NUM_JOINTS],
   const float end_pos[NUM_JOINTS],
+  const float tool_outputs[NUM_TOOLS],
   float duration)
 {
   JointSpacePathSegment::duration = duration;
@@ -219,20 +230,31 @@ JointSpacePathSegment::JointSpacePathSegment(
     JointSpacePathSegment::end_pos[i] = end_pos[i];
   }
 
+  for(int i=0; i<NUM_TOOLS; i++) {
+    JointSpacePathSegment::tool_outputs[i] = tool_outputs[i];
+  }
+
   initialized = true;
 }
 
 void JointSpacePathSegment::evaluate(
   float time, 
   float joint_positions[NUM_JOINTS], 
-  float joint_velocity[NUM_JOINTS]) const
+  float joint_velocity[NUM_JOINTS],
+  float tool_outputs[NUM_TOOLS]) const
 {
   float t = time*inv_duration;
   float s = 1.0f-t;
 
+  // perform linear interpolation
   for(int i=0; i<NUM_JOINTS; i++) {
     joint_positions[i] = start_pos[i]*s + end_pos[i]*t;
-    joint_velocity[i] = 0;
+    joint_velocity[i] = 0; // velocity currently not computed (TODO)
+  }
+
+  // copy tool putput (tool outputs are not interpolated)
+  for(int i=0; i<NUM_TOOLS; i++) {
+    tool_outputs[i] = JointSpacePathSegment::tool_outputs[i];
   }
 }
 
@@ -294,7 +316,10 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
   
   // create joint space path segment
   float duration = current_time-initial_time;
-  js_path_segment = JointSpacePathSegment(current_joint_pos, next_joint_pos, duration);
+  js_path_segment = JointSpacePathSegment(current_joint_pos, 
+                                          next_joint_pos,
+                                          path_segment->tool_outputs,
+                                          duration);
 
   // update current joint pos
   for(int i=0; i<NUM_JOINTS; i++)
diff --git a/firmware/MotionControllerRP/src/motion_control/path_segment.h b/firmware/MotionControllerRP/src/motion_control/path_segment.h
index 0f793c1..ff8ef4f 100644
--- a/firmware/MotionControllerRP/src/motion_control/path_segment.h
+++ b/firmware/MotionControllerRP/src/motion_control/path_segment.h
@@ -14,6 +14,7 @@
 //*** CONST *****************************************************************************
 
 constexpr int NUM_JOINTS = 3;
+constexpr int NUM_TOOLS = 2;
 
 //*** CLASS *****************************************************************************
 
@@ -63,11 +64,16 @@ class MotionProfileConstAcc {
 class CartesianPathSegment {
   public:
     CartesianPathSegment();
+
     CartesianPathSegment(const Pose6DF& start_pose,
                          const Pose6DF& end_pose,
                          const LinearAngular& velocity,
-                         const LinearAngular& max_acceleration);
-    CartesianPathSegment(const Pose6DF& pose,float dwell_time);
+                         const LinearAngular& max_acceleration,
+                         const float tool_outputs[NUM_TOOLS]);
+
+    CartesianPathSegment(const Pose6DF& pose, 
+                         const float tool_outputs[NUM_TOOLS],
+                         float dwell_time);
 
     void evaluate(float time, Pose6DF& pose) const;
     float get_duration() const;
@@ -90,6 +96,7 @@ class CartesianPathSegment {
     MotionProfileConstAcc motion_profile;
 
     float dwell_time;  // stay at start position for given duration if dwell_time > 0
+    float tool_outputs[NUM_TOOLS];
 };
 
 //--- JointSpacePathSegment -------------------------------------------------------------
@@ -100,11 +107,13 @@ class JointSpacePathSegment {
     JointSpacePathSegment();
     JointSpacePathSegment(const float start_pos[NUM_JOINTS],
                           const float end_pos[NUM_JOINTS],
+                          const float tool_outputs[NUM_TOOLS],
                           const float duration);
 
     void evaluate(float time, 
                   float joint_positions[NUM_JOINTS], 
-                  float joint_velocity[NUM_JOINTS]) const;
+                  float joint_velocity[NUM_JOINTS],
+                  float tool_outputs[NUM_TOOLS]) const;
 
     float get_duration();
 
@@ -116,6 +125,7 @@ class JointSpacePathSegment {
     float end_pos[NUM_JOINTS];
     float start_velocity[NUM_JOINTS];
     float end_velocity[NUM_JOINTS];
+    float tool_outputs[NUM_TOOLS];
 
     float duration;
     float inv_duration;
diff --git a/firmware/MotionControllerRP/src/robot.cpp b/firmware/MotionControllerRP/src/robot.cpp
index 7a113ad..e3aaf44 100644
--- a/firmware/MotionControllerRP/src/robot.cpp
+++ b/firmware/MotionControllerRP/src/robot.cpp
@@ -13,12 +13,17 @@
 #include "kinematic_models/kinematic_model_delta3d.h"
 #include "servo_control/homing_controller.h"
 #include "servo_control/actuator_calibration.h"
+#include "robot_joint/robot_joint.h"
 #include "pico/multicore.h"
 #include "version.h"
+#include "robot_tool/pwm_tool.h"
 
 constexpr int SPINLOCK_ID_SHARED_DATA = 0;
 constexpr int SPINLOCK_ID_JOINTS = 1;
 
+#include <NeoPixelConnect.h>
+NeoPixelConnect led(PIN_BUILTIN_LED, 1);
+
 //*** FUNCTION **************************************************************************
 
 bool startswith(const std::string& str, const std::string& prefix) {
@@ -28,123 +33,6 @@ bool startswith(const std::string& str, const std::string& prefix) {
 
 //*** CLASS *****************************************************************************
 
-//--- RobotAxis -------------------------------------------------------------------------
-
-RobotJoint::RobotJoint(MT6835Encoder* encoder, 
-                       TB6612MotorDriver* motor_driver,
-                       int pole_pairs) 
-{
-  RobotJoint::encoder = encoder;
-  RobotJoint::motor_driver = motor_driver;
-  servo_controller = new ServoController(*motor_driver, *encoder, pole_pairs);
-  position = 0.0f;
-  velocity = 0.0f;
-}
-
-RobotJoint::~RobotJoint() {
-  delete servo_controller;
-  delete motor_driver;
-  delete encoder;
-  servo_controller = nullptr;
-  motor_driver = nullptr;
-  encoder = nullptr;
-}
-
-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);
-}
-
-bool RobotJoint::calibrate(bool print_measurements) {
-  LOG_INFO("Joint-%i: calibrating joint...", joint_idx);
-
-  HomingController homing_controller;
-  bool homing_ok = homing_controller.run_blocking(servo_controller, -HOMING_VELOCITY, 
-                                                  360.0f*DEG_TO_RAD, HOMING_CURRENT,
-                                                  ENCODER_ANGLE_TO_ROTOR_ANGLE);
-  if(homing_ok == false) {
-    LOG_ERROR("Joint-%i: Calibration failed due to unsuccessful homing sequence", joint_idx);
-    return false;
-  }
-
-  // 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,
-                                     print_measurements);
-  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) {
-  servo_controller->update(position, dt, one_over_dt);
-}
-
-void RobotJoint::update_target(float p, float v) {
-  position = p;
-  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) : 
@@ -167,6 +55,9 @@ Robot::Robot(float path_segment_time_step) :
   max_acceleration = LinearAngular(500.0f, 50.0f);
   path_buffering_time_us = 50*1e3;
 
+  for(int i=0; i<NUM_TOOLS; i++)
+    robot_tools[i] = nullptr;
+
   state = ERobotState::IDLE;
 }
 
@@ -220,6 +111,13 @@ void Robot::init() {
     joints[i]->load_calibration();
   }
 
+  // add tools
+  robot_tools[0] = new PwmTool();
+  ((PwmTool*)robot_tools[0])->init(PIN_TOOL1, 8000, 8);
+
+  robot_tools[1] = new PwmTool();
+  ((PwmTool*)robot_tools[1])->init(PIN_TOOL2, 8000, 8);
+
   // setup timer for updating the motion controller (which evaluates joint space path
   // segments and produces the current target position for the servo loops)
   float motion_controller_update_time_us = 500;
@@ -280,13 +178,14 @@ void Robot::update_path_planner() {
 }
 
 /**
- * Updates the motion controller with a timer interrupt in regular intervals.
+ * Updates the motion controller with a timer interrupt in regular intervals (e.g. 2kHz).
  * The function evaluates joint space path segments and produces the current 
  * target position for the servo loops.
  */
 bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
   float joint_positions[NUM_JOINTS];
   float joint_velocities[NUM_JOINTS];
+  float tool_outputs[NUM_TOOLS];
 
   // get robot pointer
   Robot* robot = (Robot*)timer->user_data;
@@ -296,8 +195,8 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
   float dt = float(time_us - robot->last_mc_update_time)*1e-6f;
   robot->last_mc_update_time = time_us;
 
-  // get current joint position/velocity
-  bool update_ok = robot->motion_controller.update(dt, joint_positions, joint_velocities);
+  // get current joint position/velocity and tool outputs
+  bool update_ok = robot->motion_controller.update(dt, joint_positions, joint_velocities, tool_outputs);
 
   // 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)) {
@@ -308,6 +207,16 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
     spin_unlock_unsafe(robot->shared_data.lock);
   }
 
+  // update tool outputs
+  if(update_ok) {
+    auto& tools = robot->robot_tools;
+    for(int i=0; i<tools.size(); i++) {
+      auto* tool = tools[i];
+      if(tool != nullptr)
+        tool->set_value(tool_outputs[i]);
+    }
+  }
+
   // update frequency counter
   robot->motion_controller_frequency_counter.update(dt);
 
@@ -315,7 +224,7 @@ bool Robot::update_motion_controller_isr(repeating_timer_t* timer) {
 }
 
 /**
- * update servo loops, this is called from a second cpu core
+ * update servo loops, this is called from the second cpu core
  */
 void Robot::update_servo_controllers(float dt) {
   float one_over_dt = 1.0f/dt;
@@ -394,6 +303,10 @@ Pose6DF Robot::pose_from_joint_angles() {
   return pose;
 }
 
+CommandParser* Robot::get_command_parser() {
+  return &command_parser;
+}
+
 bool Robot::check_all_joints_ready() {
   bool all_ready = true;
   for(int i=0; i<NUM_JOINTS; i++) {
@@ -537,6 +450,12 @@ void Robot::process_command(const GCodeCommand& cmd, std::string& reply) {
 void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
   reply = "";
 
+  // process tool output command
+  if(cmd.get_command() == "M3") {
+    process_tool_output_command(cmd, reply);
+    return;
+  }
+
    // enable motors
   if(cmd.get_command() == "M17") {
     // read current pose from HW and set it as current pose
@@ -550,6 +469,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     spin_unlock_unsafe(joints_spin_lock);
 
     reply = "ok\n";
+    return;
   }
 
   // disable motors
@@ -560,6 +480,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     spin_unlock_unsafe(joints_spin_lock);
     
     reply = "ok\n";
+    return;
   }
 
   // get current internal position (not using encoders to read physical position)
@@ -568,6 +489,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     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";
+    return;
   }
 
     // get current internal position (not using encoders to read physical position)
@@ -579,6 +501,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
                std::to_string(angle) + " deg  (raw="+std::to_string(raw_angle)+")\n";
     }
     reply += "ok\n";
+    return;
   }
 
   // get planner queue size
@@ -586,6 +509,7 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     int s = path_planner.input_queue_size();
     reply += std::string("Queue Size: ") + std::to_string(s) + "\n";
     reply += "ok\n";
+    return;
   }
 
   // check if all planned motions are finished executing
@@ -593,16 +517,19 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     bool f = path_planner.all_finished();
     reply += f ? "1\n" : "0\n";
     reply += "ok\n";
+    return;
   }
 
   // set servo loop parameters
   if(cmd.get_command() == "M55") {
     process_set_servo_parameter_command(cmd, reply);
+    return;
   }
 
   // calibrate joint
   if(cmd.get_command() == "M56") {
     process_calibrate_joint_command(cmd, reply);
+    return;
   }
 
   // get info
@@ -624,23 +551,29 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     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";
 
+    for(int i=0; i<NUM_TOOLS; i++)
+        reply += std::string("Tool[") + std::to_string(i) + "] output: " + std::to_string(current_tool_outputs[i]) + "\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";
+    return;
   }
 
   // get firmware version
   if(cmd.get_command() == "M58") {
     reply = std::string(FIRMWARE_VERSION)+"\n";
     reply += "ok\n";
+    return;
   }
 
   // print lookup table
   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();
+    return;
   }
 
   // set linear and angular acceleration
@@ -648,16 +581,20 @@ void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply)
     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";
+    return;
   }
 }
 
 void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
   Pose6DF end_pose;
   
+  #ifndef JOINT_READY_OVERRIDE
   if(!all_joints_ready) {
     reply = "error: not all joints calibrated and homed\n";
     return;
   }
+  #endif
+  
   if(path_planner.input_queue_full()) {
     reply = "busy\n";
     return;
@@ -684,9 +621,11 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
   }
 
   // create path segment
-  CartesianPathSegment path_segment(current_pose, end_pose, 
+  CartesianPathSegment path_segment(current_pose, 
+                                    end_pose, 
                                     current_feedrate, 
-                                    max_acceleration);
+                                    max_acceleration,
+                                    current_tool_outputs);
 
   bool ok = path_planner.add_cartesian_path_segment(path_segment);
   if(ok) {
@@ -698,6 +637,10 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
   }
 }
 
+/**
+ * Immediately sets the current pose without creating path segments or 
+ * interpolating from current position. Useful for external realtime controll.
+ */
 void Robot::process_set_pose_command(const GCodeCommand& cmd, std::string& reply) {
   Pose6DF pose;
 
@@ -725,10 +668,17 @@ void Robot::process_set_pose_command(const GCodeCommand& cmd, std::string& reply
 }
 
 void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
+  #ifndef JOINT_READY_OVERRIDE
   if(!all_joints_ready) {
     reply = "error: not all joints calibrated and homed\n";
     return;
   }
+  #endif
+
+  if(path_planner.input_queue_full()) {
+    reply = "busy\n";
+    return;
+  }
 
   // get dwell time
   float dwell_time = 1.0f;
@@ -736,11 +686,15 @@ void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
   if(cmd.has_word('P')) dwell_time = cmd.get_value('P')*0.001f;   // time given in milliseconds
 
   // create path segment
-  CartesianPathSegment path_segment(current_pose, dwell_time);
-  path_planner.add_cartesian_path_segment(path_segment);
-  path_planner.run_look_ahead_planning();
+  CartesianPathSegment path_segment(current_pose, current_tool_outputs, dwell_time);
+  bool ok = path_planner.add_cartesian_path_segment(path_segment);
 
-  reply = "ok\n";
+  if(ok) {
+    path_planner.run_look_ahead_planning();
+    reply = "ok\n";
+  } else {
+    reply = "error\n";
+  }
 }
 
 void Robot::process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply) {
@@ -798,3 +752,19 @@ void Robot::process_calibrate_joint_command(const GCodeCommand& cmd, std::string
   bool ok = calibrate_joint(idx, store_calibration, print_measurements);
   reply = ok ? "ok\n" : "error\n";
 }
+
+void Robot::process_tool_output_command(const GCodeCommand& cmd, std::string& reply) {
+  // get tool index
+  int tool_index = (int)cmd.get_value('T', 0);
+  if(tool_index < 0 || tool_index >= NUM_TOOLS) {
+    reply = "error: Tool index out of range\n";
+    return;
+  }
+
+  // set current tool output value
+  float tool_value = cmd.get_value('S', 0.0f);
+  current_tool_outputs[tool_index] = tool_value;
+
+  reply = "ok\n";
+}
+
diff --git a/firmware/MotionControllerRP/src/robot.h b/firmware/MotionControllerRP/src/robot.h
index c2e917a..cf40123 100644
--- a/firmware/MotionControllerRP/src/robot.h
+++ b/firmware/MotionControllerRP/src/robot.h
@@ -20,17 +20,11 @@
 #include "motion_control/motion_controller.h"
 #include "command_parser/command_parser.h"
 
-constexpr int ENCODER_LUT_SIZE = 256; 
-
-//*** CALSS *****************************************************************************
+//*** CLASS *****************************************************************************
 
 class Robot;
-
-//--- PersistentData --------------------------------------------------------------------
-
-struct PersistentRobotData {
-  float encoder_lut[NUM_JOINTS][ENCODER_LUT_SIZE];
-};
+class RobotJoint;
+class IRobotTool;
 
 //--- SharedData ------------------------------------------------------------------------
 
@@ -54,36 +48,6 @@ struct SharedData {
   spin_lock_t* lock = nullptr;
 };
 
-//--- RobotJoint ------------------------------------------------------------------------
-
-class RobotJoint {
-  public:
-    RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
-    ~RobotJoint();
-
-    void init(int joint_idx);
-    bool calibrate(bool print_measurements);
-    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;
-
-    MT6835Encoder* encoder;
-    TB6612MotorDriver* motor_driver;
-    ServoController* servo_controller;
-};
-
 //--- Robot -----------------------------------------------------------------------------
 
 class Robot : public ICommandProcessor {
@@ -103,6 +67,8 @@ class Robot : public ICommandProcessor {
 
     void set_pose(const Pose6DF& pos);
     Pose6DF pose_from_joint_angles();
+  
+    CommandParser* get_command_parser();
 
   public:
     void send_reply(const char* str) override;
@@ -116,6 +82,7 @@ class Robot : public ICommandProcessor {
     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);
+    void process_tool_output_command(const GCodeCommand& cmd, std::string& reply);
 
   protected:
     bool check_all_joints_ready();   // checks if all joints are homed and calibrated
@@ -136,6 +103,7 @@ class Robot : public ICommandProcessor {
     CommandParser command_parser;
 
     Pose6DF current_pose;
+    float current_tool_outputs[NUM_TOOLS];
     LinearAngular max_acceleration;
     LinearAngular current_feedrate;
 
@@ -146,4 +114,6 @@ class Robot : public ICommandProcessor {
 
     FrequencyCounter servo_loop_frequency_counter;
     FrequencyCounter motion_controller_frequency_counter;
+
+    std::array<IRobotTool*, NUM_TOOLS> robot_tools;
 };
diff --git a/firmware/MotionControllerRP/src/robot_joint/robot_joint.cpp b/firmware/MotionControllerRP/src/robot_joint/robot_joint.cpp
new file mode 100644
index 0000000..b343a1f
--- /dev/null
+++ b/firmware/MotionControllerRP/src/robot_joint/robot_joint.cpp
@@ -0,0 +1,139 @@
+// --------------------------------------------------------------------------------------
+// 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 "robot_joint.h"
+#include "hw_config.h"
+#include "utilities/logging.h"
+#include "utilities/utilities.h"
+#include "servo_control/homing_controller.h"
+#include "servo_control/actuator_calibration.h"
+
+//*** FUNCTION **************************************************************************
+
+//*** CLASS *****************************************************************************
+
+//--- RobotJoint ------------------------------------------------------------------------
+
+RobotJoint::RobotJoint(MT6835Encoder* encoder, 
+                       TB6612MotorDriver* motor_driver,
+                       int pole_pairs) 
+{
+  RobotJoint::encoder = encoder;
+  RobotJoint::motor_driver = motor_driver;
+  servo_controller = new ServoController(*motor_driver, *encoder, pole_pairs);
+  position = 0.0f;
+  velocity = 0.0f;
+}
+
+RobotJoint::~RobotJoint() {
+  delete servo_controller;
+  delete motor_driver;
+  delete encoder;
+  servo_controller = nullptr;
+  motor_driver = nullptr;
+  encoder = nullptr;
+}
+
+void RobotJoint::init(int joint_idx) {
+  RobotJoint::joint_idx = joint_idx;
+
+  if(encoder->init(0x5, 0x4) == false) {
+    LOG_ERROR("Failed to initialize encoder for joint %i", joint_idx);
+  }
+
+  servo_controller->init(MOTOR_MAX_CURRENT_FACTOR);
+  servo_controller->set_motor_enabled(false, false);
+}
+
+bool RobotJoint::calibrate(bool print_measurements) {
+  LOG_INFO("Joint-%i: calibrating joint...", joint_idx);
+
+  HomingController homing_controller;
+  bool homing_ok = homing_controller.run_blocking(servo_controller, -HOMING_VELOCITY, 
+                                                  360.0f*DEG_TO_RAD, HOMING_CURRENT,
+                                                  ENCODER_ANGLE_TO_ROTOR_ANGLE,
+                                                  0.0f);
+  if(homing_ok == false) {
+    LOG_ERROR("Joint-%i: Calibration failed due to unsuccessful homing sequence", joint_idx);
+    return false;
+  }
+
+  // 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,
+                                     print_measurements);
+  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) {
+  servo_controller->update(position, dt, one_over_dt);
+}
+
+void RobotJoint::update_target(float p, float v) {
+  position = p;
+  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";
+}
diff --git a/firmware/MotionControllerRP/src/robot_joint/robot_joint.h b/firmware/MotionControllerRP/src/robot_joint/robot_joint.h
new file mode 100644
index 0000000..e5cf906
--- /dev/null
+++ b/firmware/MotionControllerRP/src/robot_joint/robot_joint.h
@@ -0,0 +1,60 @@
+// --------------------------------------------------------------------------------------
+// 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 "utilities/logging.h"
+#include "utilities/frequency_counter.h"
+#include "hardware/MT6701_encoder.h"
+#include "hardware/MT6835_encoder.h"
+#include "hardware/TB6612_motor_driver.h"
+#include "servo_control/servo_controller.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"
+#include "robot_joint_interface.h"
+
+//*** CLASS *****************************************************************************
+
+//--- RobotJoint ------------------------------------------------------------------------
+
+// TODO: create abstract interface for RobotJoint (to support remote Joints)
+//       hide direct HW access 
+
+class RobotJoint {
+  public:
+    RobotJoint(MT6835Encoder* encoder, TB6612MotorDriver* motor_driver, int pole_pairs);
+    ~RobotJoint();
+
+    void init(int joint_idx);
+    bool calibrate(bool print_measurements);
+    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;
+
+    MT6835Encoder* encoder;
+    ServoController* servo_controller;
+
+  private:
+    TB6612MotorDriver* motor_driver;
+};
diff --git a/firmware/MotionControllerRP/src/robot_joint/robot_joint_interface.h b/firmware/MotionControllerRP/src/robot_joint/robot_joint_interface.h
new file mode 100644
index 0000000..f5fc8be
--- /dev/null
+++ b/firmware/MotionControllerRP/src/robot_joint/robot_joint_interface.h
@@ -0,0 +1,28 @@
+// --------------------------------------------------------------------------------------
+// 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
+
+//*** CLASS *****************************************************************************
+
+//--- RobotJoint ------------------------------------------------------------------------
+
+// TODO: create abstract interface for RobotJoint (to support remote Joints)
+//       hide direct HW access 
+
+class IRobotJoint {
+  public:
+    virtual ~IRobotJoint() {};
+
+    void init(int joint_idx);
+    void update_target(float p, float v);
+    bool load_calibration();
+    bool store_calibration();
+
+    virtual bool calibrate(bool print_measurements) = 0;
+    virtual void update(float dt, float one_over_dt) = 0;
+};
diff --git a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp
index 0f1d7a6..4e5b0cd 100644
--- a/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp
+++ b/firmware/MotionControllerRP/src/servo_control/actuator_calibration.cpp
@@ -25,6 +25,8 @@ bool measure_calibration_data(
             size_t table_size,
             bool print_measurements)
 {
+  const float max_rmse_rad = Constants::DEG2RAD * 0.5f;
+
   LOG_INFO("Measuring motor to encoder angle lookup table...");
   int sample_count = table_size*4;
 
@@ -37,7 +39,7 @@ bool measure_calibration_data(
   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) {
+  auto run_measurement = [&](int sample_count, float field_angle_step, int& weak_field_measurements) {
     // Measure in increasing direction
     for (size_t i = 0; i < sample_count; ++i) {
       if(i>0)
@@ -47,7 +49,10 @@ bool measure_calibration_data(
       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
-
+  
+      if(servo_controller.get_encoder().get_status() & MT6835_STATUS_WEAKFIELD)
+        weak_field_measurements++;
+ 
       if(print_measurements)
         LOG_RAW("%15.10f, %15.10f, %f", motor_pos, field_angle, encoder_angle_raw);
 
@@ -57,10 +62,11 @@ bool measure_calibration_data(
   };
 
   // Measure in increasing direction
+  int weak_field_measurements = 0;
   LOG_DEBUG("Running foreward pass...");
-  run_measurement(sample_count, field_angle_step);
+  run_measurement(sample_count, field_angle_step, weak_field_measurements);
   LOG_DEBUG("Running backward pass...");
-  run_measurement(sample_count, -field_angle_step);
+  run_measurement(sample_count, -field_angle_step, weak_field_measurements);
 
   // rotate back to start position
   motor_driver.rotate_field(start_field_angle-motor_driver.get_field_angle(), 
@@ -68,20 +74,37 @@ bool measure_calibration_data(
       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.");
+  if(weak_field_measurements > 0) {
+    LOG_ERROR("Magnetic field too weak for %i of %i measurements", weak_field_measurements, sample_count);
     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);
+  // build lookup table 'encoder_raw_angle -> motor_pos'
+  float rmse = 0.0f;
+  bool ok = encoder_raw_to_motor_pos_lut.init_interpolating(encoder_angle_and_motor_pos, table_size, true);
   if(ok == false) {
-    LOG_ERROR("Creating lookup table motor_pos -> field_angle failed.");
+    LOG_ERROR("Creating lookup table 'encoder_raw_angle -> motor_pos' failed.");
     return false;
   }
+  encoder_raw_to_motor_pos_lut.optimize_lut(encoder_angle_and_motor_pos, rmse);
+  if(rmse > max_rmse_rad) {
+    LOG_WARNING("Fitting error of lookup table 'encoder_raw_angle -> motor_pos' unusually high (rms_error = %f deg)."
+                "Calibration might be invalid.", 
+                Constants::RAD2DEG*rmse);
+  }
+
+  // build lookup table 'motor_pos -> field_angle'
+  ok = motor_pos_to_field_angle_lut.init_interpolating(motor_pos_and_field_angle, table_size/2, true);
+  if(ok == false) {
+    LOG_ERROR("Creating lookup table 'motor_pos -> field_angle' failed.");
+    return false;
+  }
+  motor_pos_to_field_angle_lut.optimize_lut(motor_pos_and_field_angle, rmse);
+    if(rmse > max_rmse_rad) {
+    LOG_WARNING("Fitting error of lookup table 'motor_pos -> field_angle' is unusually high (rms_error = %f deg)."
+                "Calibration might be invalid.", 
+                Constants::RAD2DEG*rmse);
+  }
 
   LOG_INFO("finished");
   return true;
diff --git a/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp b/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp
index 7b56c27..52cf67c 100644
--- a/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp
+++ b/firmware/MotionControllerRP/src/servo_control/homing_controller.cpp
@@ -36,7 +36,7 @@ void HomingController::start(ServoController* servo_controller,
   float pole_pair_count = servo_controller->get_pole_pair_count();
   float field_angle_to_encoder_angle = 1.0f / pole_pair_count / encoder_angle_to_motor_angle;
 
-  if(retract_angle_rad >= 0.0f)
+  if(retract_angle_rad > 0.0f)
     HomingController::retract_field_angle = retract_angle_rad * pole_pair_count;
 
   // field_angle_to_rotor_angle = 1.0 / pole_pair_count
@@ -136,7 +136,7 @@ void HomingController::on_endstop_detected() {
 void HomingController::finalize() {
    auto& motor_driver = servo_ctrl->get_motor_driver();
    float pole_pair_count = servo_ctrl->get_pole_pair_count();
-
+ 
   // back off from home position
   motor_driver.rotate_field(retract_field_angle * (field_velocity>0.0f ? -1.0f : 1.0f), 
                             retract_field_velocity, [this](){
diff --git a/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp b/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
index 39a6b82..d0d1119 100644
--- a/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
+++ b/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
@@ -12,6 +12,8 @@
 #include "utilities/logging.h"
 #include "utilities/math_constants.h"
 
+#include "hw_config.h"
+
 #include <algorithm>
 
 ServoController::ServoController(
@@ -44,9 +46,14 @@ void ServoController::init(float max_motor_amplitude) {
   motor_driver.enable();
   motor_driver.set_field_angle(0.0f);
 
-  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);
+  velocity_lowpass.set_time_constant(VEL_LOWPASS_TC);
+
+  pos_controller.set_parameter(POS_KP, POS_KI, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
+  velocity_controller.set_parameter(VEL_KP, VEL_KI, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
+
+  // large 0.9° steppers
+//  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);
diff --git a/firmware/MotionControllerRP/src/utilities/lookup_table.cpp b/firmware/MotionControllerRP/src/utilities/lookup_table.cpp
index 253cc5c..375b1c3 100644
--- a/firmware/MotionControllerRP/src/utilities/lookup_table.cpp
+++ b/firmware/MotionControllerRP/src/utilities/lookup_table.cpp
@@ -147,7 +147,7 @@ bool LookupTable::init_interpolating(std::vector<std::pair<float, float>> in_out
   return true;
 }
 
-bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs) {
+bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs, float& rms_error) {
   if (lookup_table.empty() || in_out_pairs.empty())
     return false;
 
@@ -198,7 +198,9 @@ bool LookupTable::optimize_lut(std::vector<std::pair<float, float>> in_out_pairs
     //if(iter%100 == 0)
     //  LOG_DEBUG("iteration %04i: rms=%f", iter, sqrtf(total_loss));
   }
-  LOG_DEBUG("Optimizing lookup table finished: rms=%f", total_loss);
+
+  rms_error = sqrt(total_loss);
+  LOG_DEBUG("Optimizing lookup table finished: rms_error=%f", rms_error);
 
   return true;
 }
diff --git a/firmware/MotionControllerRP/src/utilities/lookup_table.h b/firmware/MotionControllerRP/src/utilities/lookup_table.h
index 456c4a1..5ea963b 100644
--- a/firmware/MotionControllerRP/src/utilities/lookup_table.h
+++ b/firmware/MotionControllerRP/src/utilities/lookup_table.h
@@ -27,7 +27,7 @@ class LookupTable {
                             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);
+    bool optimize_lut(std::vector<std::pair<float, float>> in_out_pairs, float& rms_error);
 
     // clear the lookup table, use init to use it again
     void clear();
diff --git a/firmware/MotionControllerRP/src/version.h b/firmware/MotionControllerRP/src/version.h
index 754176c..adee848 100644
--- a/firmware/MotionControllerRP/src/version.h
+++ b/firmware/MotionControllerRP/src/version.h
@@ -1 +1 @@
-static const char* FIRMWARE_VERSION = "v1.0.3";
+static const char* FIRMWARE_VERSION = "v1.0.4";