fabmodules/src/py/panel_path_snap.py

634 wiersze
20 KiB
Python

#
# panel_path_snap.py
# MTM Snap virtual machine
#
# Neil Gershenfeld
# CBA MIT 12/13/11
#
# (c) Massachusetts Institute of Technology 2011
# Permission granted for experimental and personal use;
# license for commercial sale available from MIT.
#
# todo
# process text entry
# acceleration
# job times
#
# imports
#
import wx,os,serial,time
from math import *
#
# machine definitions
#
mm_per_xstep = 0.00635 # stepper step size (400 steps/rev, 10 revs/in)
mm_per_ystep = 0.00635 # stepper step size ''
mm_per_zstep = 0.00635 # stepper step size ''
mm_per_jog_step = 0.1 # jog step size
char_delay = 0.001 # delay between command chars in seconds
#
# global variables
#
xstep = ystep = zstep = 0 # stepper position (step units)
spindle_state = 0 # on/off
move_state = 0 # on/off
#
# panel class
#
class path_snap_panel(wx.Panel):
def __init__(self,parent):
self.parent = parent
self.parent.path_file = ''
#
# start/stop spindle
#
def spindle(event):
global spindle_state
spindle_state = 1 - spindle_state
if (spindle_state == 1):
spindle_button.SetLabel('OFF')
else:
spindle_button.SetLabel('on')
#
# start/stop job
#
def start(event,start_line=5):
global move_state,ser
move_state = 1 - move_state
if (move_state == 1):
if (self.parent.path_file == ''):
print "panel_path_snap: oops -- need path file"
move_state = 0
return
start_button.SetLabel('STOP')
pause_button.SetLabel('pause')
ser = serial.Serial(port.GetValue(),115200)
ser.setDTR()
start = time.time()
send_job(start_line)
end = time.time()
ser.close()
nsegment = float(segment.GetValue())
print "%f sec, %f sec/segment"%((end-start),(end-start)/nsegment)
else:
start_button.SetLabel('start')
#
# send job
#
def send_job(start_line):
global move_state
global nx,ny,nz,dx,dy,dz,x0,y0,z0
#
# read path
#
path_file = open(self.parent.path_file,'r')
lines = path_file.readlines()
segment_label.SetLabel("/"+str(len(lines)-1))
dof = int(lines[0].split()[0])
if (dof != 3):
print "panel_path_snap: oops -- need a 3D path"
return
units = lines[1] # currently ignored, mm assumed
nx = int(lines[2].split()[0])
ny = int(lines[2].split()[1])
nz = int(lines[2].split()[2])
dx = float(lines[3].split()[0])
dy = float(lines[3].split()[1])
dz = float(lines[3].split()[2])
x0 = float(lines[4].split()[0])
y0 = float(lines[4].split()[1])
z0 = float(lines[4].split()[2])
path_file.close()
#
# get GUI values
#
speed_move = float(move_speed.GetValue())
speed_jog = float(jog_speed.GetValue())
height_jog = float(jog_height.GetValue())
#
# loop over path
#
(xold,yold,zold) = ([],[],[])
for i in range(start_line,len(lines)):
#
# check for GUI events
#
wx.Yield()
#
# quit if stopped
#
if (move_state == 0):
return
#
# read next line
#
line = lines[i].split()
#
# start of new segment?
#
if (line[0] == '.'):
#
# yes, clear old point and continue
#
(xold,yold,zold) = ([],[],[])
continue
#
# no, go to next point
#
x = int(line[0])
y = int(line[1])
z = int(line[2])
#
# first point in segment?
#
if (xold == []):
#
# yes, jog there and move down
#
move_z_abs_mm(height_jog,speed_jog)
move_xy_abs_path(x,y,speed_jog)
move_z_abs_path(z,speed_move)
else:
#
# no, move there
#
move_xyz_abs_path(x,y,z,speed_move)
#
# save old point
#
(xold,yold,zold) = (x,y,z)
#
# update segment counter and continue
#
segment.SetValue(str(i))
#
# finish
#
move_state = 0
start_button.SetLabel('start')
segment_label.SetLabel("segment")
#
# pause
#
def pause(event):
global move_state
print "pause"
if (move_state == 1):
move_state = 0
start_button.SetLabel("start")
pause_button.SetLabel("CONTINUE")
elif (pause_button.GetLabel() == "CONTINUE"):
start(0,int(segment.GetValue()))
#
# write command to bus and read acknowledgement
#
def bus_send(command):
global ser
for i in range(len(command)):
print "%d: %c = %d"%(i,command[i],ord(command[i]))
ser.write(command[i])
time.sleep(char_delay)
ack = ser.read()
#
# send z, stepper step units, ms time units
#
def send_z(dz,dt):
command = ""
if (dz >= 0):
zcmd = 'Z'
else:
zcmd = 'z'
dz1 = (abs(dz) >> 8) & 255
dz0 = abs(dz) & 255
command += zcmd+chr(2)+chr(dz1)+chr(dz0)
t1 = (abs(dt) >> 8) & 255
t0 = abs(dt) & 255
command += 'T'+chr(2)+chr(t1)+chr(t0)
command += 'G'
bus_send(command)
#
# send xy, stepper step units, ms time units
#
def send_xy(dx,dy,dt):
command = ""
if (dx >= 0):
xcmd = 'X'
else:
xcmd = 'x'
dx1 = (abs(dx) >> 8) & 255
dx0 = abs(dx) & 255
command += xcmd+chr(2)+chr(dx1)+chr(dx0)
if (dy >= 0):
ycmd = 'Y'
else:
ycmd = 'y'
dy1 = (abs(dy) >> 8) & 255
dy0 = abs(dy) & 255
command += ycmd+chr(2)+chr(dy1)+chr(dy0)
t1 = (abs(dt) >> 8) & 255
t0 = abs(dt) & 255
command += 'T'+chr(2)+chr(t1)+chr(t0)
command += 'G'
bus_send(command)
#
# send xyz, stepper step units, ms time units
#
def send_xyz(dx,dy,dz,dt):
command = ""
if (dx >= 0):
xcmd = 'X'
else:
xcmd = 'x'
dx1 = (abs(dx) >> 8) & 255
dx0 = abs(dx) & 255
command += xcmd+chr(2)+chr(dx1)+chr(dx0)
if (dy >= 0):
ycmd = 'Y'
else:
ycmd = 'y'
dy1 = (abs(dy) >> 8) & 255
dy0 = abs(dy) & 255
command += ycmd+chr(2)+chr(dy1)+chr(dy0)
if (dz >= 0):
zcmd = 'Z'
else:
zcmd = 'z'
dz1 = (abs(dz) >> 8) & 255
dz0 = abs(dz) & 255
command += zcmd+chr(2)+chr(dz1)+chr(dz0)
t1 = (abs(dt) >> 8) & 255
t0 = abs(dt) & 255
command += 'T'+chr(2)+chr(t1)+chr(t0)
command += 'G'
bus_send(command)
#
# move xy absolute, path units
#
def move_xy_abs_path(xi,yi,speed):
global xstep,ystep
global mm_per_xstep,mm_per_ystep
global nx,ny,nz,dx,dy,dz,x0,y0
if (nx != 1):
x = x0 + dx*xi/(nx-1.0)
else:
x = x0
if (ny != 1):
y = y0 + dy*yi/(ny-1.0)
else:
y = y0
xpos.SetValue("%.3f"%x)
ypos.SetValue("%.3f"%y)
deltax = x - xstep*mm_per_xstep
deltay = y - ystep*mm_per_ystep
dxstep = int(deltax/mm_per_xstep)
xstep += dxstep
dystep = int(deltay/mm_per_ystep)
ystep += dystep
distance = sqrt(deltax*deltax+deltay*deltay)
dt = int(1000* distance / speed)
send_xy(dxstep,dystep,dt)
#
# move xyz absolute, path units
#
def move_xyz_abs_path(xi,yi,zi,speed):
global xstep,ystep,zstep
global mm_per_xstep,mm_per_ystep,mm_per_zstep
global nx,ny,nz,dx,dy,dz,x0,y0,z0
if (nx != 1):
x = x0 + dx*xi/(nx-1.0)
else:
x = x0
if (ny != 1):
y = y0 + dy*yi/(ny-1.0)
else:
y = y0
if (nz != 1):
z = z0 + dz*zi/(nz-1.0)
else:
z = z0
xpos.SetValue("%.3f"%x)
ypos.SetValue("%.3f"%y)
zpos.SetValue("%.3f"%z)
deltax = x - xstep*mm_per_xstep
deltay = y - ystep*mm_per_ystep
deltaz = z - zstep*mm_per_zstep
dxstep = int(deltax/mm_per_xstep)
xstep += dxstep
dystep = int(deltay/mm_per_ystep)
ystep += dystep
dzstep = int(deltaz/mm_per_zstep)
zstep += dzstep
distance = sqrt(deltax*deltax+deltay*deltay+deltaz*deltaz)
dt = int(1000* distance / speed)
send_xyz(dxstep,dystep,dzstep,dt)
#
# move z absolute, path units
#
def move_z_abs_path(z,speed):
global zstep
global mm_per_zstep
global nz,dz,z0
if (nz != 1):
z = z0 + dz*zi/(nz-1.0)
else:
z = z0
zpos.SetValue("%.3f"%z)
deltaz = z - zstep*mm_per_zstep
dzstep = int(deltaz/mm_per_zstep)
zstep += dzstep
distance = sqrt(deltaz*deltaz)
dt = int(1000* distance / speed)
send_z(dzstep,dt)
#
# move z absolute, mm units
#
def move_z_abs_mm(z,speed):
global zstep
global mm_per_zstep
zpos.SetValue("%.3f"%z)
deltaz = z - zstep*mm_per_zstep
dzstep = int(deltaz/mm_per_zstep)
zstep += dzstep
distance = sqrt(deltaz*deltaz)
dt = int(1000* distance / speed)
send_z(dzstep,dt)
#
# move xyz relative, mm units
#
def move_xyz_rel_mm(dx,dy,dz,speed):
global xstep,ystep,zstep
global mm_per_xstep,mm_per_ystep,mm_per_zstep
dxstep = int(dx / mm_per_xstep)
xstep += dxstep
xpos.SetValue("%.3f"%(xstep*mm_per_xstep))
dystep = int(dy / mm_per_xstep)
ystep += dystep
ypos.SetValue("%.3f"%(ystep*mm_per_xstep))
dzstep = int(dz / mm_per_xstep)
zstep += dzstep
zpos.SetValue("%.3f"%(zstep*mm_per_xstep))
distance = sqrt(dx*dx+dy*dy+dz*dz)
dt = int(1000* distance / speed)
send_xyz(dxstep,dystep,dzstep,dt)
#
# move left mouse down
#
def left_down(event):
global move_state
move_state = 1
left_button.SetLabel('LEFT')
while 1:
wx.Yield()
if (move_state == 0):
return
move_xyz_rel_mm(-mm_per_jog_step,0,0,float(jog_speed.GetValue()))
#
# move left mouse up
#
def left_up(event):
global move_state
move_state = 0
left_button.SetLabel('left')
#
# move right mouse down
#
def right_down(event):
global move_state
move_state = 1
right_button.SetLabel('RIGHT')
while 1:
wx.Yield()
if (move_state == 0):
return
move_xyz_rel_mm(mm_per_jog_step,0,0,float(jog_speed.GetValue()))
#
# move right mouse up
#
def right_up(event):
global move_state
move_state = 0
right_button.SetLabel('right')
#
# move forward mouse down
#
def forward_down(event):
global move_state
move_state = 1
forward_button.SetLabel('FORWARD')
while 1:
wx.Yield()
if (move_state == 0):
return
move_xyz_rel_mm(0,-mm_per_jog_step,0,float(jog_speed.GetValue()))
#
# move forward mouse up
#
def forward_up(event):
global move_state
move_state = 0
forward_button.SetLabel('forward')
#
# move back mouse down
#
def back_down(event):
global move_state
move_state = 1
back_button.SetLabel('BACK')
while 1:
wx.Yield()
if (move_state == 0):
return
move_xyz_rel_mm(0,mm_per_jog_step,0,float(jog_speed.GetValue()))
#
# move back mouse up
#
def back_up(event):
global move_state
move_state = 0
back_button.SetLabel('back')
#
# move up mouse down
#
def up_down(event):
global move_state
move_state = 1
up_button.SetLabel('UP')
while 1:
wx.Yield()
if (move_state == 0):
return
move_xyz_rel_mm(0,0,mm_per_jog_step,float(jog_speed.GetValue()))
#
# move up mouse up
#
def up_up(event):
global move_state
move_state = 0
up_button.SetLabel('up')
#
# move down mouse down
#
def down_down(event):
global move_state
move_state = 1
down_button.SetLabel('DOWN')
while 1:
wx.Yield()
if (move_state == 0):
return
move_xyz_rel_mm(0,0,-mm_per_jog_step,float(jog_speed.GetValue()))
#
# move down mouse up
#
def down_up(event):
global move_state
move_state = 0
down_button.SetLabel('down')
#
# zero x
#
def zero_x(event):
global xstep
xstep = 0
xpos.SetValue("0.000")
#
# zero y
#
def zero_y(event):
global ystep
ystep = 0
ypos.SetValue("0.000")
#
# zero z
#
def zero_z(event):
global zstep
zstep = 0
zpos.SetValue("0.000")
#
# zero xyz
#
def zero_xyz(event):
global xstep,ystep,zstep
xstep = 0
xpos.SetValue("0.000")
ystep = 0
ypos.SetValue("0.000")
zstep = 0
zpos.SetValue("0.000")
#
# home
#
def home(event):
speed_jog = float(jog_speed.GetValue())
height_jog = float(jog_height.GetValue())
move_z_abs_mm(height_jog,speed_jog)
move_xy_abs_path(0,0,speed_jog)
#
# panel
#
wx.Panel.__init__(self,parent)
self.sizer = wx.GridBagSizer(10,10)
self.SetSizer(self.sizer)
#
# label
#
label = wx.StaticText(self,label='to: MTM Snap')
bold_font = wx.Font(10,wx.DEFAULT,wx.NORMAL,wx.BOLD)
label.SetFont(bold_font)
self.sizer.Add(label,(0,0),flag=wx.ALIGN_CENTER_HORIZONTAL)
#
# controls
#
mtm_panel = wx.Panel(self)
mtm_sizer = wx.GridBagSizer(10,10)
mtm_panel.SetSizer(mtm_sizer)
#
mtm_sizer.Add(wx.StaticText(mtm_panel,label='x y z (mm)'),(0,0),flag=wx.ALIGN_RIGHT)
xpos = wx.TextCtrl(mtm_panel,-1,'0.000')
mtm_sizer.Add(xpos,(0,1),flag=wx.ALIGN_CENTER_HORIZONTAL)
ypos = wx.TextCtrl(mtm_panel,-1,'0.000')
mtm_sizer.Add(ypos,(0,2),flag=wx.ALIGN_CENTER_HORIZONTAL)
zpos = wx.TextCtrl(mtm_panel,-1,'0.000')
mtm_sizer.Add(zpos,(0,3),flag=wx.ALIGN_CENTER_HORIZONTAL)
#
zero_xyz_button = wx.Button(mtm_panel,label='zero xyz')
zero_xyz_button.Bind(wx.EVT_BUTTON,zero_xyz)
mtm_sizer.Add(zero_xyz_button,(1,0),flag=wx.ALIGN_CENTER_HORIZONTAL)
zero_x_button = wx.Button(mtm_panel,label='zero x')
zero_x_button.Bind(wx.EVT_BUTTON,zero_x)
mtm_sizer.Add(zero_x_button,(1,1),flag=wx.ALIGN_CENTER_HORIZONTAL)
zero_y_button = wx.Button(mtm_panel,label='zero y')
zero_y_button.Bind(wx.EVT_BUTTON,zero_y)
mtm_sizer.Add(zero_y_button,(1,2),flag=wx.ALIGN_CENTER_HORIZONTAL)
zero_z_button = wx.Button(mtm_panel,label='zero z')
zero_z_button.Bind(wx.EVT_BUTTON,zero_z)
mtm_sizer.Add(zero_z_button,(1,3),flag=wx.ALIGN_CENTER_HORIZONTAL)
#
home_button = wx.Button(mtm_panel,label='home')
home_button.Bind(wx.EVT_BUTTON,home)
mtm_sizer.Add(home_button,(2,0),flag=wx.ALIGN_CENTER_HORIZONTAL)
left_button = wx.Button(mtm_panel,label='left')
left_button.Bind(wx.EVT_LEFT_DOWN,left_down)
left_button.Bind(wx.EVT_LEFT_UP,left_up)
mtm_sizer.Add(left_button,(3,0),flag=wx.ALIGN_CENTER_HORIZONTAL)
back_button = wx.Button(mtm_panel,label='back')
back_button.Bind(wx.EVT_LEFT_DOWN,back_down)
back_button.Bind(wx.EVT_LEFT_UP,back_up)
mtm_sizer.Add(back_button,(2,1),flag=wx.ALIGN_CENTER_HORIZONTAL)
mtm_sizer.Add(wx.StaticText(mtm_panel,label='jog'),(3,1),flag=(wx.ALIGN_CENTER_HORIZONTAL|wx.ALIGN_CENTER))
forward_button = wx.Button(mtm_panel,label='forward')
forward_button.Bind(wx.EVT_LEFT_DOWN,forward_down)
forward_button.Bind(wx.EVT_LEFT_UP,forward_up)
mtm_sizer.Add(forward_button,(4,1),flag=wx.ALIGN_CENTER_HORIZONTAL)
right_button = wx.Button(mtm_panel,label='right')
right_button.Bind(wx.EVT_LEFT_DOWN,right_down)
right_button.Bind(wx.EVT_LEFT_UP,right_up)
mtm_sizer.Add(right_button,(3,2),flag=wx.ALIGN_CENTER_HORIZONTAL)
up_button = wx.Button(mtm_panel,label='up')
up_button.Bind(wx.EVT_LEFT_DOWN,up_down)
up_button.Bind(wx.EVT_LEFT_UP,up_up)
mtm_sizer.Add(up_button,(2,3),flag=wx.ALIGN_CENTER_HORIZONTAL)
down_button = wx.Button(mtm_panel,label='down')
down_button.Bind(wx.EVT_LEFT_DOWN,down_down)
down_button.Bind(wx.EVT_LEFT_UP,down_up)
mtm_sizer.Add(down_button,(3,3),flag=wx.ALIGN_CENTER_HORIZONTAL)
#
mtm_sizer.Add(wx.StaticText(mtm_panel,label='speed (mm/s)'),(4,0),flag=(wx.ALIGN_CENTER_HORIZONTAL|wx.ALIGN_BOTTOM))
move_speed = wx.TextCtrl(mtm_panel,-1,'1')
mtm_sizer.Add(move_speed,(5,0),flag=wx.ALIGN_CENTER_HORIZONTAL)
mtm_sizer.Add(wx.StaticText(mtm_panel,label='jog speed (mm/s) z (mm)'),(4,2),span=(1,2),flag=(wx.ALIGN_CENTER_HORIZONTAL|wx.ALIGN_BOTTOM))
jog_speed = wx.TextCtrl(mtm_panel,-1,'10')
mtm_sizer.Add(jog_speed,(5,2),flag=wx.ALIGN_CENTER_HORIZONTAL)
jog_height = wx.TextCtrl(mtm_panel,-1,'1')
mtm_sizer.Add(jog_height,(5,3),flag=wx.ALIGN_CENTER_HORIZONTAL)
#
mtm_sizer.Add(wx.StaticText(mtm_panel,label='spindle'),(6,0),flag=(wx.ALIGN_RIGHT|wx.ALIGN_CENTER_VERTICAL))
spindle_button = wx.Button(mtm_panel,label='on')
spindle_button.Bind(wx.EVT_BUTTON,spindle)
mtm_sizer.Add(spindle_button,(6,1),flag=wx.ALIGN_CENTER_HORIZONTAL)
mtm_sizer.Add(wx.StaticText(mtm_panel,label='speed (rpm)'),(6,2),flag=(wx.ALIGN_RIGHT|wx.ALIGN_CENTER_VERTICAL))
spindle_speed = wx.TextCtrl(mtm_panel,-1,'10000')
mtm_sizer.Add(spindle_speed,(6,3),flag=wx.ALIGN_CENTER_HORIZONTAL)
#
start_button = wx.Button(mtm_panel,label='start')
start_button.Bind(wx.EVT_BUTTON,start)
mtm_sizer.Add(start_button,(7,0),flag=wx.ALIGN_CENTER_HORIZONTAL)
pause_button = wx.Button(mtm_panel,label='pause')
pause_button.Bind(wx.EVT_BUTTON,pause)
mtm_sizer.Add(pause_button,(7,1),flag=wx.ALIGN_CENTER_HORIZONTAL)
segment = wx.TextCtrl(mtm_panel,-1,'0')
mtm_sizer.Add(segment,(7,2),flag=wx.ALIGN_CENTER_HORIZONTAL)
segment_label = wx.StaticText(mtm_panel,label='segment')
mtm_sizer.Add(segment_label,(7,3),flag=(wx.ALIGN_LEFT|wx.ALIGN_CENTER_VERTICAL))
#
mtm_sizer.Add(wx.StaticText(mtm_panel,label='port'),(8,0),flag=(wx.ALIGN_RIGHT|wx.ALIGN_CENTER_VERTICAL))
port = wx.TextCtrl(mtm_panel,-1,'/dev/ttyUSB0')
mtm_sizer.Add(port,(8,1),span=(1,2),flag=wx.ALIGN_CENTER_HORIZONTAL|wx.EXPAND)
#
self.sizer.Add(mtm_panel,(1,0),flag=(wx.ALIGN_CENTER_HORIZONTAL))
#
# fit
#
self.Fit()