diff --git a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
index 6cc25db..ee568f4 100644
--- a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
+++ b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
@@ -46,7 +46,7 @@ float MT6835Encoder::read_abs_angle() {
     return raw_angle * RAW_TO_ANGLE;
 }
 
-int32_t MT6835Encoder::read_abs_angle_raw() {
+MT6835Encoder::AbsRawAngleType MT6835Encoder::read_abs_angle_raw() {
     uint8_t data[6] = {0};
     data[0] = MT6835_OP_ANGLE << 4;
     data[1] = MT6835_REG_ANGLE1;
@@ -70,6 +70,10 @@ int32_t MT6835Encoder::read_abs_angle_raw() {
     return update_abs_raw_angle(raw_angle);
 }
 
+MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() {
+  return abs_raw_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 a6f7d9f..dfaa116 100644
--- a/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
+++ b/firmware/MotionControllerRP/src/hardware/MT6835_encoder.h
@@ -145,6 +145,7 @@ class MT6835Encoder {
 
       float read_abs_angle();                         // returns the absolute angle in radians
       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
 
       int32_t get_rawcounts_per_rev();                // returns the number of raw counts per revolution
 
diff --git a/firmware/MotionControllerRP/src/motion_control/path_planner.cpp b/firmware/MotionControllerRP/src/motion_control/path_planner.cpp
index 0f0159a..8c6aeca 100644
--- a/firmware/MotionControllerRP/src/motion_control/path_planner.cpp
+++ b/firmware/MotionControllerRP/src/motion_control/path_planner.cpp
@@ -4,9 +4,14 @@
 #include "path_planner.h"
 #include "utilities/logging.h"
 
+#include <algorithm>
+
 PathPlanner::PathPlanner(IKinemtaicModel* kinematic_model, float time_step) {
   segment_time_step = time_step;
   kinematic_model = kinematic_model;
+
+  junction_deviation.linear = 0.0001;  // mm
+  junction_deviation.angular = 0.001; // rad
 }
 
 PathPlanner::~PathPlanner() {
@@ -23,9 +28,6 @@ bool PathPlanner::add_cartesian_path_segment(const CartesianPathSegment& path_se
     return false;
   }
 
-  // TODO: do look ahead planning of queue
-  new_segment->compute_motion_profile(); // for testing
-
   return true;
 }
 
@@ -34,9 +36,15 @@ void PathPlanner::process(bool disable_interrupts_for_queue_update) {
   // the segment stays in the queue until it is completed
   if(segment_generator == nullptr && ct_path_segment_queue.empty() == false) {
     auto* current_segment = ct_path_segment_queue.peek();
+
+    // compute motion profile ( active segment can not change anymore )
+    current_segment->compute_motion_profile();
+
+    // create path segment generator
     segment_generator = new JointSpacePathSegmentGenerator(current_segment, 
                                                            kinematic_model,
                                                            segment_time_step);
+
     /*LOG_INFO("Starting segment: duration=%fs, (%f, %f, %f)->(%f, %f, %f) | queue size: %i", 
       current_segment->get_duration(),
       current_segment->start_pose.translation.x,
@@ -97,5 +105,111 @@ int PathPlanner::input_queue_size() {
 }
 
 void PathPlanner::run_look_ahead_planning() {
+  if(ct_path_segment_queue.empty())
+    return;
 
+  int n = ct_path_segment_queue.size();
+  auto* last_segment = ct_path_segment_queue.get(n - 1);
+  if(segment_generator != nullptr && last_segment == segment_generator->get_path_segment())
+    return;
+
+  // --- Reverse pass ---
+  // Start from the last segment, set its end velocity to zero (or target)
+  last_segment->end_velocity = LinearAngular{0, 0};
+  
+  // Propagate backward the feasible start velocities
+  for (int i = n - 2; i >= 0; i--) {
+    CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
+    CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
+
+    // dont change active segment
+    if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
+      s2->start_velocity = s1->end_velocity;
+      continue;
+    }
+  
+    // Get minimum acceleration capability at junction
+    float acc = std::min(s1->max_acceleration.linear, s2->max_acceleration.linear);
+
+    // Compute max junction velocity feasible at junction between s1 and s2
+    float max_junction_velocity_linear = compute_max_junction_velocity(
+      s1->translation_delta_normalized,
+      s2->translation_delta_normalized,
+      acc,
+      junction_deviation.linear
+    );
+
+    // no angular junction velocity limit yet
+    float max_junction_velocity_angular = 1e10;
+
+    // compute max velocity delta
+    float v_start_max_linear = std::sqrt(std::max(0.0f, powf(s1->end_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
+    float v_start_max_angular = std::sqrt(std::max(0.0f, powf(s1->end_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
+
+    // compute final junction velocity
+    LinearAngular junction_velocity;
+    junction_velocity.linear = std::min(max_junction_velocity_linear, std::min(s2->target_velocity.linear, v_start_max_linear));
+    junction_velocity.angular = std::min(max_junction_velocity_angular, std::min(s2->target_velocity.angular, v_start_max_angular));
+    
+    // set calculated velocity to both path segments
+    s1->end_velocity = junction_velocity;
+    s2->start_velocity = junction_velocity;
+  }
+
+  // --- Forward pass ---
+  for (int i = 0; i < n - 1; i++) {
+    CartesianPathSegment* s1 = ct_path_segment_queue.get(i);
+    CartesianPathSegment* s2 = ct_path_segment_queue.get(i + 1);
+
+    // dont change active segment
+    if(segment_generator != nullptr && s1 == segment_generator->get_path_segment()) {
+      s2->start_velocity = s1->end_velocity;
+      continue;
+    }
+
+    float v_start = s1->start_velocity.linear;
+    float v_end_max_linear = std::sqrt(std::max(0.0f, powf(s1->start_velocity.linear, 2.0f) + 2 * s1->max_acceleration.linear * s1->travel_distance.linear));
+    float v_end_max_angular = std::sqrt(std::max(0.0f, powf(s1->start_velocity.angular, 2.0f) + 2 * s1->max_acceleration.angular * s1->travel_distance.angular));
+
+    LinearAngular junction_velocity;
+    if(s1->end_velocity.linear > v_end_max_linear) s1->end_velocity.linear = v_end_max_linear;
+    if(s1->end_velocity.angular > v_end_max_angular) s1->end_velocity.angular = v_end_max_angular;
+
+    s2->start_velocity = s1->end_velocity;
+  }
+
+  // debug
+  // print_cartesian_path_segments();
 }
+
+float PathPlanner::compute_max_junction_velocity(const Vec3F& dir_in_normalized, const Vec3F& dir_out_normalized, float acceleration, float junction_deviation) {
+    const float EPSILON = 1e-6f;
+    const float COS_NEAR_STRAIGHT = 0.9999f;
+    const float COS_NEAR_OPPOSITE = -0.9999f;
+
+    // Compute the cosine of the angle between the directions (negative dot product)
+    float cos_theta = -dir_in_normalized.dot(dir_out_normalized);
+
+    // Compute sin(θ/2) using half-angle identity: sin²(θ/2) = (1 - cosθ) / 2
+    float sin_theta_d2 = std::sqrt(std::max(0.0f, 0.5f * (1.0f - cos_theta)));
+
+    // Compute vmax using classical junction deviation formula
+    float denom = std::max(1.0f - sin_theta_d2, EPSILON);
+    float vmax = std::sqrt(acceleration * junction_deviation * sin_theta_d2 / denom);
+
+    return vmax;
+}
+
+void PathPlanner::print_cartesian_path_segments() {
+  LOG_INFO("Cartesian Path Segment Info");
+  int n = ct_path_segment_queue.size();
+  for (int i = 0; i < n; i++) {
+    CartesianPathSegment* s = ct_path_segment_queue.get(i);
+    LOG_INFO("  Segment %02i: [%f, %f, %f]->[%f, %f, %f]  l=%f  vs=%fmm/s   ve=%fmm/s", i, 
+      s->start_pose.translation.x, s->start_pose.translation.y, s->start_pose.translation.z,  
+      s->end_pose.translation.x, s->end_pose.translation.y, s->end_pose.translation.z,  
+      s->travel_distance.linear,
+      s->start_velocity.linear, s->end_velocity.linear);
+  }
+}
+
diff --git a/firmware/MotionControllerRP/src/motion_control/path_planner.h b/firmware/MotionControllerRP/src/motion_control/path_planner.h
index 59b6923..7eabc2a 100644
--- a/firmware/MotionControllerRP/src/motion_control/path_planner.h
+++ b/firmware/MotionControllerRP/src/motion_control/path_planner.h
@@ -13,7 +13,7 @@ class IKinemtaicModel;
 
 class PathPlanner {
   public:
-    static constexpr int CT_QUEUE_SIZE = 64;
+    static constexpr int CT_QUEUE_SIZE = 32;
     static constexpr int JS_QUEUE_SIZE = 32;
     
   public:
@@ -26,6 +26,10 @@ class PathPlanner {
     // adds a new cartesian space path segment to the planner queue
     bool add_cartesian_path_segment(const CartesianPathSegment& path_segment);
 
+    // runs look ahead path planning. call this everytime after one or more cartesian
+    // path segments have been added
+    void run_look_ahead_planning();
+
     // Retrieves the next joint space path segment from the queue, returns false
     // if queue is empty.
     bool pop_js_path_segment(JointSpacePathSegment& segment);
@@ -44,7 +48,13 @@ class PathPlanner {
     int input_queue_size();
 
   private:
-    void run_look_ahead_planning();
+    float compute_max_junction_velocity(
+      const Vec3F& dir_in_normalized, 
+      const Vec3F& dir_out_normalized, 
+      float acceleration, 
+      float junction_deviation);
+
+    void print_cartesian_path_segments();
 
   private:
     RingBuffer<CartesianPathSegment, CT_QUEUE_SIZE> ct_path_segment_queue;
@@ -53,5 +63,6 @@ class PathPlanner {
     IKinemtaicModel* kinematic_model;
     JointSpacePathSegmentGenerator* segment_generator = nullptr;
     float segment_time_step;
+    LinearAngular junction_deviation;
 };
 
diff --git a/firmware/MotionControllerRP/src/motion_control/path_segment.cpp b/firmware/MotionControllerRP/src/motion_control/path_segment.cpp
index fbb4c03..2123b1e 100644
--- a/firmware/MotionControllerRP/src/motion_control/path_segment.cpp
+++ b/firmware/MotionControllerRP/src/motion_control/path_segment.cpp
@@ -21,17 +21,15 @@ MotionProfileConstAcc::MotionProfileConstAcc(
     float max_velocity,
     float max_acceleration)
 {
-  MotionProfileConstAcc::acceleration = max_acceleration;
-  MotionProfileConstAcc::v_start = v_start;
-  MotionProfileConstAcc::v_end = v_end;
-
   if (distance <= 1e-7f) {
     MotionProfileConstAcc::t1 = 0.0f;
     MotionProfileConstAcc::t2 = 0.0f;
     MotionProfileConstAcc::t3 = 0.0f;
     MotionProfileConstAcc::d1 = 0.0f;
     MotionProfileConstAcc::d2 = 1.0f;
+    MotionProfileConstAcc::v_start = 0.0;
     MotionProfileConstAcc::v_peak = 0.0f;
+    MotionProfileConstAcc::v_end = 0.0;
     MotionProfileConstAcc::acceleration = 0.0f;
   } else {
     const float inv_max_acceleration = 1.0f / max_acceleration;
@@ -43,40 +41,44 @@ MotionProfileConstAcc::MotionProfileConstAcc(
     // Distances covered during accel/decel
     float d_accel = 0.5f * (v_start + max_velocity) * t_accel;
     float d_decel = 0.5f * (max_velocity + v_end) * t_decel;
-
     float d_cruise = distance - (d_accel + d_decel);
+    float v_peak = 0.0;
 
     if (d_cruise >= 0.0f) {
       // Trapezoidal velocity profile
       MotionProfileConstAcc::t1 = t_accel;
       MotionProfileConstAcc::t2 = t1 + d_cruise / max_velocity;
       MotionProfileConstAcc::t3 = t2 + t_decel;
-      MotionProfileConstAcc::v_peak = max_velocity;
+      v_peak = max_velocity;
     } else {
       // Triangular velocity profile: recompute peak velocity v_peak
       float v_peak_sq = max_acceleration * distance + 0.5f * (v_start * v_start + v_end * v_end);
-      float v_peak = std::sqrt(std::max(0.0f, v_peak_sq));
+      v_peak = std::sqrt(std::max(0.0f, v_peak_sq));
 
       MotionProfileConstAcc::t1 = (v_peak - v_start) * inv_max_acceleration;
       MotionProfileConstAcc::t2 = t1 + 0.0f;
       MotionProfileConstAcc::t3 = t2 + (v_peak - v_end) * inv_max_acceleration;
-      MotionProfileConstAcc::v_peak = v_peak;
     }
+
+    // normalize velocity and acceleration to interpolator range (0..1)
+    const float inv_distance = 1.0f/distance;
+    max_acceleration *= inv_distance;
+    v_start *= inv_distance;
+    v_end *= inv_distance;
+    v_peak *= inv_distance;
+
+    // assign values
+    MotionProfileConstAcc::d1 = 0.5f * (v_start + v_peak) * t1;
+    MotionProfileConstAcc::d2 = d1 + v_peak * (t2-t1);
+    MotionProfileConstAcc::v_start = v_start;
+    MotionProfileConstAcc::v_end = v_end;
+    MotionProfileConstAcc::v_peak = v_peak;
+    MotionProfileConstAcc::acceleration = max_acceleration;
   }
 
-  // normalize velocity and acceleration to interpolator range (0..1)
-  const float inv_distance = 1.0f/distance;
-  v_start *= inv_distance;
-  v_end *= inv_distance;
-  v_peak *= inv_distance;
-  acceleration *= inv_distance;
-
-  // precompute some values for faster evaluation
-  MotionProfileConstAcc::d1 = 0.5f * (v_start + v_peak) * t1;
-  MotionProfileConstAcc::d2 = d1 + v_peak * (t2-t1);
-
-  //LOG_INFO("d1=%f, d2=%f, d3=%f", d1, d2, distance);
-  //LOG_INFO("t1=%f, t2=%f, t3=%f", t1, t2, t3);
+  // LOG_INFO("d1=%f, d2=%f, d3=%f", MotionProfileConstAcc::d1, MotionProfileConstAcc::d2, distance);
+  // LOG_INFO("t1=%f, t2=%f, t3=%f", MotionProfileConstAcc::t1, MotionProfileConstAcc::t2, MotionProfileConstAcc::t3);
+  // LOG_INFO("vs=%f, vp=%f, ve=%f", MotionProfileConstAcc::v_start, MotionProfileConstAcc::v_peak, MotionProfileConstAcc::v_end);
 }
 
 float MotionProfileConstAcc::evaluate(float time) const {
@@ -119,8 +121,19 @@ CartesianPathSegment::CartesianPathSegment(const Pose6DF& start_pose,
   CartesianPathSegment::end_velocity = LinearAngular(0.0f, 0.0f);
   CartesianPathSegment::max_acceleration = max_acceleration;
 
-  travel_distance.linear = (end_pose.translation - start_pose.translation).length();
+  Vec3F translation_delta = end_pose.translation - start_pose.translation;
+  travel_distance.linear = (translation_delta).length();
   travel_distance.angular = (start_pose.rotation.normalized_inverse() * end_pose.rotation).angle();
+
+  translation_delta_normalized = translation_delta.normalized();
+
+
+  /*
+  QuaternionF rotation_delta = (end_pose.rotation * start_pose.rotation.normalized_inverse());
+  Vec3F axis;
+  float angle;
+  rotation_delta.to_axis_angle(axis, angle);
+  rotation_delta_axis = axis; */
 }
 
 CartesianPathSegment::CartesianPathSegment(const Pose6DF& pose, float dwell_time)
@@ -144,9 +157,9 @@ void CartesianPathSegment::compute_motion_profile() {
   if(dwell_time > 0.0f) {
     motion_profile = MotionProfileConstAcc(dwell_time);
   } else {
-    MotionProfileConstAcc linear_profile(travel_distance.linear,start_velocity.linear, 
-                                        end_velocity.linear, target_velocity.linear, 
-                                        max_acceleration.linear);
+    MotionProfileConstAcc linear_profile(travel_distance.linear, start_velocity.linear, 
+                                         end_velocity.linear, target_velocity.linear, 
+                                         max_acceleration.linear);
 
     MotionProfileConstAcc angular_profile(travel_distance.angular, start_velocity.angular, 
                                           end_velocity.angular, target_velocity.angular, 
@@ -164,6 +177,7 @@ void CartesianPathSegment::compute_motion_profile() {
 void CartesianPathSegment::evaluate(float time, Pose6DF& pose) const {
   // evaluate motion profile
   float t = motion_profile.evaluate(time);
+  // LOG_INFO(">t_x [mm]: %f", t);
 
   // interpolate pose
   pose = Pose6DF::lerp(start_pose, end_pose, t);
@@ -265,6 +279,7 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
   // evaluate path to get new end position
   Pose6DF seg_end_pose;
   path_segment->evaluate(current_time, seg_end_pose);
+  // LOG_INFO(">pos_x [mm]: %f", seg_end_pose.translation.x);
 
   // evaluate inverse kinematic model here
   float next_joint_pos[NUM_JOINTS];
@@ -279,4 +294,8 @@ bool JointSpacePathSegmentGenerator::generate_next(JointSpacePathSegment& js_pat
     current_joint_pos[i] = next_joint_pos[i];
 
   return end_reached;
-}
\ No newline at end of file
+}
+
+const CartesianPathSegment* JointSpacePathSegmentGenerator::get_path_segment() const {
+  return path_segment;
+}
diff --git a/firmware/MotionControllerRP/src/motion_control/path_segment.h b/firmware/MotionControllerRP/src/motion_control/path_segment.h
index b56f48a..57b6c72 100644
--- a/firmware/MotionControllerRP/src/motion_control/path_segment.h
+++ b/firmware/MotionControllerRP/src/motion_control/path_segment.h
@@ -64,7 +64,6 @@ class CartesianPathSegment {
 
     void evaluate(float time, Pose6DF& pose) const;
     float get_duration() const;
-
     void compute_motion_profile();
 
   public:
@@ -76,6 +75,10 @@ class CartesianPathSegment {
     LinearAngular end_velocity;
     LinearAngular max_acceleration;
 
+    LinearAngular max_velocity_delta;
+    Vec3F translation_delta_normalized;
+    //Vec3F rotation_delta_axis;
+
     LinearAngular travel_distance;
     MotionProfileConstAcc motion_profile;
 
@@ -121,8 +124,9 @@ class JointSpacePathSegmentGenerator {
       float time_step
     );
 
-    void reset();
-    bool generate_next(JointSpacePathSegment& js_path_segment);
+    void                        reset();
+    bool                        generate_next(JointSpacePathSegment& js_path_segment);
+    const CartesianPathSegment* get_path_segment() const;
 
   private:
     float delta_time;                     // time step size
diff --git a/firmware/MotionControllerRP/src/robot.cpp b/firmware/MotionControllerRP/src/robot.cpp
index 60e1b68..53019e2 100644
--- a/firmware/MotionControllerRP/src/robot.cpp
+++ b/firmware/MotionControllerRP/src/robot.cpp
@@ -88,8 +88,9 @@ Robot::Robot(float path_segment_time_step) :
 
   command_parser.set_command_processor(this);
 
+  current_feedrate = LinearAngular(10.0f, 1.0f);
   max_acceleration = LinearAngular(500.0f, 50.0f);
-  path_buffering_time_us = 100*1000;
+  path_buffering_time_us = 50*1e3;
 
   state = ERobotState::IDLE;
 }
@@ -183,6 +184,13 @@ void Robot::update_command_parser() {
 
   // update command parse which will queue command to the path planner
   command_parser.update();
+
+  // TESTING:
+  //sleep_ms(10);
+  //float pos_error = joints[1]->servo_controller->get_position_error();
+  //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);
 }
 
 /**
@@ -194,15 +202,14 @@ void Robot::update_path_planner() {
   // check if buffering starts
   uint64_t time = time_us_64();
   if(state == ERobotState::IDLE && path_planner.input_queue_size() > 0) {
-    state = ERobotState::BUFFERING_PAH;
+    state = ERobotState::BUFFERING_PATH;
     path_buffering_start_time = time;
   }
 
   // check if execution starts
   uint64_t buffering_time = time-path_buffering_start_time;
-  if(state == ERobotState::BUFFERING_PAH && buffering_time > path_buffering_time_us) {
+  if(state == ERobotState::BUFFERING_PATH && buffering_time > path_buffering_time_us) {
     state = ERobotState::EXECUTING_PATH;
-    path_buffering_start_time = time_us_64();
   }
 
   // execute path
@@ -287,6 +294,7 @@ void Robot::send_reply(const char* str) {
 
 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(startswith(cmd.get_command(), "M")) process_machine_command(cmd, reply);
   else reply="error: unknown command\n";
@@ -295,9 +303,17 @@ void Robot::process_command(const GCodeCommand& cmd, std::string& reply) {
 void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply) {
   Pose6DF end_pose;
 
+  if(path_planner.input_queue_full()) {
+    reply = "error: input queue full\n";
+    return;
+  }
+
   // read feed rate
-  float feed_linear = cmd.get_value('F', 10.0f);
-  float feed_angular = cmd.get_value('R', 1.0f);
+  current_feedrate.linear = cmd.get_value('F', current_feedrate.linear);
+  current_feedrate.angular = cmd.get_value('R', current_feedrate.angular);
+
+  if(cmd.has_word('I'))
+    state = ERobotState::EXECUTING_PATH;
 
   // read translation
   end_pose.translation.x = cmd.get_value('X', current_pose.translation.x);
@@ -314,33 +330,54 @@ void Robot::process_motion_command(const GCodeCommand& cmd, std::string& reply)
 
   // create path segment
   CartesianPathSegment path_segment(current_pose, end_pose, 
-                                    LinearAngular(feed_linear, feed_angular), 
+                                    current_feedrate, 
                                     max_acceleration);
 
-  path_planner.add_cartesian_path_segment(path_segment);
-  current_pose = end_pose;
-
-  reply = "ok\n";
+  bool ok = path_planner.add_cartesian_path_segment(path_segment);
+  if(ok) {
+    path_planner.run_look_ahead_planning();
+    current_pose = end_pose;
+    reply = "ok\n";
+  } else {
+    reply = "error\n";
+  }
 }
 
 void Robot::process_machine_command(const GCodeCommand& cmd, std::string& reply) {
+  reply = "";
+  
   if(cmd.get_command() == "M50") {
-    reply = "Current Position: ";
+    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";
   }
   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";
   }
 }
 
@@ -353,8 +390,26 @@ void Robot::process_dwell_command(const GCodeCommand& cmd, std::string& reply) {
   // create path segment
   CartesianPathSegment path_segment(current_pose, dwell_time);
   path_planner.add_cartesian_path_segment(path_segment);
+  path_planner.run_look_ahead_planning();
 
   reply = "ok\n";
 }
 
+void Robot::process_set_servo_parameter_command(const GCodeCommand& cmd, std::string& reply) {
+  // example: M55 A150 B50000 C0.2 D100 E F0.0025
+  bool has_all = cmd.has_word('A') && cmd.has_word('B') && cmd.has_word('C') && 
+                 cmd.has_word('D') && cmd.has_word('F');
+
+  if(has_all == false)
+    reply = "error: not all parameters given (A,B,C,D,F expected)\n";
+
+  for(int i=0; i<NUM_JOINTS; i++) {
+
+    joints[i]->servo_controller->velocity_lowpass.set_time_constant(cmd.get_value('F'));
+    joints[i]->servo_controller->pos_controller.set_parameter(cmd.get_value('A'), cmd.get_value('B'), 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
+    joints[i]->servo_controller->velocity_controller.set_parameter(cmd.get_value('C'), cmd.get_value('D'), 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
+  }
+
+  reply = "ok\n";
+}
 
diff --git a/firmware/MotionControllerRP/src/robot.h b/firmware/MotionControllerRP/src/robot.h
index a196bcb..3738850 100644
--- a/firmware/MotionControllerRP/src/robot.h
+++ b/firmware/MotionControllerRP/src/robot.h
@@ -21,7 +21,7 @@ class Robot;
 
 enum class ERobotState {
   IDLE = 0,
-  BUFFERING_PAH = 1,
+  BUFFERING_PATH = 1,
   EXECUTING_PATH = 2,
   ERROR = 3
 };
@@ -84,6 +84,8 @@ class Robot : public ICommandProcessor {
     void process_motion_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);
 
   protected:
     static bool update_motion_controller_isr(repeating_timer_t* timer); // called from update timer
@@ -102,6 +104,7 @@ class Robot : public ICommandProcessor {
 
     LinearAngular max_acceleration;
     Pose6DF current_pose;
+    LinearAngular current_feedrate;
 
     SharedData shared_data;
 
diff --git a/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp b/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
index e22dc70..69940b1 100644
--- a/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
+++ b/firmware/MotionControllerRP/src/servo_control/servo_controller.cpp
@@ -34,9 +34,9 @@ void ServoController::init(float max_motor_amplitude) {
     sleep_ms(1);
   }
 
-  velocity_lowpass.set_time_constant(0.0025f);
-  pos_controller.set_parameter(150.0f, 50000.0f, 0.0f, Constants::PI_F*2.0F, Constants::PI_F*0.5F);
-  velocity_controller.set_parameter(0.2f, 100.0f, 0.0f, Constants::PI_F*0.45f, Constants::PI_F*0.45f);
+  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);
 }
 
 void ServoController::set_encoder_lut(LookupTable& enc_to_pos_lut) {
diff --git a/firmware/MotionControllerRP/src/servo_control/servo_controller.h b/firmware/MotionControllerRP/src/servo_control/servo_controller.h
index 19491d4..2d13bde 100644
--- a/firmware/MotionControllerRP/src/servo_control/servo_controller.h
+++ b/firmware/MotionControllerRP/src/servo_control/servo_controller.h
@@ -50,15 +50,16 @@ class ServoController {
     float motor_pos_to_field_angle(float motor_pos);
     float motor_velocity_to_field_velocity(float v);
 
+  public:
+    LowpassFilter velocity_lowpass;
+    PIDController pos_controller;
+    PIDController velocity_controller;
+
   private:
     ENCODER_TYPE& encoder;
     MOTOR_DRIVER_TYPE& motor_driver;
     LookupTable enc_to_pos_lut;
 
-    LowpassFilter velocity_lowpass;
-    PIDController pos_controller;
-    PIDController velocity_controller;
-
     float motor_pos = 0;                  // current motor position
     float motor_pos_prev = 0;             // previous motor position
     float pos_error = 0;                  // current position error as computed by upate()
diff --git a/firmware/MotionControllerRP/src/utilities/ringbuffer.h b/firmware/MotionControllerRP/src/utilities/ringbuffer.h
index 51b8ee9..bf452f1 100644
--- a/firmware/MotionControllerRP/src/utilities/ringbuffer.h
+++ b/firmware/MotionControllerRP/src/utilities/ringbuffer.h
@@ -33,6 +33,11 @@ class RingBuffer {
       return &(buffer[tail]);
     }
 
+    T* get(int i) {
+        if (count == 0) return nullptr;
+        return &(buffer[(tail + i) % N]);
+    }
+
     bool empty() const { return count == 0; }
     bool full() const { return count == N; }
     int size() const { return count; }