Skip to content
Snippets Groups Projects
Commit 73b690b6 authored by ipa-fxm's avatar ipa-fxm
Browse files

restrutcture prog and temporarily remove publishing of flags to reduce load on ur controller

parent 33357bba
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
ur_driver/prog
+ 52
− 6
View file @ 73b690b6
......@@ -43,7 +43,6 @@ def driverProg():
q = get_joint_positions()
qdot = get_joint_speeds()
tau = get_joint_torques()
wrench = get_tcp_force()
enter_critical
socket_send_int(MSG_JOINT_STATES)
socket_send_int(floor(MULT_jointstate * q[0]))
......@@ -64,6 +63,22 @@ def driverProg():
socket_send_int(floor(MULT_jointstate * tau[3]))
socket_send_int(floor(MULT_jointstate * tau[4]))
socket_send_int(floor(MULT_jointstate * tau[5]))
#socket_send_int(7895160) # Recognizable ".xxx" or 00787878
exit_critical
end
#socket_open(HOSTNAME, 50002)
send_joint_state()
while True:
send_joint_state()
sync()
end
sync()
end
thread wrench_statePublisherThread():
def send_wrench_state():
wrench = get_tcp_force()
enter_critical
socket_send_int(MSG_WRENCH)
socket_send_int(floor(MULT_wrench * wrench[0]))
socket_send_int(floor(MULT_wrench * wrench[1]))
......@@ -71,28 +86,55 @@ def driverProg():
socket_send_int(floor(MULT_wrench * wrench[3]))
socket_send_int(floor(MULT_wrench * wrench[4]))
socket_send_int(floor(MULT_wrench * wrench[5]))
exit_critical
end
send_wrench_state()
while True:
send_wrench_state()
sync()
end
sync()
end
thread io_statePublisherThread():
def send_io_state():
enter_critical
socket_send_int(MSG_GET_IO)
socket_send_int(1*get_digital_in(0) + 2*get_digital_in(1) + 4 * get_digital_in(2) + 8 * get_digital_in(3) + 16 * get_digital_in(4) + 32 * get_digital_in(5) + 64 * get_digital_in(6) + 128 * get_digital_in(7) + 256 * get_digital_in(8) + 512 * get_digital_in(9))
socket_send_int(1*get_digital_out(0) + 2*get_digital_out(1) + 4 * get_digital_out(2) + 8 * get_digital_out(3) + 16 * get_digital_out(4) + 32 * get_digital_out(5) + 64 * get_digital_out(6) + 128 * get_digital_out(7) + 256 * get_digital_out(8) + 512 * get_digital_out(9))
socket_send_int(pow(2,0)*get_flag(0) + pow(2,1)*get_flag(1) + pow(2,2)*get_flag(2) + pow(2,3)*get_flag(3) + pow(2,4)*get_flag(4) + pow(2,5)*get_flag(5) + pow(2,6)*get_flag(6) + pow(2,7)*get_flag(7) + pow(2,8)*get_flag(8) + pow(2,9)*get_flag(9) + pow(2,10)*get_flag(10) + pow(2,11)*get_flag(11) + pow(2,12)*get_flag(12) + pow(2,13)*get_flag(13) + pow(2,14)*get_flag(14) + pow(2,15)*get_flag(15) + pow(2,16)*get_flag(16) + pow(2,17)*get_flag(17) + pow(2,18)*get_flag(18) + pow(2,19)*get_flag(19) + pow(2,20)*get_flag(20) + pow(2,21)*get_flag(21) + pow(2,22)*get_flag(22) + pow(2,23)*get_flag(23) + pow(2,24)*get_flag(24) + pow(2,25)*get_flag(25) + pow(2,26)*get_flag(26) + pow(2,27)*get_flag(27) + pow(2,28)*get_flag(28) + pow(2,29)*get_flag(29) + pow(2,30)*get_flag(30) + pow(2,31)*get_flag(31) + pow(2,32)*get_flag(32))
####socket_send_int(pow(2,0)*get_flag(0) + pow(2,1)*get_flag(1) + pow(2,2)*get_flag(2) + pow(2,3)*get_flag(3) + pow(2,4)*get_flag(4) + pow(2,5)*get_flag(5) + pow(2,6)*get_flag(6) + pow(2,7)*get_flag(7) + pow(2,8)*get_flag(8) + pow(2,9)*get_flag(9) + pow(2,10)*get_flag(10) + pow(2,11)*get_flag(11) + pow(2,12)*get_flag(12) + pow(2,13)*get_flag(13) + pow(2,14)*get_flag(14) + pow(2,15)*get_flag(15) + pow(2,16)*get_flag(16) + pow(2,17)*get_flag(17) + pow(2,18)*get_flag(18) + pow(2,19)*get_flag(19) + pow(2,20)*get_flag(20) + pow(2,21)*get_flag(21) + pow(2,22)*get_flag(22) + pow(2,23)*get_flag(23) + pow(2,24)*get_flag(24) + pow(2,25)*get_flag(25) + pow(2,26)*get_flag(26) + pow(2,27)*get_flag(27) + pow(2,28)*get_flag(28) + pow(2,29)*get_flag(29) + pow(2,30)*get_flag(30) + pow(2,31)*get_flag(31))
#socket_send_int(1*get_flag(0) + 2*get_flag(1) + 4*get_flag(2) + 8*get_flag(3) + 16*get_flag(4) + 32*get_flag(5) + 64*get_flag(6) + 128*get_flag(7))
#socket_send_int(1*get_flag(8) + 2*get_flag(9) + 4*get_flag(10) + 8*get_flag(11) + 16*get_flag(12) + 32*get_flag(13) + 64*get_flag(14) + 128*get_flag(15))
#socket_send_int(1*get_flag(16) + 2*get_flag(17) + 4*get_flag(18) + 8*get_flag(19) + 16*get_flag(20) + 32*get_flag(21) + 64*get_flag(22) + 128*get_flag(23))
#socket_send_int(1*get_flag(24) + 2*get_flag(25) + 4*get_flag(26) + 8*get_flag(27) + 16*get_flag(28) + 32*get_flag(29) + 64*get_flag(30) + 128*get_flag(31))
socket_send_int(get_analog_out(0) * MULT_analog)
socket_send_int(get_analog_out(1) * MULT_analog)
socket_send_int(get_analog_in(0) * MULT_analog)
socket_send_int(get_analog_in(1) * MULT_analog)
socket_send_int(get_analog_in(2) * MULT_analog)
socket_send_int(get_analog_in(3) * MULT_analog)
#socket_send_int(7895160) # Recognizable ".xxx" or 00787878
exit_critical
end
#socket_open(HOSTNAME, 50002)
send_joint_state()
#temporarily reduce publish rate for io_states
io_count = 0
io_slowdown_ratio = 10
send_io_state()
while True:
send_joint_state()
if io_count != io_slowdown_ratio:
io_count=io_count+1
else:
send_io_state()
io_count=0
end
sync()
end
sync()
end
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
......@@ -145,6 +187,8 @@ def driverProg():
send_out("hello")
thread_state = run statePublisherThread()
thread_wrench_state = run wrench_statePublisherThread()
thread_io_state = run io_statePublisherThread()
thread_servo = run servoThread()
# Servoes in a circle
......@@ -281,6 +325,8 @@ def driverProg():
#sleep(1)
kill thread_state
kill thread_wrench_state
kill thread_io_state
socket_send_int(MSG_QUIT)
end
driverProg()
ur_driver/src/ur_driver/driver.py
+ 33
− 8
View file @ 73b690b6
......@@ -356,6 +356,10 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
#gets digital in states
while len(buf) < 4:
buf = buf + self.recv_more()
#print "MSG_GET_IO"
#print type(buf)
#print len(buf)
#print "Buf:", [ord(b) for b in buf]
inp = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
msg = IOStates()
......@@ -368,13 +372,34 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = buf[4:]
for i in range(0, 10):
msg.digital_out_states.append(DigitalOut(i, (inp & (1<<i))>>i))
#gets flag states
while len(buf) < 4:
buf = buf + self.recv_more()
inp = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:]
for i in range(0, 32):
msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
##gets flag states (0-7)
#while len(buf) < 4:
#buf = buf + self.recv_more()
#inp = struct.unpack_from("!i", buf, 0)[0]
#buf = buf[4:]
#for i in range(0, 8):
#msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
##gets flag states (8-15)
#while len(buf) < 4:
#buf = buf + self.recv_more()
#inp = struct.unpack_from("!i", buf, 0)[0]
#buf = buf[4:]
#for i in range(8, 16):
#msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
##gets flag states (16-23)
#while len(buf) < 4:
#buf = buf + self.recv_more()
#inp = struct.unpack_from("!i", buf, 0)[0]
#buf = buf[4:]
#for i in range(16, 24):
#msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
##gets flag states (24-31)
#while len(buf) < 4:
#buf = buf + self.recv_more()
#inp = struct.unpack_from("!i", buf, 0)[0]
#buf = buf[4:]
#for i in range(24, 32):
#msg.flag_states.append(Flag(i, (inp & (1<<i))>>i))
#gets analog_out[0] state
while len(buf) < 4:
buf = buf + self.recv_more()
......@@ -488,7 +513,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
#print params
with self.socket_lock:
self.request.send(buf)
time.sleep(IO_SLEEP_TIME+.5)
time.sleep(IO_SLEEP_TIME)
def set_flag(self, pin, val):
params = [MSG_SET_FLAG] + \
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment