philipp.kramer/urcontrol
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

keyboard input through pygame

Browse source
This commit is contained in:
Philipp Kramer 2022-06-09 15:47:27 +02:00
parent b19b962c90
commit a2b69f8188
1 changed files with 44 additions and 26 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
tests/pantilt_inv.py
View file

@ -2,9 +2,14 @@ import rtde_control
import time import time
import math import math
import socket import socket
import keyboard # pip install keyboard
import numpy as np import numpy as np
import pygame
pygame.init()
screen = pygame.display.set_mode([500, 500])
running = True
# Socket stuff # Socket stuff
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
host ="" host =""
@ -59,7 +64,7 @@ def RPYtoVec(roll,pitch,yaw):
return rx, ry, rz return rx, ry, rz
paused=False paused=True
actual_q = rtde_r.getActualQ() #current joint rotations in radians. actual_q = rtde_r.getActualQ() #current joint rotations in radians.
print("Actual Q:"+str(actual_q)) print("Actual Q:"+str(actual_q))
@ -109,19 +114,33 @@ speed=[1,1,1,3,3,3]
def constrain(v,_min,_max): def constrain(v,_min,_max):
return min(_max,max(_min,v)) return min(_max,max(_min,v))
print("Press s to start")
print("Press q to stop") print("Press q to stop")
print("Press p to pause") print("Press p to pause")
while not keyboard.is_pressed("s"): print("Press s to resume")
if keyboard.is_pressed("q"):
exit()
time.sleep(0.1)
print("Starting Servo") print("Starting Servo")
while not keyboard.is_pressed("q"): while running:
start = time.time() 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 # Receive Socket stuff
try: try:
client, address = s.accept() client, address = s.accept()
@ -138,7 +157,7 @@ while not keyboard.is_pressed("q"):
#print("Received :", repr(data)) #print("Received :", repr(data))
if (len(lastdata)>1): if (len(lastdata)>1):
splitdata=lastdata[-2].split(",") splitdata=lastdata[-2].split(",")
if len(splitdata)==2:
try: try:
_pan=float(splitdata[0]) _pan=float(splitdata[0])
_tilt=float(splitdata[1]) _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) #print(joint_q)
if not paused:
rtde_c.servoJ(joint_q, velocity, acceleration, dt, lookahead_time, gain) 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() end = time.time()
stepduration = end - start stepduration = end - start