diff --git a/urosc.py b/urosc.py
index 74f0053..09d173a 100644
--- a/urosc.py
+++ b/urosc.py
@@ -8,7 +8,8 @@ import math
 import numpy as np
 import curses
 
-from rtde_receive import RTDEReceiveInterface as RTDEReceive
+import rtde_receive
+import rtde_control
 
 
 
@@ -16,13 +17,30 @@ from rtde_receive import RTDEReceiveInterface as RTDEReceive
 
 clients = []
 rtde_r=None
+rtde_c=None
 server=None
 
 #Looptimings
 frequency_loop=500
 frequency_oscsend=50
 frequency_rtde=None 
-frequency_scr=30
+frequency_scr=25
+
+#Robot Control Parameters
+# Parameters
+velocity = 0.1
+acceleration = 0.1
+lookahead_time = 0.1
+gain = 300
+joint_q_aim=[0,0,0,0,0,0] #joint rotations aiming for
+joint_q = [0,0,0,0,0,0] #joint rotation to be send
+velocitylimit=[1,1,1,3,3,3] #joint velocity limits
+
+
+#osc receive statistics
+oscreceiveintervals = np.zeros(100)
+oscreceiveintervals_pos=0
+last_oscreceive=0
 
 def toRad(d):
     return d/360*2*math.pi
@@ -35,15 +53,25 @@ def mapFromTo(x,a,b,c,d):
    y=(x-a)/(b-a)*(d-c)+c
    return y
 
-def handle_received_osc(address, *args): 
+def constrain(v,_min,_max):
+    return min(_max,max(_min,v))
+
+def handle_received_osc_joint(address, *args): 
+    oscreceiveintervals[oscreceiveintervals_pos]=time.time()-last_oscreceive
+    last_oscreceive=time.time()
+    oscreceiveintervals_pos+=1
+    oscreceiveintervals_pos%=np.size(oscreceiveintervals)
+    
     print(f"Received OSC message on {address}") 
-    for arg in args: 
-        print(f"Data: {arg}") 
+    for iarg, arg in enumerate(args): 
+        #print(f"Data: {arg}") 
+        joint_q_aim[iarg]=float(arg)
+
  
 def setupOSC(ip, port): 
     # Set up the dispatcher for the server 
     disp = dispatcher.Dispatcher() 
-    disp.map("/filter", handle_received_osc)  # You can change '/filter' to any address pattern you expect 
+    disp.map("/joint", handle_received_osc_joint)  # You can change '/filter' to any address pattern you expect 
  
     # Set up OSC server 
     server = osc_server.ThreadingOSCUDPServer((ip, port), disp) 
@@ -67,7 +95,9 @@ def setupClients(clientips,port):
 
 def setupRTDE(robotip,frequency):
     dt = 1 / frequency
-    rtde_r = RTDEReceive(robotip, frequency)
+    rtde_r = rtde_receive.RTDEReceiveInterface(robotip, frequency)
+
+    rtde_c = rtde_control.RTDEControlInterface(robotip)
 
     return rtde_r
  
@@ -109,19 +139,21 @@ def main(stdscr):
 
     last_sendosc=0
     last_receivertde=0
+    last_sendrtde=0
     last_updatescr=0
 
     q=[0,0,0,0,0,0]
+
     
     try:
         while key != ord('q'):
-            looptime=time.time()
+            looptime = time.time()
             
             start = time.time()
             
 
             #Get robot joint angled
-            if looptime-last_receivertde>=1.0/frequency_rtde:
+            if looptime-last_receivertde>=1.0/frequency_receivertde:
                 last_receivertde=looptime
 
                 if rtde_r is not None:
@@ -141,6 +173,15 @@ def main(stdscr):
                     for jointi in range(6):
                         c.send_message("/j"+str(jointi), toDeg(q[jointi]))
 
+            if looptime-last_sendrtde>=1.0/frequency_sendrtde:
+                last_sendrtde=looptime
+
+                for i in range(6): #update joint_q with max speeds
+                    joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -velocitylimit[i]/frequency_sendrtde,velocitylimit[i]/frequency_sendrtde) #constrain joint speeds
+
+                if rtde_c is not None:
+                    rtde_c.servoJ(joint_q, velocity, acceleration, 1.0/frequency_sendrtde, lookahead_time, gain)
+
 
             #Display
             if looptime-last_updatescr>=1.0/frequency_scr:
@@ -148,8 +189,9 @@ def main(stdscr):
                 key = stdscr.getch()
                 stdscr.clear()
 
-                stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(3))
+                stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(1))
                 stdscr.addstr(1, 0, "loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)), curses.color_pair(1))
+                stdscr.addstr(2, 0, "osc receive frequency="+str(round(1.0/np.mean(oscreceiveintervals),0))+" \t pos="+str(oscreceiveintervals_pos), curses.color_pair(1))
                 
                 startbarsx=10
                 endbarsx=width-1
@@ -170,11 +212,11 @@ def main(stdscr):
                             poschar="]"
                             
                         if (len(poschar)>0):
-                            stdscr.addstr(2+j, px, poschar, curses.color_pair(1))
+                            stdscr.addstr(3+j, px, poschar, curses.color_pair(1))
                     valuetext=str(round(toDeg(q[j]),2))
                     if q[j]>=0: #has no negative sign
                         valuetext=" "+valuetext
-                    stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1))
+                    stdscr.addstr(3+j, 0, valuetext, curses.color_pair(1))
 
                     
                 # Refresh the screen
@@ -187,6 +229,9 @@ def main(stdscr):
             loopdurations_pos+=1
             loopdurations_pos%=np.size(loopdurations)
 
+            
+            
+
             if loopduration < loop_dt:
                 time.sleep(loop_dt - loopduration)
             loopcounter += 1
@@ -217,16 +262,27 @@ if __name__ == "__main__":
     
     
     parser.add_argument(
-        "-f",
-        "--frequency",
-        dest="frequency",
-        help="the frequency at which the data is recorded (default is 500Hz)",
+        "-fs",
+        "--frequencysend",
+        dest="frequencysend",
+        help="the frequency at which the robot control data is send",
         type=float,
         default=500.0,
         metavar="<frequency>")
+
+    parser.add_argument(
+        "-fr",
+        "--frequencyreceive",
+        dest="frequencyreceive",
+        help="the frequency at which the robot actualq data is received",
+        type=float,
+        default=500.0,
+        metavar="<frequency>")
+    
     args = parser.parse_args()
 
-    frequency_rtde=args.frequency
+    frequency_receivertde=args.frequencyreceive
+    frequency_sendrtde=args.frequencysend
 
     clientips=[args.clientip]
  
@@ -245,4 +301,7 @@ if __name__ == "__main__":
         input("Ended. Press Enter to shutdown")
         server.shutdown() 
         server_thread.join() 
+    if rtde_c is not None:
+        rtde_c.servoStop()
+        rtde_c.stopScript()