Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

cleaned up control urscript

Browse Source 
Less verbose output and more explicit shutdown. Part of #13
This commit is contained in:
Felix Mauch
2019-06-05 11:41:15 +02:00
committed by Tristan Schnell
parent 7b6175d248
commit 673459e9de
1 changed files with 11 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
ur_rtde_driver/resources/servoj.urscript
View File

@@ -2,6 +2,7 @@ global steptime = get_steptime()
textmsg("steptime=", steptime) textmsg("steptime=", steptime)
global MULT_jointstate = {{JOINT_STATE_REPLACE}} global MULT_jointstate = {{JOINT_STATE_REPLACE}}
global SERVO_STOPPED = -2
global SERVO_UNINITIALIZED = -1 global SERVO_UNINITIALIZED = -1
global SERVO_IDLE = 0 global SERVO_IDLE = 0
global SERVO_RUNNING = 1 global SERVO_RUNNING = 1
@@ -28,7 +29,7 @@ end
thread servoThread(): thread servoThread():
state = SERVO_IDLE state = SERVO_IDLE
while True: while state > SERVO_STOPPED:
enter_critical enter_critical
q = cmd_servo_q q = cmd_servo_q
do_extrapolate = False do_extrapolate = False
@@ -36,7 +37,9 @@ thread servoThread():
do_extrapolate = True do_extrapolate = True
end end
state = cmd_servo_state state = cmd_servo_state
cmd_servo_state = SERVO_IDLE if cmd_servo_state > SERVO_UNINITIALIZED:
cmd_servo_state = SERVO_IDLE
end
exit_critical exit_critical
if do_extrapolate: if do_extrapolate:
textmsg("No new setpoint received. Extrapolating.") textmsg("No new setpoint received. Extrapolating.")
@@ -45,10 +48,10 @@ thread servoThread():
elif state == SERVO_RUNNING: elif state == SERVO_RUNNING:
servoj(q, t=steptime, {{SERVO_J_REPLACE}}) servoj(q, t=steptime, {{SERVO_J_REPLACE}})
else: else:
textmsg("Should not be here")
sync() sync()
end end
end end
textmsg("servo thread ended")
stopj(0.1) stopj(0.1)
end end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
@@ -58,16 +61,19 @@ keepalive = -2
params_mult = socket_read_binary_integer(6+1, "reverse_socket") params_mult = socket_read_binary_integer(6+1, "reverse_socket")
keepalive = params_mult[7] keepalive = params_mult[7]
while keepalive > 0: while keepalive > 0:
params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.2) # steptime could work as well, but does not work in simulation params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
if params_mult[0] > 0: if params_mult[0] > 0:
keepalive = params_mult[7] keepalive = params_mult[7]
q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
set_servo_setpoint(q) set_servo_setpoint(q)
else: else:
# TODO: Extrapolation goes here
keepalive = keepalive - 1 keepalive = keepalive - 1
end end
end end
textmsg("Stopping communication and servoing")
enter_critical
cmd_servo_state = SERVO_STOPPED
exit_critical
sleep(.1) sleep(.1)
socket_close() socket_close()
kill thread_servo kill thread_servo