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,7 +157,7 @@ while not keyboard.is_pressed("q"):
                 #print("Received :", repr(data))
                 if (len(lastdata)>1):
                     splitdata=lastdata[-2].split(",")
-                
+                    if len(splitdata)==2:
                         try:
                             _pan=float(splitdata[0])
                             _tilt=float(splitdata[1])
@@ -195,17 +214,16 @@ 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)
 
+    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