Commit 346f06f0 authored by Sam Calisch's avatar Sam Calisch

cleanup

parent 5abad4cf
import socket
import time
import struct
from sys import stdout
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
HOST = "192.168.1.51"
PORT = 30003
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))
robot_modes = ['Disconnected','Confirm Safety','Booting','Power Off','Power On','Idle','Backdrive','Running','Updating Firmware']
joint_modes = {
'236':'Shutting Down',
'237':'D Calibration',
'238':'Backdrive',
'239':'Power Off',
'245':'Not Responding',
'246':'Initialization',
'247':'Booting',
'248':'D Calibration Error',
'249':'Bootloader',
'250':'Calibration',
'252':'Fault',
'253':'Running',
'255':'Idle'}
safety_modes = ['Normal','Reduced','Protective Stop','Recovery','Safegaurd']
def print_packet(packet):
print "\033[2J"
print "\033[H"
print "Message Size:\t%d"%packet[0]
print "Time:\t\t%f"%packet[1]
print "q target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[2:8])
print "qd target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[8:14])
print "qdd target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[14:20])
print "I target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[20:26])
print "M target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[26:32])
print "q actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[32:38])
print "qd actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[38:44])
print "I actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[44:50])
print "I control:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[50:56])
print "Tool vec act:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[56:62])
print "Tool speed act:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[62:68])
print "TCP force:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[68:74])
print "Tool vec targ:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[74:80])
print "TCP speed targ:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[80:86])
print "Digital Ins:\t%s"%('{0:08b}'.format(packet[86]))
print "Motor temps:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[87:93])
print "Control Timer:\t%f"%packet[93]
#94 unused
print "Robot mode:\t%s"%robot_modes[int(packet[95])]
print "Joint 1 mode:\t%s"%joint_modes['%d'%packet[96]]
print "Joint 2 mode:\t%s"%joint_modes['%d'%packet[97]]
print "Joint 3 mode:\t%s"%joint_modes['%d'%packet[98]]
print "Joint 4 mode:\t%s"%joint_modes['%d'%packet[99]]
print "Joint 5 mode:\t%s"%joint_modes['%d'%packet[100]]
print "Joint 6 mode:\t%s"%joint_modes['%d'%packet[101]]
print "Safety mode:\t%s"%safety_modes[int(packet[102])-1]
print "Tool accel:\t(%f,%f,%f)"%tuple(packet[109:112])
print "Speed scale:\t%f"%packet[118]
print "Lin Momentum:\t%f"%packet[119]
print "Main voltage:\t%f"%packet[122]
print "Robot voltage:\t%f"%packet[123]
print "Robot current:\t%f"%packet[124]
print "Joint voltage:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[125:131])
#values = '>I'+85*'d'+'q'+7*'d'+7*'q'+30*'d'
values = '>I'+85*'d'+'q'+44*'d'
struct = struct.Struct(values)
last = time.time()
try:
while(True):
data = s.recv(1044)
print_packet(struct.unpack(data))
print "Packet interval: %.3f ms"%(1000*(time.time()-last))
last = time.time()
except(KeyboardInterrupt):
pass
s.close()
print "Thank you."
import socket
import time
import struct
from sys import stdout
#Sam Calisch, 2015
#Center for Bits and Atoms, MIT
HOST = "192.168.1.52"
PORT = 30002
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((HOST, PORT))
robot_modes = ['Disconnected','Confirm Safety','Booting','Power Off','Power On','Idle','Backdrive','Running','Updating Firmware']
joint_modes = {
'236':'Shutting Down',
'237':'D Calibration',
'238':'Backdrive',
'239':'Power Off',
'245':'Not Responding',
'246':'Initialization',
'247':'Booting',
'248':'D Calibration Error',
'249':'Bootloader',
'250':'Calibration',
'252':'Fault',
'253':'Running',
'255':'Idle'}
safety_modes = ['Normal','Reduced','Protective Stop','Recovery','Safegaurd']
def print_packet(packet):
print "\033[2J"
print "\033[H"
print "Message Size:\t%d"%packet[0]
print "Time:\t\t%f"%packet[1]
print "q target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[2:8])
print "qd target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[8:14])
print "qdd target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[14:20])
print "I target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[20:26])
print "M target:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[26:32])
print "q actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[32:38])
print "qd actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[38:44])
print "I actual:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[44:50])
print "I control:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[50:56])
print "Tool vec act:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[56:62])
print "Tool speed act:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[62:68])
print "TCP force:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[68:74])
print "Tool vec targ:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[74:80])
print "TCP speed targ:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[80:86])
print "Digital Ins:\t%s"%('{0:08b}'.format(packet[86]))
print "Motor temps:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[87:93])
print "Control Timer:\t%f"%packet[93]
#94 unused
print "Robot mode:\t%s"%robot_modes[int(packet[95])]
print "Joint 1 mode:\t%s"%joint_modes['%d'%packet[96]]
print "Joint 2 mode:\t%s"%joint_modes['%d'%packet[97]]
print "Joint 3 mode:\t%s"%joint_modes['%d'%packet[98]]
print "Joint 4 mode:\t%s"%joint_modes['%d'%packet[99]]
print "Joint 5 mode:\t%s"%joint_modes['%d'%packet[100]]
print "Joint 6 mode:\t%s"%joint_modes['%d'%packet[101]]
print "Safety mode:\t%s"%safety_modes[int(packet[102])-1]
print "Tool accel:\t(%f,%f,%f)"%tuple(packet[109:112])
print "Speed scale:\t%f"%packet[118]
print "Lin Momentum:\t%f"%packet[119]
print "Main voltage:\t%f"%packet[122]
print "Robot voltage:\t%f"%packet[123]
print "Robot current:\t%f"%packet[124]
print "Joint voltage:\t(%f,%f,%f,%f,%f,%f)"%tuple(packet[125:131])
header_struct = struct.Struct('>iB')
robot_mode_struct = struct.Struct('>'+'Q'+7*'?'+2*'B'+2*'d')
joint_data_struct = struct.Struct( '>'+6*(3*'d'+4*'f'+'B') )
tool_data_struct = struct.Struct('>'+2*'b'+2*'d'+'f'+'B'+2*'f'+'B')
masterboard_data_struct = struct.Struct('>'+2*'i'+2*'b'+2*'d'+2*'b'+2*'d'+4*'f'+2*'B'+'b'+'L')
cartesian_info_struct = struct.Struct('>'+12*'d')
config_data_struct = struct.Struct('>'+53*'d'+4*'i')
force_data_struct = struct.Struct('>'+7*'d')
subpackages = {
'0':(robot_mode_struct,38,"Robot Mode"),
'1':(joint_data_struct,251,"Joint Data"),
'2':(tool_data_struct,37,"Tool Data"),
'3':(masterboard_data_struct,72,"Masterboard Data"),
'4':(cartesian_info_struct,101,"Cartesian Info"),
'6':(config_data_struct,445,"Config Data"),
'7':(force_data_struct,61,"Force Data")
}
def handle_subpackage(s,remaining):
header = s.recv(5)
header = header_struct.unpack(header)
sublength = header[0]
subtype = header[1]
try:
subpackage = subpackages[str(subtype)]
assert(subpackage[1] == sublength ) #make sure lengths match
print "\t",subpackage[2]
data = s.recv(sublength-5)
#print '\t',subpackage[0].unpack(data)
except(KeyError):
#print "\tPackage type %d not known."%subtype
s.recv(sublength-5)
return remaining-sublength
def flush(s):
print "flushing"
header = s.recv(5)
header = header_struct.unpack(header)
s.recv(header[1])
last = time.time()
try:
while(True):
s.send("set_digital_out(2,True)\n") #only for testing response
header = s.recv(5)
header = header_struct.unpack(header)
if header[1]!=16: #if we're not at packet start, flush
flush(s)
else:
print "Packet interval: %.1f ms"%(1000*(time.time()-last))
last = time.time()
remaining = header[0]-5
while(remaining>0):
remaining = handle_subpackage(s,remaining)
assert(remaining>=0)
except(KeyboardInterrupt):
pass
s.close()
print "Thank you."
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