diff --git a/ur_driver/prog b/ur_driver/prog
index 66ebf4fe6d2659f049ab6ad2a8e6734bb3660262..6f118e1a5880171dd56f9cb3e14f73ec34aaaae0 100644
--- a/ur_driver/prog
+++ b/ur_driver/prog
@@ -9,6 +9,11 @@ def driverProg():
MSG_STOPJ = 6
MSG_SERVOJ = 7
MSG_WRENCH = 9
+ MSG_SET_DIGITAL_OUT = 10
+ MSG_GET_IO = 11
+ MSG_SET_FLAG = 12
+ MSG_SET_TOOL_VOLTAGE = 13
+ MSG_SET_ANALOG_OUT = 14
MULT_wrench = 10000.0
MULT_jointstate = 10000.0
MULT_time = 1000000.0
@@ -63,6 +68,16 @@ 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]))
+ 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(get_analog_out(0)*1000000)
+ socket_send_int(get_analog_out(1)*1000000)
+ socket_send_int(get_analog_in(0)*1000000)
+ socket_send_int(get_analog_in(1)*1000000)
+ socket_send_int(get_analog_in(2)*1000000)
+ socket_send_int(get_analog_in(3)*1000000)
#socket_send_int(7895160) # Recognizable ".xxx" or 00787878
exit_critical
end
@@ -205,6 +220,46 @@ def driverProg():
elif mtype == MSG_STOPJ:
send_out("Received stopj")
stopj(1.0)
+ elif mtype == MSG_SET_DIGITAL_OUT:
+ #send_out("Received Digital Out Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ if params_mult[2] > 0:
+ set_digital_out(params_mult[1], True)
+ elif params_mult[2] == 0:
+ set_digital_out(params_mult[1], False)
+ end
+ elif mtype == MSG_SET_FLAG:
+ #send_out("Received Set Flag Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ if params_mult[2] != 0:
+ set_flag(params_mult[1], True)
+ elif params_mult[2] == 0:
+ set_flag(params_mult[1], False)
+ end
+ elif mtype == MSG_SET_ANALOG_OUT:
+ #send_out("Received Analog Out Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ set_analog_out(params_mult[1], (params_mult[2] / 1000000))
+ elif mtype == MSG_SET_TOOL_VOLTAGE:
+ #send_out("Received Tool Voltage Signal")
+ # Reads the parameters
+ params_mult = socket_read_binary_integer(1+6+1)
+ if params_mult[0] == 0:
+ send_out("Received no parameters for set_out message")
+ end
+ set_tool_voltage(params_mult[1])
else:
send_out("Received unknown message type")
end
diff --git a/ur_driver/src/ur_driver/onandoff.py b/ur_driver/src/ur_driver/onandoff.py
new file mode 100755
index 0000000000000000000000000000000000000000..fdfa2c84394e3995c68745dc8bee6d68567e3428
--- /dev/null
+++ b/ur_driver/src/ur_driver/onandoff.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+import ur_IO
+import time
+
+if __name__ == "__main__":
+ print "toggling things"
+ ur_IO.get_states()
+ print "listener has been activated"
+ while(True):
+ ur_IO.set_tool_voltage(12)
+ ur_IO.set_digital_out(0, True)
+ ur_IO.set_analog_out(0, 0.75)
+ ur_IO.set_flag(0, True)
+ print(ur_IO.Flag_States[0])
+ print(ur_IO.Analog_Out_States[0])
+ print(ur_IO.Digital_Out_States[0])
+ time.sleep(1)
+ ur_IO.set_tool_voltage(24)
+ ur_IO.set_digital_out(0, False)
+ ur_IO.set_analog_out(0, 0.25)
+ ur_IO.set_flag(0, False)
+ print(ur_IO.Flag_States[0])
+ print(ur_IO.Analog_Out_States[0])
+ print(ur_IO.Digital_Out_States[0])
+ time.sleep(1)
+
+
diff --git a/ur_driver/src/ur_driver/ur_IO.py b/ur_driver/src/ur_driver/ur_IO.py
new file mode 100755
index 0000000000000000000000000000000000000000..9df952d71a037dd8f4148443b22f8deef8012dcd
--- /dev/null
+++ b/ur_driver/src/ur_driver/ur_IO.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+
+import sys
+import rospy
+from ur_driver.srv import *
+from ur_driver.msg import *
+
+FUN_SET_DIGITAL_OUT = 1
+FUN_SET_FLAG = 2
+FUN_SET_ANALOG_OUT = 3
+FUN_SET_TOOL_VOLTAGE = 4
+
+Flag_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Digital_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Digital_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Analog_Out_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+Analog_In_States = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
+
+ANALOG_TOLERANCE_VALUE = 0.01
+DELAY_TIME = 0.3
+i = 0
+set_io = rospy.ServiceProxy('set_io', SetIO)
+
+def set_io_val(fun, pin, val):
+ rospy.wait_for_service('set_io')
+ try:
+ set_io(fun, pin, val)
+ except rospy.ServiceException, e:
+ print "Service call failed: %s"%e
+
+def set_tool_voltage(volts):
+ set_io_val(FUN_SET_TOOL_VOLTAGE, volts, 0)
+
+def set_digital_out(pin, val):
+ set_io_val(FUN_SET_DIGITAL_OUT, pin, val)
+ if Digital_Out_States[pin] != val:
+ rospy.logwarn("DIGITAL OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
+ set_digital_out(pin, val)
+
+def set_analog_out(pin, val):
+ set_io_val(FUN_SET_ANALOG_OUT, pin, val)
+ if abs(Analog_Out_States[pin] - val) > ANALOG_TOLERANCE_VALUE:
+ rospy.logwarn("ANALOG OUT IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
+ set_digital_out(pin, val)
+
+def set_flag(pin, val):
+ set_io_val(FUN_SET_FLAG, pin, val)
+ if Flag_States[pin] != val:
+ rospy.logwarn("SETTING A FLAG IS NOT A HAPPY CAMPER, its not setting a pin on the first try, it may help to increase IO_SLEEP_TIME in driver.py")
+ set_digital_out(pin, val)
+
+def callback(data):
+ for i in range(0,32):
+ del Flag_States[i]
+ Flag_States.insert(i, data.flag_states[i].state)
+ for i in range(0,10):
+ del Digital_Out_States[i]
+ Digital_Out_States.insert(i, data.digital_out_states[i].state)
+ for i in range(0,10):
+ del Digital_In_States[i]
+ Digital_In_States.insert(i, data.digital_in_states[i].state)
+ for i in range(0,2):
+ del Analog_Out_States[i]
+ Analog_Out_States.insert(i, data.analog_out_states[i].state)
+ for i in range(0,4):
+ del Analog_In_States[i]
+ Analog_In_States.insert(i, data.analog_in_states[i].state)
+
+def get_states():
+ rospy.init_node('UR_State_Getter')
+ rospy.Subscriber("io_states", IOStates, callback)
+