diff --git a/RAPID/LOGGER.mod b/RAPID/LOGGER.mod
index 4b13d19..e567680 100755
--- a/RAPID/LOGGER.mod
+++ b/RAPID/LOGGER.mod
@@ -20,7 +20,7 @@ PERS num BUFFER_LEFT;
 PERS bool BUFFER_LOCKED;
 PERS bool MOVING;
 CONST num MAX_BUFFER := 128;
-
+PERS num POINTS_DONE;
 
 !//Logger sampling rate
 !PERS num loggerWaitTime:= 0.01;  !Recommended for real controller
@@ -87,6 +87,7 @@ PROC main()
 !        data := data + NumToStr(position.rot.q4,3) + " ";
         !data := data + NumToStr(bufferLeft, 0) + " ";
         data := data + NumToStr(BUFFER_LEFT, 0) + " ";
+        data := data + NumToStr(POINTS_DONE, 0) + " ";
         
 		IF connected = TRUE THEN
 			SocketSend clientSocket \Str:=data;
diff --git a/RAPID/MOVE.mod b/RAPID/MOVE.mod
index c07ee65..6d500db 100644
--- a/RAPID/MOVE.mod
+++ b/RAPID/MOVE.mod
@@ -4,6 +4,7 @@ MODULE move
     PERS speeddata bufferSpeeds{MAX_BUFFER};
     PERS num BUFFER_POS;
     PERS num BUFFER_LEFT;
+    PERS num POINTS_DONE;
     PERS tooldata currentTool;    
     PERS wobjdata currentWobj;   
     PERS speeddata currentSpeed;
@@ -12,7 +13,9 @@ MODULE move
     PERS bool MOVING;
     VAR triggdata Movement;
     
-    PERS robtarget target := [[0,0,500],[0,0,-1,0], [0,0,0,0], [ 11, 12.3, 9E9, 9E9, 9E9, 9E9]];
+    PERS robtarget target := [[700,500,10],[1,0,0,0], [0,0,0,0], [ 11, 12.3, 9E+09, 9E+09, 9E+09, 9E+09]];
+    TASK PERS wobjdata wobjTest:=[FALSE,TRUE,"",[[1000,-1000,1000],[0.5,0.5,-0.5,0.5]],[[0,0,0],[1,0,0,0]]];
+    CONST robtarget positionZero:=[[0,0,300],[1,0,0,0],[-1,1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
     
     PROC main()
         BUFFER_LEFT := MAX_BUFFER;
@@ -23,7 +26,8 @@ MODULE move
         MOVING := FALSE;
         BUFFER_LOCKED := FALSE;
         ConfL \Off;
-        SingArea \LockAxis4;
+        !SingArea \LockAxis4;
+        !SingArea \Wrist;
         
         !BUFFER_POS := 0;
         WHILE TRUE DO
@@ -36,6 +40,7 @@ MODULE move
     ENDPROC
     
     
+    
     PROC movePoint()
         MoveL bufferTargets{BUFFER_POS}, bufferSpeeds{BUFFER_POS}, currentZone, currentTool \WObj:=currentWobj;
         WHILE BUFFER_LOCKED DO
@@ -51,6 +56,10 @@ MODULE move
             BUFFER_POS := BUFFER_POS - MAX_BUFFER;
         ENDIF
         BUFFER_LOCKED := FALSE;
+        POINTS_DONE := POINTS_DONE + 1;
+        IF BUFFER_LEFT = MAX_BUFFER THEN
+            POINTS_DONE := 0;
+        ENDIF
         !TriggL bufferTargets{1}, bufferSpeeds{1}, Movement, currentZone, currentTool \WObj:=currentWobj;
         !moveBuffer;
     ENDPROC
diff --git a/RAPID/SERVER.mod b/RAPID/SERVER.mod
index 76043a7..ca27fdb 100755
--- a/RAPID/SERVER.mod
+++ b/RAPID/SERVER.mod
@@ -5,8 +5,8 @@ MODULE SERVER
 !////////////////
 
 !//Robot configuration
-PERS tooldata currentTool := [TRUE,[[0,0,0],[-0.27065,0.65328,-0.2706,0.65328]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];    
-PERS wobjdata currentWobj := [FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];   
+PERS tooldata currentTool := [TRUE,[[-319.5,116.3,100.5],[0.21263,-0.62721,0.32651,-0.67438]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]];    
+PERS wobjdata currentWobj := [FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[1317,-952,125.1],[1,0.006,0.007,-0.016]]];   
 PERS speeddata currentSpeed;
 PERS zonedata currentZone;
 
@@ -35,10 +35,11 @@ VAR bool moveCompleted; !Set to true after finishing a Move instruction.
 
 !//Buffered move variables
 CONST num MAX_BUFFER := 128;
-PERS bool paused := TRUE;
+PERS bool paused := FALSE;
 PERS num BUFFER_POS;
 PERS num BUFFER_LEFT;
 PERS bool MOVING;
+PERS num POINTS_DONE;
 PERS robtarget bufferTargets{MAX_BUFFER};
 PERS speeddata bufferSpeeds{MAX_BUFFER};
 PERS bool BUFFER_LOCKED;
@@ -168,6 +169,10 @@ PROC main()
     VAR robtarget pwobj3;
     VAR wobjdata cwobj;
     VAR num writePos := 1;
+    VAR num speed := 0;
+    VAR speeddata newSpeed;
+    
+    POINTS_DONE := 0;
     
     
     !//Motion configuration
@@ -191,7 +196,7 @@ PROC main()
         addString := "";            
 
         !//Wait for a command
-        SocketReceive clientSocket \Str:=receivedString  \ReadNoOfBytes:=67 \Time:=WAIT_MAX;
+        SocketReceive clientSocket \Str:=receivedString  \ReadNoOfBytes:=76 \Time:=WAIT_MAX;
         ParseMsg receivedString;
         
         !//Execution of the command
@@ -325,11 +330,13 @@ PROC main()
                         currentZone.pzone_tcp := 0.0;
                         currentZone.pzone_ori := 0.0;
                         currentZone.zone_ori := 0.0;
+                        !currentZone := [false, 0.1, 0.1, 0.1, 0.01, 0.1, 0.01];
                     ELSE
                         currentZone.finep := FALSE;
                         currentZone.pzone_tcp := params{2};
                         currentZone.pzone_ori := params{3};
                         currentZone.zone_ori := params{4};
+                        !currentZone := [false, 0.1, 0.1, 0.1, 0.01, 0.1, 0.01];
                     ENDIF
                     ok := SERVER_OK;
                 ELSE
@@ -340,11 +347,18 @@ PROC main()
                 WHILE BUFFER_LOCKED DO
                     !wait
                 ENDWHILE
-                IF nParams = 7 THEN
+                IF nParams = 8 THEN
                     cartesianTarget :=[[params{1},params{2},params{3}],
                                         [params{4},params{5},params{6},params{7}],
                                         [0,0,0,0],
                                         externalAxis];
+                    speed := params{8};
+                    if speed < 1 OR speed > 250 THEN
+                        newSpeed := [250, 250, 250, 250];
+                    ELSE
+                        newSpeed := [speed, speed, speed, speed];                    
+                    ENDIF
+                                        
 !                    IF BUFFER_POS < MAX_BUFFER THEN
 !                        BUFFER_POS := BUFFER_POS + 1;
 !                        bufferTargets{BUFFER_POS} := cartesianTarget;
@@ -356,7 +370,7 @@ PROC main()
                             writePos := writePos - MAX_BUFFER;
                         ENDIF
                         bufferTargets{writePos} := cartesianTarget;
-                        bufferSpeeds{writePos} := currentSpeed;
+                        bufferSpeeds{writePos} := newSpeed;
                         BUFFER_LEFT := BUFFER_LEFT - 1;
                     ENDIF
                     !addString := NumToStr(MAX_BUFFER - BUFFER_POS,2);
@@ -380,6 +394,7 @@ PROC main()
                         BUFFER_POS := 1;
                     ENDIF
                     ok := SERVER_OK;
+                    POINTS_DONE := 0;
                 ELSE
                     ok:=SERVER_BAD_MSG;
                 ENDIF
@@ -557,4 +572,4 @@ ERROR (LONG_JMP_ALL_ERR)
     ENDTEST
 ENDPROC
 
-ENDMODULE
+ENDMODULE
\ No newline at end of file
diff --git a/abb_node/packages/abb_communications/abb.py b/abb_node/packages/abb_communications/abb.py
index 8321b8a..718a785 100755
--- a/abb_node/packages/abb_communications/abb.py
+++ b/abb_node/packages/abb_communications/abb.py
@@ -104,10 +104,11 @@ def readLogger(self):
                 #self.bufferLeft = int(data[9])
                 self.pose = []
                 self.bufferLeft = int(data[2])
+                pointsDone = int(data[3])
 
-                print(self.pose, self.bufferLeft)
+                print(self.pose, self.bufferLeft, pointsDone)
                 if self.callback != None:
-                    self.callback(self.pose, self.bufferLeft)
+                    self.callback(self.pose, self.bufferLeft, pointsDone)
 
 
     def set_units(self, linear, angular):
@@ -201,12 +202,12 @@ def set_zone(self,
         msg += format(zone[2], "+08.4f") + " " #10
         self.send(msg) #33
 
-    def buffer_add(self, pose):
+    def buffer_add(self, pose, speed = 250):
         '''
         Appends single pose to the remote buffer
         Move will execute at current speed (which you can change between buffer_add calls)
         '''
-        msg = "30 " + self.format_pose(pose)
+        msg = "30 " + self.format_pose(pose) + format(speed, "+08.2f") +  " "
         self.send(msg, False)
 
     def clear_buffer(self):
@@ -257,7 +258,7 @@ def sender(self):
         message = msg["message"]
         wait_for_response = msg["wait_for_response"]
 
-        while len(message) < 66:
+        while len(message) < 75:
             message += "*"
         message += "#"