philipp.kramer
/
urcontrol
1
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

add manual control

This commit is contained in:
Philipp Kramer 2022-08-23 10:01:34 +02:00
parent 3fe2bcbe19
commit 4327d8f162
1 changed files with 34 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
pantilt_inv.py
View File

@ -159,6 +159,11 @@ speed=speed_1
joint_min=[toRad(-90),toRad(-90-40),toRad(90-90),toRad(-180-80),toRad(-90-90),toRad(0-75)]
joint_max=[toRad(90),toRad(-90+40),toRad(90+40),toRad(-180+110),toRad(-90+90),toRad(0+75)]
manual_pan=0
manual_tilt=0
manual_last_time=0
manual_speed=0.5
def constrain(v,_min,_max):
return min(_max,max(_min,v))
@ -192,10 +197,26 @@ while running:
speed=speed_3
if event.key == pygame.K_4:
speed=speed_4
if event.type == pygame.QUIT:
running = False
# Live key movement
if time.time()+0.1 >= manual_last_time: #limit frequency. in seconds
manual_last_time=time.time()
keys = pygame.key.get_pressed()
if keys[pygame.K_LEFT]:
manual_pan -= manual_speed
if keys[pygame.K_RIGHT]:
manual_pan += manual_speed
if keys[pygame.K_UP]:
manual_tilt += manual_speed
if keys[pygame.K_DOWN]:
manual_tilt -= manual_speed
#print("Manual pan="+str(manual_pan)+" tilt="+str(manual_tilt)+" time="+str(time.time()))
# Receive Socket stuff
try:
client, address = s.accept()
@ -232,6 +253,15 @@ while running:
print("Not a float")
except socket.error:
pass
else:
_pan=manual_pan
_tilt=manual_tilt
if _pan>=-winkellimit and _pan <=winkellimit and _tilt>=-winkellimit and _tilt <=winkellimit:
pan=_pan
tilt=_tilt
# Robot Control stuff
@ -291,9 +321,10 @@ while running:
for i in range(6):
drawBar(screen,(10,10+30*(3+i)),(100,20),mapFromTo(joint_q[i],joint_min[i],joint_max[i],0.0,1.0),jointnames[i],str(round(toDeg(joint_q[i]),2))+"°")
for i in range(6):
_jointdiff = joint_q_aim[i]-joint_q[i]
drawBar(screen,(200,10+30*(3+i)),(100,20),mapFromTo(_jointdiff,-speed[i]/frequency,speed[i]/frequency,0.0,1.0),"diff_"+str(jointnames[i]),str(round(toDeg(_jointdiff),3))+"°")
if use_rtde:
for i in range(6):
_jointdiff = joint_q_aim[i]-joint_q[i]
drawBar(screen,(200,10+30*(3+i)),(100,20),mapFromTo(_jointdiff,-speed[i]/frequency,speed[i]/frequency,0.0,1.0),"diff_"+str(jointnames[i]),str(round(toDeg(_jointdiff),3))+"°")