Commit 8edad989 authored by Sam Calisch's avatar Sam Calisch

first cnc sewing

	update get current pose to take robot name and ip as command line arguments
	test raspberry pi dynamixel sewing
parent 6a5632ff
import socket
import sys
from ax12 import *
dyna = Ax12()
servo_min = 452
servo_max = 522
#dyna.learnServos(verbose=False) #to learn what dynamixel motors are connected
#right now this is hard coded for servo id 1. could change this eventually...
def handle_data(data,connection):
data = data.split(',')
while(True):
try:
if data[0] == 'm':
p = int(data[1])
if (servo_min <= p) and (p <= servo_max):
dyna.move(1,int(data[1]))
elif data[0] == 's':
#there seem to be problems in ax12.py for this...
connection.sendall("%04d"%dyna.readSpeed(1))
elif data[0] == 'p':
connection.sendall("%04d"%dyna.readPosition(1))
elif data[0] == 't':
connection.sendall("%04d"%dyna.readTemperature(1))
elif data[0] == 'l':
connection.sendall("%04d"%dyna.readLoad(1))
elif data[0] == 'v':
connection.sendall("%04d"%dyna.readVoltage(1))
except(dyna.timeoutError):
continue
break
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_address = ('192.168.1.55', 10000)
print 'starting up on %s port %s' % server_address
sock.bind(server_address)
sock.listen(1)
while True:
print 'waiting for a connection'
connection, client_address = sock.accept()
print 'connection from', client_address
try:
while True:
data = connection.recv(6)
if data:
handle_data(data,connection)
#connection.sendall(data)
else:
break
except KeyboardInterrupt:
break
finally:
print "closing socket"
connection.close()
\ No newline at end of file
import socket
import sys
from time import sleep
def move(p,sock,delay=1.):
sock.sendall('m,%04d'%p)
sleep(delay-.1)
sock.sendall('p,0000')
print "Position: %d "%int(sock.recv(4))
sleep(.1)
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_address = ('192.168.1.55', 10000)
sock.connect(server_address)
'''
test script for dynamixel over tcp/ip
message format: first char gives command type:
-m: move
-p: get position
-s: get speed
-t: get temperature
-v: get voltage
first char followed by comma
four digits give a position
'''
try:
sock.sendall('v,0000')
print "Voltage: %1f volts"%(float(sock.recv(4))/10)
sleep(.1)
sock.sendall('t,0000')
print "Temperature: %d degrees Celsius"%int(sock.recv(4))
sleep(.1)
sock.sendall('p,0000')
print "Position: %d "%int(sock.recv(4))
sleep(.1)
#foot up: 452
#zero: 473
#full tilt: 522
#move(452,sock)
move(473,sock)
move(490,sock)
#move(522,sock)
move(473,sock)
move(452,sock)
finally:
print 'closing socket'
sock.close()
\ No newline at end of file
from UR10 import *
from numpy import *
import sys
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
siegfried = UR10("Siegfried","192.168.1.51")
siegfried.open_socket(30002)
#siegfried = UR10("Siegfried","192.168.1.51")
robot = UR10(sys.argv[1],sys.argv[2])
robot.open_socket(30002)
last = time.time()
for i in range(10): siegfried.read_packet_secondary() #establish starting position
print "[%f, %f, %f, %f, %f, %f]"%tuple(siegfried.tool_vector)
siegfried.close_socket()
for i in range(10): robot.read_packet_secondary() #establish starting position
print "[%f, %f, %f, %f, %f, %f]"%tuple(robot.tool_vector)
robot.close_socket()
print "Thank you."
from UR10 import *
from numpy import *
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
siegfried = UR10("Roy","192.168.1.52")
siegfried.stream_script('sewing_test.script')
def vacuum_test():
set_standard_analog_input_domain(0, 1)
set_standard_analog_input_domain(1, 1)
set_tool_analog_input_domain(0, 1)
set_tool_analog_input_domain(1, 1)
set_analog_outputdomain(0, 0)
set_analog_outputdomain(1, 0)
set_tool_voltage(0)
set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0])
set_payload(0.0)
set_gravity([0.0, 0.0, 9.82])
socket_open("192.168.1.55",10000)
$ 1 "Starting"
$ 3 "Waypoint_1"
start = p[0.183287, -0.522058, 0.463555, -3.141534, -0.000175, 0.000103]
movej(start, a=.5, v=1.0)
movej(pose_add(start,p[0,0,-.1,0,0,0]), a=.5, v=1.0)
sleep(.1)
socket_send_string("m,0473")
sleep(1.)
socket_send_string("m,0490")
sleep(1.)
socket_send_string("m,0473")
sleep(.2)
socket_send_string("m,0452")
sleep(.2)
socket_close()
end
......@@ -57,22 +57,23 @@ def stream_test_1():
end
end
global alpha=.95 #smoothing coefficient. zero -> no smoothing
global alpha=.875 #smoothing coefficient. zero -> no smoothing
global Fx=0.0
global Fy=0.0
global Fz=0.0
global Mx=0.0
global My=0.0
global Mz=0.0
global Fz_max=2
global F_max=15
global z_increment=0.0005
global safe_z=.02
global var_x=.142
global var_y=.185
global var_d=.015
global var_xx=.525
global var_yy=.275
global var_r = .01
global safe_z=.005
global scale_factor = .5
global var_x=.142*scale_factor
global var_y=.185*scale_factor
global var_d=.015*scale_factor
global var_xx=.525*scale_factor
global var_yy=.275*scale_factor
global var_r = .01*scale_factor
global position_increm=p[-0.501350, -0.475087, 0.193433, -1.209338, -1.209378, 1.209117]
def push_z_axis():
......@@ -80,12 +81,12 @@ def stream_test_1():
while (True):
global position_increm=get_forward_kin()
position_increm[2] = position_increm[2]-z_increment
movel(position_increm,1.2,0.01,0,0.0001)
movel(position_increm,1.2,0.002,0,0.0001)
end
end
if (norm(Fx)<Fz_max):
if (norm(Fx)<F_max):
global thread_handler_68=run Thread_while_68()
while (norm(Fx)<Fz_max):
while (norm(Fx)<F_max):
sync()
end
kill thread_handler_68
......@@ -116,7 +117,7 @@ def stream_test_1():
push_z_axis()
sleep(.1)
movel(p,a=.5,v=.05,t=0,r=.0001)
return pose_add(position_increm,p[0,0,.0008,0,0,0])
return pose_add(position_increm,p[0,0,.001,0,0,0])
end
def process_loop(approach,pts):
......@@ -144,11 +145,14 @@ def stream_test_1():
rq_set_zero() #zero load cell
$ 3 "measuring"
app_1 = p[-0.501350, -0.475087, 0.193433, -1.209338, -1.209378, 1.209117]
app_1 = pose_add(app_1,p[-.02,.02,0,0,0,0])
#app_1 = p[-0.501350, -0.475087, 0.193433, -1.209338, -1.209378, 1.209117]
app_1 = p[-0.547443, -0.565046, 0.238797, -1.209782, -1.195192, 1.223953]
#app_1 = pose_add(app_1,p[-.17,.32,-.005,0,0,0])
global c1 = set_z(app_1)
global c12=set_z(pose_add(app_1,p[0,.5*var_xx,0,0,0,0]))
global c2=set_z(pose_add(app_1,p[0,var_xx,0,0,0,0]))
global c23=set_z(pose_add(app_1,p[-var_yy/2,var_xx,0,0,0,0]))
global c3=set_z(pose_add(app_1,p[-var_yy,var_xx,0,0,0,0]))
global c34=set_z(pose_add(app_1,p[-var_yy,.5*var_xx,0,0,0,0]))
global c4=set_z(pose_add(app_1,p[-var_yy,0,0,0,0,0]))
......@@ -162,6 +166,6 @@ def stream_test_1():
$ 4 "tracing"
process_loop(app_2,[d1,d2,d3,d4,d5,d6]) #inside out
process_loop(app_1,[c1,c12,c2,c3,c34,c4])
process_loop(app_1,[c1,c12,c2,c23,c3,c34,c4])
end
......@@ -5,4 +5,4 @@ from numpy import *
#Center for Bits and Atoms, MIT
siegfried = UR10("Siegfried","192.168.1.51")
siegfried.stream_script('stream_test_1.script')
\ No newline at end of file
siegfried.stream_script('stream_test_1.script')
def weld_test_force():
set_standard_analog_input_domain(0, 1)
set_standard_analog_input_domain(1, 1)
set_tool_analog_input_domain(0, 1)
set_tool_analog_input_domain(1, 1)
set_analog_outputdomain(0, 0)
set_analog_outputdomain(1, 0)
set_tool_voltage(12)
set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0])
set_payload(0.0)
set_gravity([0.0, 0.0, 9.82])
global Fx=0.0
global Fy=0.0
global Fz=0.0
global Mx=0.0
global My=0.0
global Mz=0.0
global position_increm=p[-0.5013557, -0.47510922, 0.18295662, -1.2093452, -1.2093372, 1.2091138]
global welding_surface=p[-0.5069571143377952,-0.4929437829881331,0.18348710066782717,-1.642548079816635E-5,-3.140294310983835,-5.513005848767689E-5]
def push_z_axis():
thread Thread_while_68():
while (True):
global position_increm=get_forward_kin()
position_increm[2] = position_increm[2]-z_increment
movel(position_increm,1.2,0.01,0,0.0001)
end
end
if (norm(Fx)<Fz_max):
global thread_handler_68=run Thread_while_68()
while (norm(Fx)<Fz_max):
sync()
end
kill thread_handler_68
end
end
def set_z(b,p):
movel(pose_add(b,p),a=1.2, v=0.25)
push_z_axis()
return position_increm
end
$ 2 "BeforeStart"
$ 3 "socket_close('stream')"
socket_close("stream")
$ 4 "socket_open('127.0.0.1',63351,'stream')"
socket_open("127.0.0.1",63351,"stream")
def rq_print_serial_number():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET SNU","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
def rq_print_firmware_version():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET FWV","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
def rq_print_prod_year():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET PYE","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
def rq_set_zero():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("SET ZRO","acc")
sleep(0.1)
socket_close("acc")
end
end
def rq_get_version():
if(socket_open("127.0.0.1",63350,"acc")):
socket_send_string("GET VER","acc")
string_from_server = socket_read_string("acc")
popup(string_from_server)
socket_close("acc")
end
end
$ 6 "Fx≔0.0"
global Fx=0.0
$ 7 "Fy≔0.0"
global Fy=0.0
$ 8 "Fz≔0.0"
global Fz=0.0
$ 9 "Mx≔0.0"
global Mx=0.0
$ 10 "My≔0.0"
global My=0.0
$ 11 "Mz≔0.0"
global Mz=0.0
$ 12 "Fz_max≔4"
global Fz_max=4
$ 15 "z_increment≔0.0005"
global z_increment=0.0005
$ 16 "safe_z≔.02"
global safe_z=.02
$ 17 "var_x≔.142"
global var_x=.142
$ 18 "var_y≔.185"
global var_y=.185
$ 19 "var_d≔.015"
global var_d=.015
$ 20 "var_xx≔.525"
global var_xx=.525
$ 21 "var_yy≔.275"
global var_yy=.275
$ 72 "Thread_1"
thread Thread_1():
while (True):
while(1):
listofstuff = socket_read_ascii_float(6,"stream")
if(listofstuff[0] != 0):
Fx = listofstuff[1]
Fy = listofstuff[2]
Fz = listofstuff[3]
Mx = listofstuff[4]
My = listofstuff[5]
Mz = listofstuff[6]
end
textmsg(Fz)
end
end
end
threadId_Thread_1 = run Thread_1()
$ 22 "Robot Program"
$ 23 "rq_set_zero()"
rq_set_zero()
$ 24 "MoveL"
$ 25 "app_1"
movel([0.5955118832732094, -1.2447842704539802, 2.0183618281967632, -0.7743991549648204, 2.1642834408424187, -3.142822139040165], a=1.2, v=0.25)
$ 27 "home_var≔set_z(app_1)"
global home_var≔set_z(app_1)
$ 29 "c1≔set_z(app_1,p[0,var_xx,0,0,0,0])"
global c1=set_z(app_1,p[0,var_xx,0,0,0,0])
$ 30 "c2≔set_z(app_1,p[-var_yy,var_xx,0,0,0,0])"
global c2=set_z(app_1,p[-var_yy,var_xx,0,0,0,0])
$ 31 "c3≔set_z(app_1,p[-var_yy,0,0,0,0,0])"
global c3=set_z(app_1,p[-var_yy,0,0,0,0,0])
$ 32 "d1≔set_z(app_1,p[-var_yy/2+var_y/2,var_xx/2-var_d/2,0,0,0,0])"
global d1=set_z(app_1,p[-var_yy/2+var_y/2,var_xx/2-var_d/2,0,0,0,0])
$ 33 "app_2≔d1"
global app_2=d1
$ 34 "d2≔set_z(app_1,p[-var_yy/2+var_y/2,var_xx/2+var_d/2,0,0,0,0])"
global d2=set_z(app_1,p[-var_yy/2+var_y/2,var_xx/2+var_d/2,0,0,0,0])
$ 35 "d3≔set_z(app_1,p[-var_yy/2,var_xx/2+var_x/2,0,0,0,0])"
global d3=set_z(app_1,p[-var_yy/2,var_xx/2+var_x/2,0,0,0,0])
$ 36 "d4≔set_z(app_1,p[-var_yy/2-var_y/2,var_xx/2+var_d/2,0,0,0,0])"
global d4=set_z(app_1,p[-var_yy/2-var_y/2,var_xx/2+var_d/2,0,0,0,0])
$ 37 "d5≔set_z(app_1,p[-var_yy/2-var_y/2,var_xx/2-var_d/2,0,0,0,0])"
global d5=set_z(app_1,p[-var_yy/2-var_y/2,var_xx/2-var_d/2,0,0,0,0])
$ 38 "d6≔set_z(app_1,p[-var_yy/2,var_xx/2-var_x/2,0,0,0,0])"
global d6=set_z(app_1,p[-var_yy/2,var_xx/2-var_x/2,0,0,0,0])
$ 39 "Set TO[0]=On"
set_tool_digital_out(0, True)
$ 40 "MoveP"
$ 41 "c1"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], c1), a=1.2, v=0.25, r=0.01)
$ 42 "c2"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], c2), a=1.2, v=0.25, r=0.01)
$ 43 "c3"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], c3), a=1.2, v=0.25, r=0.01)
$ 44 "home_var"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], home_var), a=1.2, v=0.25, r=0.0)
$ 45 "Set TO[0]=Off"
set_tool_digital_out(0, False)
$ 46 "MoveL"
$ 47 "fixed_app"
movel([0.5955118832732094, -1.2447842704539802, 2.0183618281967632, -0.7743991549648204, 2.1642834408424187, -3.142822139040165], a=1.2, v=0.25)
$ 48 "app_2"
movel(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], app_2), a=1.2, v=0.25)
$ 49 "d1"
movel(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d1), a=1.2, v=0.25)
$ 50 "Set TO[0]=On"
set_tool_digital_out(0, True)
$ 51 "MoveP"
$ 52 "d1"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d1), a=1.2, v=0.25, r=0.01)
$ 53 "d2"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d2), a=1.2, v=0.25, r=0.01)
$ 54 "d3"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d3), a=1.2, v=0.25, r=0.01)
$ 55 "d4"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d4), a=1.2, v=0.25, r=0.01)
$ 56 "d5"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d5), a=1.2, v=0.25, r=0.01)
$ 57 "d6"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d6), a=1.2, v=0.25, r=0.01)
$ 58 "d1"
movep(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], d1), a=1.2, v=0.25, r=0.0)
$ 59 "Set TO[0]=Off"
set_tool_digital_out(0, False)
$ 60 "MoveL"
$ 61 "app_2"
movel(pose_trans(p[0.0,0.0,0.0,0.0,0.0,0.0], app_2), a=1.2, v=0.25)
end
Markdown is supported
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