diff --git a/pantilt_inv.py b/pantilt_inv.py index 049aa01..3f8371d 100644 --- a/pantilt_inv.py +++ b/pantilt_inv.py @@ -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))+"°")