keyboard input through pygame

tests/pantilt_inv.py

@@ -2,9 +2,14 @@ import rtde_control
 import time
 import math
 import socket
-import keyboard # pip install keyboard
 import numpy as np
 
+import pygame
+
+pygame.init()
+screen = pygame.display.set_mode([500, 500])
+running = True
+
 # Socket stuff
 s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
 host =""
@@ -59,7 +64,7 @@ def RPYtoVec(roll,pitch,yaw):
 
     return rx, ry, rz
 
-paused=False
+paused=True
 
 actual_q = rtde_r.getActualQ() #current joint rotations in radians.
 print("Actual Q:"+str(actual_q))
@@ -109,19 +114,33 @@ speed=[1,1,1,3,3,3]
 def constrain(v,_min,_max):
     return min(_max,max(_min,v))
 
-print("Press s to start")
+
 print("Press q to stop")
 print("Press p to pause")
-while not keyboard.is_pressed("s"):
-    if keyboard.is_pressed("q"):
-        exit()
-    time.sleep(0.1)
+print("Press s to resume")
+
 
 print("Starting Servo")
-while not keyboard.is_pressed("q"):
+while running:
     
     start = time.time()
 
+
+
+    for event in pygame.event.get():
+        if event.type == pygame.KEYDOWN:
+            if event.key == pygame.K_p and paused==False:
+                print("paused")
+                paused=True
+            if event.key == pygame.K_s and paused==True:
+                print("resumed")
+                paused=False
+            if event.key == pygame.K_q:
+                print("stop")
+                running=False
+        if event.type == pygame.QUIT:
+            running = False
+
     # Receive Socket stuff
     try:
         client, address = s.accept()
@@ -138,19 +157,19 @@ while not keyboard.is_pressed("q"):
                 #print("Received :", repr(data))
                 if (len(lastdata)>1):
                     splitdata=lastdata[-2].split(",")
-                
-                    try:
-                        _pan=float(splitdata[0])
-                        _tilt=float(splitdata[1])
+                    if len(splitdata)==2:
+                        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
+                            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")
+                            print("Pan="+str(pan)+" Tilt="+str(tilt))
+                        except ValueError:
+                            print("Not a float")
         except socket.error:
             pass
 
@@ -195,16 +214,15 @@ while not keyboard.is_pressed("q"):
         
 
 
-    if keyboard.is_pressed("p"):
-        print("paused")
-        paused=True
-    if keyboard.is_pressed("s"):
-        print("resumed")
-        paused=False
     
     #print(joint_q)
 
-    rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain)
+    if not paused:
+        rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain)
+
+    screen.fill((255, 255, 255))
+    pygame.draw.circle(screen, (0, 0, 255), (250, 250), 75)
+    pygame.display.flip()
     
     end = time.time()
     stepduration = end - start