From 621a353140d0ec52d0e4dee778b06bd3f6b7a410 Mon Sep 17 00:00:00 2001
From: Philipp Kramer <philipp.kramer@theater.digital>
Date: Tue, 31 May 2022 16:18:56 +0200
Subject: [PATCH] add pan tilt movement

---
 tests/pantilt.py           | 80 +++++++++++++++++++++++++++++++-------
 tests/socketreceivetest.py | 13 +++++--
 2 files changed, 75 insertions(+), 18 deletions(-)

diff --git a/tests/pantilt.py b/tests/pantilt.py
index fdba009..8090125 100644
--- a/tests/pantilt.py
+++ b/tests/pantilt.py
@@ -3,10 +3,22 @@ import rtde_control
 import time
 import math
 import socket
+import keyboard # pip install keyboard
+
+# Socket stuff
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+host =""
+port =30002
+s.bind((host,port))
+s.listen(1) # Number of connections
+s.setblocking(False)
+client = None
+
+
+# Robot Control Stuff
 
 rtde_c = rtde_control.RTDEControlInterface("192.168.1.101")
 
-
 import rtde_receive
 rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.101")
 
@@ -35,37 +47,74 @@ rtde_c.moveJ(joint_q,0.1,0.1) #move to initial position
 
 
 # Parameters
-duration = 20 #in seconds
 
-velocity = 0.1   #0.5
+
+velocity = 0.2   #0.5
 acceleration = 0.1   #0.5
 frequency= 100
 dt = 1.0/frequency  # 2ms
 lookahead_time = 0.1
 gain = 300
 
-speed=0.1/frequency
+speed=1/frequency
 
 
 def constrain(v,_min,_max):
     return min(_max,max(_min,v))
 
-print("Starting Servo")
-for i in range(frequency*duration):
-    start = time.time()
-    
-    joint_q_aim = [0,toRad(-90),toRad(90-tilt/2),toRad(-180-tilt/2),toRad(-90-pan),toRad(0)]
+print("Press s to start!")
+while not keyboard.is_pressed("s"):
+    time.sleep(0.1)
+
+print("Starting Servo")
+while not keyboard.is_pressed("q"):
+    
+    start = time.time()
+
+    # Receive Socket stuff
+    try:
+        client, address = s.accept()
+        print("Connected to", address)
+
+    except socket.error:
+        pass
+
+    if client is not None:
+        try:
+            data = client.recv( 1024 ).decode( 'utf-8' )
+            if data:
+                lastdata=data.split('\n')
+                #print("Received :", repr(data))
+                if (len(lastdata)>1):
+                    splitdata=lastdata[-2].split(",")
+                
+                    try:
+                        _pan=float(splitdata[0])
+                        _tilt=float(splitdata[1])
+
+                        winkellimit=45
+                        if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit:
+                            pan=_pan
+                            tilt=_tilt
+
+                        print("Pan="+str(pan)+" Tilt="+str(tilt))
+                    except ValueError:
+                        print("Not a float")
+        except socket.error:
+            pass
+
+
+    # Robot Control stuff
+
+    
+    joint_q_aim = [0,toRad(-90),toRad(90),toRad(-180-tilt),toRad(-90-pan),toRad(0)]
 
-    if (round(i/(frequency*5))%2==1):
-        tilt=20
-    else:
-        tilt=-20
 
     for i in range(6):
         joint_q[i] += constrain(joint_q_aim[i]-joint_q[i], -speed,speed)
         
     
-    print(joint_q)
+    #print(joint_q)
 
     rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain)
     
@@ -82,3 +131,6 @@ rtde_c.stopScript()
 
 
 
+
+
+s.close()
\ No newline at end of file
diff --git a/tests/socketreceivetest.py b/tests/socketreceivetest.py
index 9ead2da..932378a 100644
--- a/tests/socketreceivetest.py
+++ b/tests/socketreceivetest.py
@@ -22,11 +22,16 @@ while True:
             data = client.recv( 1024 ).decode( 'utf-8' )
             if data:
                 lastdata=data.split('\n')
-                splitdata=lastdata[-2].split(",")
-                print("lastdata="+str(lastdata))
-                
                 #print("Received :", repr(data))
-                print(splitdata)
+                if (len(lastdata)>1):
+                    splitdata=lastdata[-2].split(",")
+                
+                    try:
+                        pan=float(splitdata[0])
+                        tilt=float(splitdata[1])
+                        print("Pan="+str(pan)+" Tilt="+str(tilt))
+                    except ValueError:
+                        print("Not a float")
         except socket.error:
             pass