diff --git a/servoTest/servoTest.ino b/servoTest/servoTest.ino
index c75229f..d4d8328 100644
--- a/servoTest/servoTest.ino
+++ b/servoTest/servoTest.ino
@@ -1,55 +1,95 @@
-#include <Servo.h>
-#include <avr/io.h>
 #include "config.h"
+#include <Servo.h>
+// #include <avr/io.h>
 
+// mee arm Servo pins
 #define S_left_pin 10
 #define S_right_pin 5
 #define S_turn_pin 6
 #define S_grip_pin 9
 
-Servo S_left,S_right,S_turn,S_grip;
+// Command enum for serial communication
+// L - left servo
+// R - right servo
+// T - turn servo
+// G - grip servo
+typedef enum { L, R, T, G, H, S, D } CMD;
 
+char CMD2char(CMD c) {
+  return (c == L) ? 'L' : (c == R) ? 'R' : (c == T) ? 'T' : 'G';
+}
+
+void error(String msg) {
+  Serial.print("error: ");
+  Serial.println(msg);
+}
+
+// Servo setup
+Servo S_left, S_right, S_turn, S_grip;
 const Servo *axis[4] = {&S_left, &S_right, &S_turn, &S_grip};
 const byte axCount = 4;
-
-int ServoPos[] = {90,90,90,90};
+int servoPos[] = {90, 90, 90, 90};
 bool absPos; // absolute /relative positioning
 
-typedef enum {
-    L,
-    R,
-    T,
-    G
-}ax;
+// Move axis[dir] to the position set in servoPos[dir]
+void movePos(CMD dir) {
+  // safety check
+  if (dir > G) {
+    error("Not a valid Axis!!");
+    return;
+  }
+  if (servoPos[dir] < 20 || servoPos[dir] > 175) {
+    error("move out of range");
+    return;
+  }
 
-
-void error(String msg){
-    Serial.print("error: ");
-    Serial.println(msg);
-}
-
-void movePos(ax dir){
-    int val = axis[dir]->read();
-    if(ServoPos[dir] < 20 || ServoPos[dir] > 175){
-      error("move out of range");
-      return;
+  // if actual servo pos is different to wanted move towards it
+  int val = axis[dir]->read();
+  if (val != servoPos[dir]) {
+    int delta = servoPos[dir] - val;
+    int inc;
+    while (delta) {
+      inc = delta / 5;
+      if (inc == 0) {
+        error("movement increment is 0?");
+        return;
       }
-    if(val != ServoPos[dir]){
-      int delta = ServoPos[dir]-val;
-      while(delta){
-        if(delta < 0){
-          axis[dir]->write((val--) - 1);
-          delta+=1;
-        }else{
-          axis[dir]->write((val++) + 1);
-          delta-=1;
-          }
-          delay(50);
-        }
-
+      axis[dir]->write(val + inc);
+      delta -= inc;
+      val += inc;
     }
+    delay(50); // TODO  no delay
+  }
 }
 
+int cmdVal;
+CMD cmd;
+
+void moveCmdHandler() {
+  Serial.print("Servo_Command: ");
+  Serial.print(CMD2char(cmd));
+  Serial.println(cmdVal);
+  if (absPos) {
+    servoPos[cmd] = constrain(cmdVal, 20, 165);
+  } else {
+    servoPos[cmd] = constrain(servoPos[cmd] + cmdVal, 20, 165);
+  }
+  Serial.print("new val:  ");
+  Serial.println(servoPos[cmd]);
+}
+
+void statusHandler() {
+  Serial.println((absPos) ? "Absolute Positioning" : "Relative Positioning");
+  for (int i = 0; i < axCount; i++) {
+    Serial.print(CMD2char(i));
+    Serial.print("--");
+    Serial.println(servoPos[i]);
+  }
+}
+// Serial Input buffer
+char serialBuf[8];
+int bufSize = 8;
+
 void setup() {
   S_left.attach(S_left_pin);
   S_right.attach(S_right_pin);
@@ -59,129 +99,81 @@ void setup() {
   Serial.begin(BAUDRATE);
   Serial.println("reading Servo positions");
   absPos = true;
-  for(int i =0; i < axCount; i++){
-      ServoPos[i] = axis[i]->read();
-      Serial.print("--");
-      Serial.println(ServoPos[i]);
-  }
-  delay(1000);
+  delay(500);
 }
-
-
-//Serial Input buffer
-char serialBuf[8];
-int bufSize = 8;
-
-
 void loop() {
 
-   // print changes 
-   for(int i =0; i < axCount; i++){
-      int val = axis[i]->read();
-      if(val != ServoPos[i]){
-        Serial.print("-- ");
-        Serial.println(i);
-        Serial.print(val);
-        Serial.print("-->");
-        Serial.println(ServoPos[i]);
-        movePos(i);
-      }
-      delay(50);
+  // print changes
+  int val;
+  for (int i = 0; i < axCount; i++) {
+    val = axis[i]->read();
+    if (val != servoPos[i]) {
+      Serial.print("-- ");
+      Serial.println(i);
+      Serial.print(val);
+      Serial.print("-->");
+      Serial.println(servoPos[i]);
+      movePos(i);
+    }
+    delay(50);
   }
-  
-  //read position command from serial 
-  int serialIn = Serial.available();
-  if(serialIn){
-    int r = Serial.readBytesUntil('\n',serialBuf, bufSize);
+
+  // read position command from serial
+  if (Serial.available()) {
+    int r = Serial.readBytesUntil('\n', serialBuf, bufSize);
     Serial.print("read: ");
     Serial.println(r);
-    //Serial.println(serialBuf);
-    if(r < 2){
-        error("command to short!");
-        return;
+    // Serial.println(serialBuf);
+    if (r < 2) {
+      error("command to short!");
+      return;
     }
     String numStr = "";
-    for(int i = 1; i < r; i++){
-        if(isDigit(serialBuf[i]) || isPunct(serialBuf[i])){
-            numStr += serialBuf[i];
-        }else{
-            break;
-        }
-      }
-    Serial.print("NumStr: ");
-    Serial.println(numStr);
-    int pos = numStr.toInt();
-
-    switch(serialBuf[0]){
-        case 'L':
-          Serial.println("Servo_Command L");
-          Serial.println(pos);
-          if(absPos){
-            ServoPos[L] = constrain(pos, 20, 165);
-          }else{
-            ServoPos[L] += pos;
-          }
-          Serial.println(ServoPos[L]);
-          break;
-        case 'R':
-          Serial.println("Servo_Command R");
-          Serial.println(pos);
-
-          if(absPos){
-            ServoPos[R] = constrain(pos, 20, 170);
-          }else{
-              ServoPos[R] += pos;
-          }
-
-          Serial.println(ServoPos[R]);
-
-          break;
-        case 'T':
-          Serial.println("Servo_Command T");
-          Serial.println(pos);
-          if(absPos){
-          ServoPos[T] = constrain(pos, 20, 170);
-          }else{
-              ServoPos[T] += pos;
-          }
-
-          Serial.println(ServoPos[T]);
-          break;
-        case 'G':
-          Serial.println("Servo_Command G");
-          Serial.println(pos);
-          if(absPos){
-          ServoPos[G] = constrain(pos, 90, 170);
-          }else{
-              ServoPos[G] += pos;
-          }
-
-          Serial.println(ServoPos[G]);
-          break;
-        case 'H':
-          Serial.println((absPos)?"Absolute Positioning" : "Relative Positioning");
-          for(int i =0; i < axCount; i++){
-              char axSym = (i==L)?'L':(i==R)?'R':(i==T)?'T':'G';
-              Serial.print(axSym);
-              Serial.print("--");
-              Serial.println(ServoPos[i]);
-          }
-          break;
-        case 'S':
-          if(serialBuf[1] == '0') absPos = false;
-          else absPos = true;
-          break;
-        case 'D': //default
-          for(int i = 0; i < axCount;i++){
-          ServoPos[i] = 90;
-          }
-          break;
-          
- 
-          
-        default:
-        error("invalid axis description");
+    for (int i = 1; i < r; i++) {
+      if (isDigit(serialBuf[i]) || isPunct(serialBuf[i])) {
+        numStr += serialBuf[i];
+      } else {
         break;
       }
-  }
+    }
+    // Serial.print("NumStr: ");
+    // Serial.println(numStr);
+    cmdVal = numStr.toInt();
+    char c = serialBuf[0];
+    cmd = (c == 'L')   ? L
+          : (c == 'R') ? R
+          : (c == 'T') ? T
+          : (c == 'G') ? G
+          : (c == 'H') ? H
+          : (c == 'S') ? S
+          : (c == 'D') ? D
+                       : -1;
 
+    switch (serialBuf[0]) {
+    case 'L':
+    case 'R':
+    case 'T':
+    case 'G':
+      moveCmdHandler();
+      break;
+    case 'H':
+      statusHandler();
+      break;
+    case 'S': // select relative / absolute positioning
+      if (serialBuf[1] == '0')
+        absPos = false;
+      else
+        absPos = true;
+      break;
+    case 'D': // default pos at 90° for all servos
+      for (int i = 0; i < axCount; i++) {
+        servoPos[i] = 90;
+      }
+      break;
+
+    default:
+      error("invalid command");
+      break;
+    }
+  }
+}