diff --git a/.gitignore b/.gitignore
index 404c407..e7421e7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,2 @@
 staubli_val3_driver/val3/ros_server/.outlining.json
+*outlining.json
diff --git a/README.md b/README.md
index c374df0..d690731 100644
--- a/README.md
+++ b/README.md
@@ -9,7 +9,7 @@
 ## Overview
 
 This repository contains the `staubli_val3_driver` package which provides a set of VAL3 libraries and an application which together implement a [simple_message][] compatible server implementation.
-Together with the nodes in [industrial_robot_client][], this server can be used as a ROS 1 driver that allows motion control of Staubli CS8/CS8C controlled robots, by exposing a [FollowJointTrajectory][] [action][] server, which is compatible with MoveIt and other nodes that implement an action client.
+Together with the nodes in [industrial_robot_client][], this server can be used as a ROS 1 driver that allows motion control of Staubli CS8/CS9 controlled robots, by exposing a [FollowJointTrajectory][] [action][] server, which is compatible with MoveIt and other nodes that implement an action client.
 
 
 ## Documentation
@@ -19,8 +19,7 @@ Refer to the `staubli_val3_driver` [readme](./staubli_val3_driver/README.md) for
 
 ## Compatibility
 
-The current version of the driver is compatible with CS8/CS8C controllers only.
-Future work is planned to extend this to support CS9 controllers as well.
+The current version of the driver is compatible with Staubli CS8 and CS9 controllers.
 
 
 
diff --git a/staubli_val3_driver/README.md b/staubli_val3_driver/README.md
index 08b6b82..88750e9 100644
--- a/staubli_val3_driver/README.md
+++ b/staubli_val3_driver/README.md
@@ -2,48 +2,56 @@
 
 ## Overview
 
-This ROS-I driver was developed in Staubli's VAL 3 language for use with 6-axis Staubli robot manipulators.
+This ROS-I driver was developed in Staubli's VAL 3 language for use with 6-axis
+Staubli robot manipulators.
 
-It is advisable to try this driver on the emulator in [Staubli Robotics Suite (SRS)](https://www.staubli.com/en-us/robotics/product-range/robot-software/pc-robot-programming-srs/) first.
+It is advisable to try this driver on Staubli's emulator in Staubli Robotics Suite (SRS) first.
 
 
 ## Requirements
 
 * Staubli 6-axis robot manipulator
-* Staubli CS8/CS8C controller
-* VAL3 version s7.7.2 or greater
+* Staubli CS8/CS9 controller
+* VAL 3 version s7.7.2 or greater
   * this is very important, since this implementation uses return values of `sioGet()`
     only available from s7.7.2 onwards
 * Staubli Robotics Suite 2019 (not required but strongly recommended)
 
 ## Installation
 
-Installing the driver to a Staubli controller simply consists of transferring the contents of the `val3` folder to the controller itself.
+Installing the driver to a Staubli controller simply consists of transferring the
+contents of the `val3` folder to the controller itself.
 
 ### Clone this repository
 
-Clone [staubli_val3_driver](https://github.com/ros-industrial/staubli_val3_driver) into the `src` space of your workspace:
+Clone branch `kinetic-devel` of [staubli_experimental](https://github.com/ros-industrial/staubli_experimental):
 
 ```shell
-git clone https://github.com/ros-industrial/staubli_val3_driver.git
+git clone https://github.com/ros-industrial/staubli_experimental -b kinetic-devel
 ```
 
-### Transfer driver to Staubli CS8/CS8C controller
+### Transfer driver to Staubli controller
 
-There are 2 ways of transferring the VAL 3 application to the controller:
+There are multiple ways of transferring VAL 3 applications to the controller:
 
-1. Copy the contents of `val3` folder onto a USB memory stick (<2GB given the CS8/CS8C limitations), plugging the stick into the controller and using the teach pendant to copy the folders
+1. Copy the contents of `val3` folder onto a USB memory stick (<2GB if using CS8), 
+plugging the stick into the controller and using the teach pendant to copy the folders
 
-1. Use the Transfer Manager in SRS to copy the VAL 3 applications to the controller. (Home -> Controller -> Transfer Manager)
+2. Use the Transfer Manager in SRS to copy the contents of `val3` folder to the controller. (Home -> Controller -> Transfer Manager)
 
-### Open the VAL3 application with Staubli SRS
+2. Use an FTP software to copy the contents of `val3` folder to the controller.
 
-Although it is possible to edit the source files with any text editor (they are essentially XML files), it is advisable to use [Staubli Robotics Suite](https://www.staubli.com/en-us/robotics/product-range/robot-software/pc-robot-programming-srs/):
+### Open the VAL 3 application with Staubli SRS
+
+Although it is possible to edit the source files with any text editor (they are
+essentially XML files), it is advisable to use Staubli Robotics Suite:
 
 * Copy contents of folder `val3` into the `usrapp` folder of the Staubli cell
-* Open the `ros_server` VAL3 project located inside the `ros_server` folder
+* Open the `ros_server` VAL 3 appplication located inside the `ros_server` folder
+
+SRS offers autocompletion, syntax highlighting and syntax checking, amongst other
+useful features, such as a Staubli controller/teach pendant emulator.
 
-SRS offers autocompletion, syntax highlighting and syntax checking, amongst other useful features, such as a Staubli controller/teach pendant emulator. 
 
 ## Usage
 
@@ -56,14 +64,29 @@ From `Main menu`:
 
 ### Configuration
 
-The TCP sockets on the CS8/CS8C controller/emulator must be configured prior to use, otherwise a runtime error will be generated and the driver will not work.
+The TCP sockets on the CS8/CS9 controller/emulator must be configured prior to using
+the driver, otherwise a runtime error will be displayed on the teach pendant and
+the driver will not work.
+
+Two sockets (TCP Servers) are required.
 
-Two sockets (TCP Servers) are required. From `Main menu`:
+#### CS8
+
+ From `Main menu`:
 
 1. Control panel --> I/O --> Socket --> TCP Servers
 2. Configure two sockets
-   * Name: Feedback, Port: 11002, Timeout: -1, Delimiter: 13, Nagle: Off
-   * Name: Motion, Port: 11000, Timeout: -1, Delimiter: 13, Nagle: Off
+   * Name: Feedback, Port: 11002, Timeout: -1, End of string: 13, Nagle: Off
+   * Name: Motion, Port: 11000, Timeout: -1, End of string: 13, Nagle: Off
+
+#### CS9
+
+ From `Home`:
+
+1. IO --> Socket --> TCP Servers --> "+"
+2. Configure two sockets
+   * Name: Feedback, Port: 11002, Timeout: -1, End of string: 13, Nagle: Off
+   * Name: Motion, Port: 11000, Timeout: -1, End of string: 13, Nagle: Off
 
 ### Run the driver (ROS-I server)
 
@@ -71,22 +94,33 @@ Check that:
 
 1. The contents of the `val3` folder (both `ros_server` and `ros_libs` folders)
 have been transferred to the Staubli controller
-2. The VAL3 application `ros_server` has been loaded
+2. The VAL 3 application `ros_server` has been loaded
 3. Both TCP Server sockets have been configured properly
 
-Press the `Run` button, ensure that `ros_server` is highlighted, then press `F8` (Ok).
+#### CS8
+
+Press the `Run` button, ensure that `ros_server` is highlighted,
+then press `F8` (Ok).
+
+#### CS9
+
+VAL# --> Memory --> select `ros_server` --> ▶
 
-Depending on which working mode the controller is in, arm power may need to be enabled manually (a message will pop up on the screen).
-Once arm power is enabled, the robot will only move if the `Move` button has been pressed (or is kept pressed if in manual mode).
+Notice that depending on which mode of operation is currently active, the motors
+may need to be enabled manually (a message will pop up on the screen). Likewise,
+the robot will only move if the `Move` button has been pressed (or is kept pressed
+if in manual mode).
 
 ### Run the industrial_robot_client node (ROS-I client)
 
-The `indigo-devel` branch provides launch files (within the `staubli_val3_driver` ROS package). Simply run:
+The `kinetic-devel` branch provides launch files (within the `staubli_val3_driver`
+ROS package). Simply run:
 
 ```shell
-roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:=<controller IP address>
+roslaunch staubli_val3_driver robot_interface_streaming.launch robot_ip:=<Controller IP address>
 ```
 
 ## Bugs, suggestions and feature requests
 
-Please report any bugs you may find, make any suggestions you may have and request new features you may find useful via GitHub.
+Please report any bugs you may find, make any suggestions you may have and request
+new features you may find useful via GitHub.
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/ack.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/ack.pgx
new file mode 100644
index 0000000..b3d0f18
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/ack.pgx
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="ack" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nHeaderTime" type="num" xsi:type="element" />
+      <Parameter name="x_nElapsedTime" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // display time between valid trajectory point received and ACK sent
+  
+  gotoxy(scMonitor, 0,9)
+  put(scMonitor, "recvMsgHeader:")
+  gotoxy(scMonitor, 15,9)
+  put(scMonitor, toString(".4", x_nHeaderTime))
+    
+  gotoxy(scMonitor, 0,10)
+  put(scMonitor, "other states:")
+  gotoxy(scMonitor, 15,10)
+  put(scMonitor, toString(".4", x_nElapsedTime))
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/clearScreen.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/clearScreen.pgx
new file mode 100644
index 0000000..93c6571
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/clearScreen.pgx
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="clearScreen" access="public">
+    <Code><![CDATA[begin
+  
+  // Clear pendant screen
+  
+  cls(scMonitor)
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/debug.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/debug.pgx
new file mode 100644
index 0000000..944814a
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/debug.pgx
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="debug" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nExecStatus" type="num" xsi:type="element" use="reference" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Debug
+  
+  gotoxy(scMonitor, 18,12)
+  switch x_nExecStatus
+    case 0
+      put(scMonitor, "[+]")
+      x_nExecStatus = 1
+    break
+    case 1
+      put(scMonitor, "[-]")
+      x_nExecStatus = 0
+    break
+    default
+    break
+  endSwitch
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/feedbackStatus.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/feedbackStatus.pgx
new file mode 100644
index 0000000..566629b
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/feedbackStatus.pgx
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="feedbackStatus" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nOutConnFlag" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update connection status on port 11002 (feedback server)
+  
+  gotoxy(scMonitor, 0,1)
+  // 11 characteres: gotoxy(12,1) to insert status
+  put(scMonitor, "Port 11002:")
+  gotoxy(scMonitor, 12,1)
+  switch x_nOutConnFlag
+    case -1
+      put(scMonitor, "connection lost")
+    break
+    case 0
+      put(scMonitor, "not connected")
+    break
+    case 1
+      put(scMonitor, "connected")
+    break
+    default
+      put(scMonitor, "not connected")
+    break
+  endSwitch
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/init.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/init.pgx
new file mode 100644
index 0000000..5784605
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/init.pgx
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="init" access="public">
+    <Code><![CDATA[begin
+  
+  // Init user page
+  
+  userPage(scMonitor)
+  cls(scMonitor)
+  title(scMonitor, "ROS-I server")
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.dtx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.dtx
new file mode 100644
index 0000000..a4e6320
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.dtx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Database xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Data/2">
+  <Datas>
+    <Data name="mNomSpeed" access="private" xsi:type="array" type="mdesc" size="1" />
+    <Data name="scMonitor" access="private" xsi:type="array" type="screen" size="1" />
+  </Datas>
+</Database>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.pjx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.pjx
new file mode 100644
index 0000000..86090d8
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/interfaceCS8.pjx
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project xmlns="http://www.staubli.com/robotics/VAL3/Project/3">
+  <Parameters version="s7.11" stackSize="5000" millimeterUnit="true" />
+  <Programs>
+    <Program file="ack.pgx" />
+    <Program file="clearScreen.pgx" />
+    <Program file="debug.pgx" />
+    <Program file="feedbackStatus.pgx" />
+    <Program file="init.pgx" />
+    <Program file="motionBuffer.pgx" />
+    <Program file="motionProgress.pgx" />
+    <Program file="motionStatus.pgx" />
+    <Program file="start.pgx" />
+    <Program file="stop.pgx" />
+    <Program file="trajBuffer.pgx" />
+    <Program file="velOverwrite.pgx" />
+  </Programs>
+  <Database>
+    <Data file="interfaceCS8.dtx" />
+  </Database>
+  <Libraries>
+    <Library alias="libQueueFuncs" path="Disk://ros_libs/QueueFuncs/QueueFuncs.pjx" />
+  </Libraries>
+</Project>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionBuffer.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionBuffer.pgx
new file mode 100644
index 0000000..1d17b91
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionBuffer.pgx
@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionBuffer" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMovePts" type="num" xsi:type="element" />
+      <Parameter name="x_nElems" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update motion buffer size
+  
+  gotoxy(scMonitor, 0,4)
+  put(scMonitor, "Motion buffer:")
+  gotoxy(scMonitor, 19,4)
+  put(scMonitor, x_nElems)
+  gotoxy(scMonitor, 24,4)
+  put(scMonitor, x_nMovePts)
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionProgress.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionProgress.pgx
new file mode 100644
index 0000000..4d3f149
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionProgress.pgx
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionProgress" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMoveId" type="num" xsi:type="element" />
+      <Parameter name="x_nMotProg" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // display nMoveId and nMotionProgress
+  
+  gotoxy(scMonitor, 0,7)
+  put(scMonitor, "moveId:")
+  gotoxy(scMonitor, 8,7)
+  put(scMonitor, x_nMoveId)
+  gotoxy(scMonitor, 13,7)
+  put(scMonitor, "progress:")
+  gotoxy(scMonitor, 23,7)
+  put(scMonitor, x_nMotProg)
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionStatus.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionStatus.pgx
new file mode 100644
index 0000000..6899449
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/motionStatus.pgx
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionStatus" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nInConnFlag" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update connection status on port 11000 (motion server)
+  
+  gotoxy(scMonitor, 0,0)
+  // 11 characteres: gotoxy(12,0) to insert status
+  put(scMonitor, "Port 11000:")
+  gotoxy(scMonitor, 12,0)
+  switch x_nInConnFlag
+    case -1
+      put(scMonitor, "connection lost")
+    break
+    case 0
+      put(scMonitor, "not connected")
+    break
+    case 1
+      put(scMonitor, "connected")
+    break
+    default
+      put(scMonitor, "not connected")
+    break
+  endSwitch
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/start.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/start.pgx
new file mode 100644
index 0000000..11a27c4
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/start.pgx
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="start">
+    <Code><![CDATA[begin
+  
+  // Library to handle all of the interface actions for CS8 controller
+  // Loaded at runtime in ros_server()
+  
+  // DO NOT auto-load
+  // Auto-loading will break cross-controller functionality
+  // The correct library will be loaded at runtime, otherwise an error will be generated
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/stop.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/stop.pgx
new file mode 100644
index 0000000..f236f28
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/stop.pgx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="stop">
+    <Code><![CDATA[begin
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/trajBuffer.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/trajBuffer.pgx
new file mode 100644
index 0000000..ecf6076
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/trajBuffer.pgx
@@ -0,0 +1,23 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="trajBuffer" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nElems" type="num" xsi:type="element" />
+      <Parameter name="x_nPtsPopped" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update trajectory buffer size
+  
+  gotoxy(scMonitor, 0,3)
+  // 18 characteres: gotoxy(19,3) to insert status
+  put(scMonitor, "Trajectory buffer:")
+  gotoxy(scMonitor, 19,3)
+  put(scMonitor, x_nElems)
+  // also print number of points popped from buffer
+  gotoxy(scMonitor, 24,3)
+  put(scMonitor, x_nPtsPopped)
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/velOverwrite.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/velOverwrite.pgx
new file mode 100644
index 0000000..b61f267
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS8/velOverwrite.pgx
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="velOverwrite" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_bOverwriteVel" type="bool" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  gotoxy(scMonitor, 0,5)
+  put(scMonitor, "Velocity overwrite:")
+  gotoxy(scMonitor, 20,5)
+  if(x_bOverwriteVel == true)
+    put(scMonitor, "enabled")
+  else
+    put(scMonitor, "disabled")
+  endIf
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/ack.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/ack.pgx
new file mode 100644
index 0000000..6700122
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/ack.pgx
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="ack" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nHeaderTime" type="num" xsi:type="element" />
+      <Parameter name="x_nElapsedTime" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // display time between valid trajectory point received and ACK sent
+  
+  userPageSet("uxMonitor", "lblHeaderTime", "innerHTML", toString(".4", x_nHeaderTime))
+  userPageSet("uxMonitor", "lblElapsedTime", "innerHTML", toString(".4", x_nElapsedTime))
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/clearScreen.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/clearScreen.pgx
new file mode 100644
index 0000000..ce019f3
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/clearScreen.pgx
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="clearScreen" access="public">
+    <Code><![CDATA[begin
+  
+  // Do nothing - not needed on CS9
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/debug.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/debug.pgx
new file mode 100644
index 0000000..3721bb9
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/debug.pgx
@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="debug" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nExecStatus" type="num" xsi:type="element" use="reference" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing - not needed on CS9
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/feedbackStatus.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/feedbackStatus.pgx
new file mode 100644
index 0000000..2175748
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/feedbackStatus.pgx
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="feedbackStatus" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nOutConnFlag" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update connection status on port 11002 (feedback server)
+  
+  switch x_nOutConnFlag
+    case -1
+      userPageSet("uxMonitor", "lblFeedbackState", "innerHTML", "connection lost")
+    break
+    case 0
+      userPageSet("uxMonitor", "lblFeedbackState", "innerHTML", "not connected")
+    break
+    case 1
+      userPageSet("uxMonitor", "lblFeedbackState", "innerHTML", "connected")
+    break
+    default
+      userPageSet("uxMonitor", "lblFeedbackState", "innerHTML", "not connected")
+    break
+  endSwitch
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/init.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/init.pgx
new file mode 100644
index 0000000..e57bced
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/init.pgx
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="init" access="public">
+    <Code><![CDATA[begin
+  
+  // Initialize UI
+  
+  userPage("uxMonitor")
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interface/uxMonitor.bindings.json b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interface/uxMonitor.bindings.json
new file mode 100644
index 0000000..fe6d014
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interface/uxMonitor.bindings.json
@@ -0,0 +1,5 @@
+{
+  "version": "1.0",
+  "bindings": [],
+  "callbacks": []
+}
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interface/uxMonitor.html b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interface/uxMonitor.html
new file mode 100644
index 0000000..c8fd0f3
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interface/uxMonitor.html
@@ -0,0 +1,38 @@
+<!DOCTYPE html SYSTEM "-//W3C//DTD VALHTML Strict//EN">
+  <html xmlns="http://www.staubli.com/robotics/2016/03/valhtml">
+    <head>
+      <meta http-equiv="Content-Type" content="text/html; charset=UTF-8"></meta>
+      <meta http-equiv="Cache-Control" content="no-cache, no-store, must-revalidate"></meta>
+      <meta http-equiv="Pragma" content="no-cache"></meta>
+      <meta http-equiv="Expires" content="0"></meta>
+      <script type="text/javascript" src="/vendor/libs/stblUserPage.min.js"></script>
+      <link href="/vendor/libs/up.min.v2.css" rel="stylesheet" media="all" type="text/css"></link>
+      <meta name="src-version" content="8.9.1"></meta>
+    </head>
+    <body style="width: 480px; height: 690px; margin: 0; background-color: #FFFFFF">
+      <label style="top: 8px; position: absolute; left: 8px; font-weight: bold; font-size: 20px; width: 140px; height: 26px" id="lblTitle">ROS-I server</label>
+      <hr id="hl1" style="left: 8px; position: absolute; top: 33px; width: 464px; height: 0px; border-color: #000000"></hr>
+      <label style="top: 59px; position: absolute; left: 18px; font-size: 16px; font-weight: bold; width: 138px; height: 20px" id="lblFeedbackLabel">Feedback server</label>
+      <label style="width: 130px; height: 20px; top: 77px; font-weight: normal; font-size: 12px; font-style: italic; left: 18px; position: absolute" id="lblFeedbackPort">Port 11002</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; top: 59px; position: absolute; left: 172px" id="lblFeedbackState">-</label>
+      <label style="width: 130px; height: 20px; font-weight: normal; font-size: 12px; font-style: italic; left: 18px; position: absolute; top: 129px" id="lblMotionPort">Port 11000</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; left: 18px; position: absolute; top: 111px" id="lblMotionLabel">Motion server</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 172px; position: absolute; top: 111px" id="lblMotionState">-</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; left: 18px; position: absolute; top: 157px" id="lblTrajectoryBufferLabel">Trajectory buffer</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 172px; position: absolute; top: 157px" id="lblTrajectoryBuffer">-</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; top: 157px; position: absolute; left: 324px" id="lblTrajectoryBufferPopped">-</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; left: 18px; position: absolute; top: 185px" id="lblMotionBufferLabel">Motion buffer</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 172px; position: absolute; top: 185px" id="lblMotionBuffer">-</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 324px; position: absolute; top: 185px" id="lblMotionBufferPopped">-</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; left: 18px; position: absolute; top: 213px" id="lblVelocityOverwriteLabel">Velocity overwrite</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 172px; position: absolute; top: 213px" id="lblVelocityOverwrite">-</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; top: 241px; position: absolute; left: 18px" id="lblMoveIdLabel">Move ID</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; top: 241px; position: absolute; left: 172px" id="lblMoveId">-</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 172px; position: absolute; top: 269px" id="lblMotionProgress">-</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; left: 18px; position: absolute; top: 269px" id="lblMotionProgressLabel">Motion progress</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; left: 18px; position: absolute; top: 297px" id="lblHeaderTimeLabel">Header time</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 172px; position: absolute; top: 297px" id="lblHeaderTime">-</label>
+      <label style="font-size: 16px; font-weight: bold; width: 138px; height: 20px; left: 18px; position: absolute; top: 325px" id="lblElapsedTimeLabel">Elapsed time</label>
+      <label style="font-size: 16px; width: 138px; height: 20px; text-align: left; font-weight: normal; left: 172px; position: absolute; top: 325px" id="lblElapsedTime">-</label>
+    </body>
+  </html>
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interfaceCS9.dtx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interfaceCS9.dtx
new file mode 100644
index 0000000..3ab4915
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interfaceCS9.dtx
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Database xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Data/2">
+  <Datas>
+    <Data name="mNomSpeed" access="private" xsi:type="array" type="mdesc" size="1" />
+  </Datas>
+</Database>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interfaceCS9.pjx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interfaceCS9.pjx
new file mode 100644
index 0000000..46e2eb3
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/interfaceCS9.pjx
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project xmlns="http://www.staubli.com/robotics/VAL3/Project/3">
+  <Parameters version="s7.11" stackSize="5000" millimeterUnit="true" />
+  <Programs>
+    <Program file="ack.pgx" />
+    <Program file="clearScreen.pgx" />
+    <Program file="debug.pgx" />
+    <Program file="feedbackStatus.pgx" />
+    <Program file="init.pgx" />
+    <Program file="motionBuffer.pgx" />
+    <Program file="motionProgress.pgx" />
+    <Program file="motionStatus.pgx" />
+    <Program file="start.pgx" />
+    <Program file="stop.pgx" />
+    <Program file="trajBuffer.pgx" />
+    <Program file="velOverwrite.pgx" />
+  </Programs>
+  <Database>
+    <Data file="interfaceCS9.dtx" />
+  </Database>
+  <Libraries>
+    <Library alias="libQueueFuncs" path="Disk://ros_libs/QueueFuncs/QueueFuncs.pjx" />
+  </Libraries>
+</Project>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionBuffer.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionBuffer.pgx
new file mode 100644
index 0000000..7362e89
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionBuffer.pgx
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionBuffer" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMovePts" type="num" xsi:type="element" />
+      <Parameter name="x_nElems" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update motion buffer size
+  
+  userPageSet("uxMonitor", "lblMotionBuffer", "innerHTML", x_nElems)
+  userPageSet("uxMonitor", "lblMotionBufferPopped", "innerHTML", x_nMovePts)
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionProgress.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionProgress.pgx
new file mode 100644
index 0000000..c21d391
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionProgress.pgx
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionProgress" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMoveId" type="num" xsi:type="element" />
+      <Parameter name="x_nMotProg" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // display nMoveId and nMotionProgress
+  
+  userPageSet("uxMonitor", "lblMoveId", "innerHTML", x_nMoveId)
+  userPageSet("uxMonitor", "lblMotionProgress", "innerHTML", x_nMotProg)
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionStatus.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionStatus.pgx
new file mode 100644
index 0000000..14612b8
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/motionStatus.pgx
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionStatus" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nInConnFlag" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update connection status on port 11000 (motion server)
+  
+  switch x_nInConnFlag
+    case -1
+      userPageSet("uxMonitor", "lblMotionState", "innerHTML", "connection lost")
+    break
+    case 0
+      userPageSet("uxMonitor", "lblMotionState", "innerHTML", "not connected")
+    break
+    case 1
+      userPageSet("uxMonitor", "lblMotionState", "innerHTML", "connected")
+    break
+    default
+      userPageSet("uxMonitor", "lblMotionState", "innerHTML", "not connected")
+    break
+  endSwitch
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/start.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/start.pgx
new file mode 100644
index 0000000..15e1740
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/start.pgx
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="start">
+    <Code><![CDATA[begin
+  
+  // Library to handle all of the interface actions for CS9 controller
+  // Loaded at runtime in ros_server()
+  
+  // DO NOT auto-load
+  // Auto-loading will break cross-controller functionality
+  // The correct library will be loaded at runtime, otherwise an error will be generated
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/stop.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/stop.pgx
new file mode 100644
index 0000000..f236f28
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/stop.pgx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="stop">
+    <Code><![CDATA[begin
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/trajBuffer.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/trajBuffer.pgx
new file mode 100644
index 0000000..263528e
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/trajBuffer.pgx
@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="trajBuffer" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nElems" type="num" xsi:type="element" />
+      <Parameter name="x_nPtsPopped" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // update trajectory buffer size
+  
+  userPageSet("uxMonitor", "lblTrajectoryBuffer", "innerHTML", x_nElems)
+  userPageSet("uxMonitor", "lblTrajectoryBufferPopped", "innerHTML", x_nPtsPopped)
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/velOverwrite.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/velOverwrite.pgx
new file mode 100644
index 0000000..e0c841c
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceCS9/velOverwrite.pgx
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="velOverwrite" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_bOverwriteVel" type="bool" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // display velocity overwrite status
+  
+  if(x_bOverwriteVel == true)
+    userPageSet("uxMonitor", "lblVelocityOverwrite", "innerHTML", "enabled")
+  else
+    userPageSet("uxMonitor", "lblVelocityOverwrite", "innerHTML", "disabled")
+  endIf
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/ack.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/ack.pgx
new file mode 100644
index 0000000..a671280
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/ack.pgx
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="ack" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nHeaderTime" type="num" xsi:type="element" />
+      <Parameter name="x_nElapsedTime" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/clearScreen.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/clearScreen.pgx
new file mode 100644
index 0000000..eda0a05
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/clearScreen.pgx
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="clearScreen" access="public">
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/debug.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/debug.pgx
new file mode 100644
index 0000000..9638fa8
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/debug.pgx
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="debug" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nExecStatus" type="num" xsi:type="element" use="reference" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/feedbackStatus.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/feedbackStatus.pgx
new file mode 100644
index 0000000..d53e790
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/feedbackStatus.pgx
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="feedbackStatus" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nOutConnFlag" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/init.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/init.pgx
new file mode 100644
index 0000000..d16ab4a
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/init.pgx
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="init" access="public">
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/interfaceDefault.dtx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/interfaceDefault.dtx
new file mode 100644
index 0000000..c86a38f
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/interfaceDefault.dtx
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Database xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Data/2">
+  <Datas />
+</Database>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/interfaceDefault.pjx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/interfaceDefault.pjx
new file mode 100644
index 0000000..dff6593
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/interfaceDefault.pjx
@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project xmlns="http://www.staubli.com/robotics/VAL3/Project/3">
+  <Parameters version="s7.11" stackSize="5000" millimeterUnit="true" />
+  <Programs>
+    <Program file="ack.pgx" />
+    <Program file="clearScreen.pgx" />
+    <Program file="debug.pgx" />
+    <Program file="feedbackStatus.pgx" />
+    <Program file="init.pgx" />
+    <Program file="motionBuffer.pgx" />
+    <Program file="motionProgress.pgx" />
+    <Program file="motionStatus.pgx" />
+    <Program file="start.pgx" />
+    <Program file="stop.pgx" />
+    <Program file="trajBuffer.pgx" />
+    <Program file="velOverwrite.pgx" />
+  </Programs>
+  <Database>
+    <Data file="interfaceDefault.dtx" />
+  </Database>
+  <Libraries>
+    <Library alias="libQueueFuncs" path="Disk://ros_libs/QueueFuncs/QueueFuncs.pjx" />
+  </Libraries>
+</Project>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionBuffer.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionBuffer.pgx
new file mode 100644
index 0000000..202490c
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionBuffer.pgx
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionBuffer" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMovePts" type="num" xsi:type="element" />
+      <Parameter name="x_nElems" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionProgress.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionProgress.pgx
new file mode 100644
index 0000000..ad668ca
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionProgress.pgx
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionProgress" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nMoveId" type="num" xsi:type="element" />
+      <Parameter name="x_nMotProg" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionStatus.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionStatus.pgx
new file mode 100644
index 0000000..afe1929
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/motionStatus.pgx
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="motionStatus" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nInConnFlag" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/start.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/start.pgx
new file mode 100644
index 0000000..0b9d3af
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/start.pgx
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="start">
+    <Code><![CDATA[begin
+  
+  // Blank interface library to load at runtime
+  // The correct library will be loaded in the application
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/stop.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/stop.pgx
new file mode 100644
index 0000000..f236f28
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/stop.pgx
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="stop">
+    <Code><![CDATA[begin
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/trajBuffer.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/trajBuffer.pgx
new file mode 100644
index 0000000..5e25a43
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/trajBuffer.pgx
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="trajBuffer" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_nElems" type="num" xsi:type="element" />
+      <Parameter name="x_nPtsPopped" type="num" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/velOverwrite.pgx b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/velOverwrite.pgx
new file mode 100644
index 0000000..c1bc692
--- /dev/null
+++ b/staubli_val3_driver/val3/ros_libs/UserInterface/interfaceDefault/velOverwrite.pgx
@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="velOverwrite" access="public">
+    <Parameters xmlns="http://www.staubli.com/robotics/VAL3/Param/1">
+      <Parameter name="x_bOverwriteVel" type="bool" xsi:type="element" />
+    </Parameters>
+    <Code><![CDATA[begin
+  
+  // Do nothing 
+  // See start() program for more details
+  
+end]]></Code>
+  </Program>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_server/ros_server.dtx b/staubli_val3_driver/val3/ros_server/ros_server.dtx
index 0ded1f8..71a4f3a 100644
--- a/staubli_val3_driver/val3/ros_server/ros_server.dtx
+++ b/staubli_val3_driver/val3/ros_server/ros_server.dtx
@@ -266,8 +266,6 @@
     <Data name="sScreenTaskName" access="public" xsi:type="array" type="string" size="1" />
     <Data name="nInConnFlag" access="public" xsi:type="array" type="num" size="1" />
     <Data name="nOutConnFlag" access="public" xsi:type="array" type="num" size="1" />
-    <Data name="scDebug" access="public" xsi:type="array" type="screen" size="1" />
-    <Data name="scMonitor" access="public" xsi:type="array" type="screen" size="1" />
     <Data name="sHtbtTaskName" access="public" xsi:type="array" type="string" size="1" />
     <Data name="nPtsPopped" access="public" xsi:type="array" type="num" size="1" />
     <Data name="bStopNow" access="public" xsi:type="array" type="bool" size="1" />
diff --git a/staubli_val3_driver/val3/ros_server/ros_server.pjx b/staubli_val3_driver/val3/ros_server/ros_server.pjx
index d15ee8a..3b5f2dd 100644
--- a/staubli_val3_driver/val3/ros_server/ros_server.pjx
+++ b/staubli_val3_driver/val3/ros_server/ros_server.pjx
@@ -40,6 +40,7 @@
   </Database>
   <Libraries>
     <Library alias="libQueueFuncs" path="Disk://ros_libs/QueueFuncs/QueueFuncs.pjx" />
+    <Library alias="libInterface" path="Disk://ros_libs/UserInterface/interfaceDefault/interfaceDefault.pjx" autoload="false" />
   </Libraries>
   <Types>
     <Type name="RosSimpleMsg" path="Disk://ros_libs/RosSimpleMsg/RosSimpleMsg.pjx" />
diff --git a/staubli_val3_driver/val3/ros_server/screenUpdate.pgx b/staubli_val3_driver/val3/ros_server/screenUpdate.pgx
index 19cb3aa..45c7365 100644
--- a/staubli_val3_driver/val3/ros_server/screenUpdate.pgx
+++ b/staubli_val3_driver/val3/ros_server/screenUpdate.pgx
@@ -6,7 +6,7 @@
       <Local name="l_nExecStatus" type="num" xsi:type="array" size="1" />
       <Local name="l_nElems" type="num" xsi:type="array" size="1" />
     </Locals>
-    <Code><![CDATA[begin  
+    <Code><![CDATA[begin
   // Copyright (c) 2016, Ocado Technology - Robotics Research Team
   //
   // Licensed under the Apache License, Version 2.0 (the "License");
@@ -21,144 +21,64 @@
   // See the License for the specific language governing permissions and
   // limitations under the License.
 
-
-
   l_nExecStatus = 0
-  userPage(scMonitor)
-  cls(scMonitor)
-  title(scMonitor, "ROS-I server")
+
+  call libInterface:init()
   
-  while (true)
+  while(true)
+    
     // clear screen
-    cls(scMonitor)
+    call libInterface:clearScreen()
     
-    // toggle velocity overwrite upon release of F1 key
-    l_nKey = getKey(scMonitor)
-    call libQueueFuncs:getNumElems(qMoveBuffer,l_nElems)
-    if (l_nKey == 277 and bOverwriteVel == false)
-      if (l_nElems == 0)
-        bOverwriteVel = true
-      else
-        popUpMsg("Motion buffer not empty; cannot enable V/OW")
-      endIf
-    elseIf (l_nKey == 278 and bOverwriteVel == true)
-      if (l_nElems == 0)
-        bOverwriteVel = false
-      else
-        popUpMsg("Motion buffer not empty; cannot disable V/OW")
-      endIf
-    endIf
+    // @TODO: Remove all code related to velocity overwrite
+    //    // toggle velocity overwrite upon release of F1 key
+    //    l_nKey = getKey(scMonitor)
+    //    call libQueueFuncs:getNumElems(qMoveBuffer,l_nElems)
+    //    if (l_nKey == 277 and bOverwriteVel == false)
+    //      if (l_nElems == 0)
+    //        bOverwriteVel = true
+    //      else
+    //        popUpMsg("Motion buffer not empty; cannot enable V/OW")
+    //      endIf
+    //    elseIf (l_nKey == 278 and bOverwriteVel == true)
+    //      if (l_nElems == 0)
+    //        bOverwriteVel = false
+    //      else
+    //        popUpMsg("Motion buffer not empty; cannot disable V/OW")
+    //      endIf
+    //    endIf
     
     // update connection status on port 11000
-    gotoxy(scMonitor, 0,0)
-    // 11 characteres: gotoxy(12,0) to insert status
-    put(scMonitor, "Port 11000:")
-    gotoxy(scMonitor, 12,0)
-    switch nInConnFlag
-      case -1
-        put(scMonitor, "connection lost")
-      break
-      case 0
-        put(scMonitor, "not connected")
-      break
-      case 1
-        put(scMonitor, "connected")
-      break
-      default
-        put(scMonitor, "not connected")
-      break
-    endSwitch
+    call libInterface:motionStatus(nInConnFlag)
     
     // update connection status on port 11002
-    gotoxy(scMonitor, 0,1)
-    // 11 characteres: gotoxy(12,1) to insert status
-    put(scMonitor, "Port 11002:")
-    gotoxy(scMonitor, 12,1)
-    switch nOutConnFlag
-      case -1
-        put(scMonitor, "connection lost")
-      break
-      case 0
-        put(scMonitor, "not connected")
-      break
-      case 1
-        put(scMonitor, "connected")
-      break
-      default
-        put(scMonitor, "not connected")
-      break
-    endSwitch
+    call libInterface:feedbackStatus(nOutConnFlag)
     
     // update trajectory buffer size
     call libQueueFuncs:getNumElems(qTrajPtBuffer,l_nElems)
-    gotoxy(scMonitor, 0,3)
-    // 18 characteres: gotoxy(19,3) to insert status
-    put(scMonitor, "Trajectory buffer:")
-    gotoxy(scMonitor, 19,3)
-    put(scMonitor, l_nElems)
-    // also print number of points popped from buffer
-    gotoxy(scMonitor, 24,3)
-    put(scMonitor, nPtsPopped)
+    call libInterface:trajBuffer(l_nElems,nPtsPopped)
+    
     // do the same for move buffer
     call libQueueFuncs:getNumElems(qMoveBuffer,l_nElems)
-    gotoxy(scMonitor, 0,4)
-    put(scMonitor, "Motion buffer:")
-    gotoxy(scMonitor, 19,4)
-    put(scMonitor, l_nElems)
-    gotoxy(scMonitor, 24,4)
-    put(scMonitor, nMovePts)
+    call libInterface:motionBuffer(l_nElems,nPtsPopped)
     
     // display velocity overwrite status
-    gotoxy(scMonitor, 0,5)
-    put(scMonitor, "Velocity overwrite:")
-    gotoxy(scMonitor, 20,5)
-    if (bOverwriteVel == true)
-      put(scMonitor, "enabled")
-    else
-      put(scMonitor, "disabled")
-    endIf
+    call libInterface:velOverwrite(bOverwriteVel)
     
     // display nMoveId and nMotionProgress
-    gotoxy(scMonitor, 0,7)
-    put(scMonitor, "moveId:")
-    gotoxy(scMonitor, 8,7)
-    put(scMonitor, nMoveId)
-    gotoxy(scMonitor, 13,7)
-    put(scMonitor, "progress:")
-    gotoxy(scMonitor, 23,7)
-    put(scMonitor, nMotionProgress)
+    call libInterface:motionProgress(nMoveId,nMotionProgress)
     
     // display time between valid trajectory point received and ACK sent
-    gotoxy(scMonitor, 0,9)
-    put(scMonitor, "recvMsgHeader:")
-    gotoxy(scMonitor, 15,9)
-    put(scMonitor, toString(".4", nHeaderTime))
-    
-    gotoxy(scMonitor, 0,10)
-    put(scMonitor, "other states:")
-    gotoxy(scMonitor, 15,10)
-    put(scMonitor, toString(".4", nElapsedTime))
+    call libInterface:ack(nHeaderTime,nElapsedTime)
     
     // update debug status
-    gotoxy(scMonitor, 18,12)
-    switch l_nExecStatus
-      case 0
-        put(scMonitor, "[+]")
-        l_nExecStatus = 1
-      break
-      case 1
-        put(scMonitor, "[-]")
-        l_nExecStatus = 0
-      break
-      default
-      break
-    endSwitch
+    call libInterface:debug(l_nExecStatus)
     
-    // add Function keys labels
-    gotoxy(scMonitor, 30,13)
-    put(scMonitor, "V/ON")
-    gotoxy(scMonitor, 35,13)
-    put(scMonitor, "V/OFF")
+    //    // add Function keys labels
+    //    gotoxy(scMonitor, 30,13)
+    //    put(scMonitor, "V/ON")
+    //    gotoxy(scMonitor, 35,13)
+    //    put(scMonitor, "V/OFF")
     
     // sequence task
     //delay(0)
@@ -166,4 +86,4 @@
   endWhile
 end]]></Code>
   </Program>
-</Programs>
+</Programs>
\ No newline at end of file
diff --git a/staubli_val3_driver/val3/ros_server/start.pgx b/staubli_val3_driver/val3/ros_server/start.pgx
index 46369c9..3eeb66c 100644
--- a/staubli_val3_driver/val3/ros_server/start.pgx
+++ b/staubli_val3_driver/val3/ros_server/start.pgx
@@ -28,19 +28,32 @@
   // See the License for the specific language governing permissions and
   // limitations under the License.
 
-  // Check for VAL 3 version >= 7.7
 
-  l_sVersionMajor = mid(getVersion("VAL3"),1,1)
-  l_sVersionMinor = mid(getVersion("VAL3"),2,1)
-  toNum(l_sVersionMajor, l_nVersionMajor, l_bOkMajor)
-  toNum(l_sVersionMinor, l_nVersionMinor, l_bOkMinor)
-  if(!l_bOkMajor or !l_bOkMinor)
-    popUpMsg("Error reading VAL 3 version")
-    taskKill("ros_server~")
-  endIf
-  if( !((l_nVersionMajor == 7) and (l_nVersionMinor >= 7)))
-    popUpMsg("Error: VAL 3 version is < 7.7")
-    taskKill("ros_server~")
+  // Load interface library for CS8 or CS9
+  //
+  // CS9 handles the UI differently than CS8, requires
+  // the UI funtions to be loaded in seperate libraries
+  
+  if(mid(getVersion("System"),1,1) == "7")
+    // CS8 - VAL 3 rev 7
+    // Check for VAL 3 version >= 7.7
+    l_sVersionMajor = mid(getVersion("VAL3"),1,1)
+    l_sVersionMinor = mid(getVersion("VAL3"),2,1)
+    toNum(l_sVersionMajor, l_nVersionMajor, l_bOkMajor)
+    toNum(l_sVersionMinor, l_nVersionMinor, l_bOkMinor)
+    if(!l_bOkMajor or !l_bOkMinor)
+      popUpMsg("Error reading VAL 3 version")
+      taskKill("ros_server~")
+    endIf
+    if( !((l_nVersionMajor == 7) and (l_nVersionMinor >= 7)))
+      // VAL 3 version is too old
+      popUpMsg("Error: VAL 3 version is < 7.7, socket communication will not work!")
+      taskKill("ros_server~")
+    endIf
+    libInterface:libLoad("ros_libs/UserInterface/interfaceCS8")
+  elseIf(mid(getVersion("System"),1,1) == "8")
+    // CS9 - VAL 3 rev 8
+    libInterface:libLoad("ros_libs/UserInterface/interfaceCS9")
   endIf
 
   // feedback task will run every 20ms (50Hz)
diff --git a/staubli_val3_driver/val3/ros_server/stop.pgx b/staubli_val3_driver/val3/ros_server/stop.pgx
index 51fe3f6..53fec5d 100644
--- a/staubli_val3_driver/val3/ros_server/stop.pgx
+++ b/staubli_val3_driver/val3/ros_server/stop.pgx
@@ -1,6 +1,6 @@
-<?xml version="1.0" encoding="utf-8" ?>
-<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2" >
-  <Program name="stop" access="private" >
+<?xml version="1.0" encoding="utf-8"?>
+<Programs xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns="http://www.staubli.com/robotics/VAL3/Program/2">
+  <Program name="stop">
     <Code><![CDATA[begin
   // Copyright (c) 2016, Ocado Technology - Robotics Research Team
   //
@@ -20,7 +20,7 @@
 
   resetMotion()
   popUpMsg("Pending movement commands have been cancelled.")
-  cls()
+  call libInterface:clearScreen()
 end]]></Code>
   </Program>
-</Programs>
+</Programs>
\ No newline at end of file