diff --git a/FILAMENT DRIVE/INVENTOR FILES/lockfile.lck b/FILAMENT DRIVE/INVENTOR FILES/lockfile.lck deleted file mode 100644 index ff16597..0000000 Binary files a/FILAMENT DRIVE/INVENTOR FILES/lockfile.lck and /dev/null differ diff --git a/Wally/DOCUMENTATION/INVERSE KINEMATICS.png b/Wally/DOCUMENTATION/INVERSE KINEMATICS.png new file mode 100644 index 0000000..1c74afa Binary files /dev/null and b/Wally/DOCUMENTATION/INVERSE KINEMATICS.png differ diff --git a/Wally/GCODE PREPROCESSOR/wally segmentize.py b/Wally/GCODE PREPROCESSOR/wally segmentize.py index c86da66..5c2da63 100644 --- a/Wally/GCODE PREPROCESSOR/wally segmentize.py +++ b/Wally/GCODE PREPROCESSOR/wally segmentize.py @@ -1,76 +1,190 @@ -import math,copy,turtle +import math,copy,turtle,sys -front_z=152.0 -back_z=151.5 -square_z=75 #the z height where the bed arms are at 90 degrees +try: + import numpy +except: + print "Download numpy." + raw_input("PRESS ENTER TO EXIT PROGRAM") + sys.exit() + +try: + import scipy.optimize + import scipy.interpolate + +except: + print "Download scipy." + raw_input("PRESS ENTER TO EXIT PROGRAM") + sys.exit() + +#various machine coordinate sets where the effector barely touches the bed +touch_points=[(1000,1000,149.6),(1300,1300,150.5),(800,1400,150.1),(1400,800,150.3),(1200,1200,150.3)] + +#CALIBRATION DATA FOR THE BED HEIGHT +z_machine_actual=[(1,0.08),(2,0.84),(3,1.76),(4,2.68), + (5,3.62),(6,4.52),(7,5.45),(8,6.35), + (9,7.21),(10,8.13),(11,9.11),(12,10.03), + (13,11.05),(14,12.11),(15,13.19),(16,14.3), + (17,15.42),(18,16.51),(19,17.61),(20,18.71), + (21,19.74),(22,20.72),(23,21.65),(24,22.57), + (25,23.5),(26,24.44),(27,25.41),(28,26.39), + (29,27.41),(30,28.45),(31,29.51),(32,30.61), + (33,31.69),(34,32.77),(35,33.87),(36,34.94), + (37,36.01),(38,37.01),(39,37.97),(40,38.97), + (41,39.89),(42,40.85),(43,41.81),(44,42.79), + (45,43.84),(46,44.88),(47,45.96),(48,47.02), + (49,48.11),(50,49.24),(51,50.34),(52,51.42), + (53,52.48),(54,53.5),(55,54.5),(56,55.48), + (57,56.47),(58,57.43),(59,58.41),(60,59.43), + (61,60.48),(62,61.52),(63,62.57),(64,63.65), + (65,64.75),(66,65.83),(67,66.94),(68,68.01), + (69,69.09),(70,70.13),(71,71.15),(72,72.14), + (73,73.1),(74,74.07),(75,75.08),(76,76.08), + (77,77.13),(78,78.14),(79,79.19),(80,80.25), + (81,81.32),(82,82.38),(83,83.44),(84,84.51), + (85,85.58),(86,86.66),(87,87.62),(88,88.6), + (89,89.54),(90,90.5),(91,91.47),(92,92.45), + (93,93.47),(94,94.5),(95,95.55),(96,96.6), + (97,97.62),(98,98.65),(99,99.72),(100,100.76), + (101,101.83),(102,102.86),(103,103.85),(104,104.85), + (105,105.81),(106,106.8),(107,107.72),(108,108.68), + (109,109.7),(110,110.7),(111,111.71),(112,112.73), + (113,113.75),(114,114.76),(115,115.8),(116,116.84), + (117,117.87),(118,118.88),(119,119.87),(120,120.85), + (121,121.74),(122,122.73),(123,123.63),(124,124.54), + (125,125.49),(126,126.46),(127,127.42),(128,128.4), + (129,129.39),(130,130.39),(131,131.4),(132,132.32), + (133,133.42),(134,134.44),(135,135.42),(136,136.4), + (137,137.37),(138,138.29),(139,139.2),(140,140.1), + (141,141.05),(142,141.97),(143,142.95),(144,143.89), + (145,144.87),(146,145.85),(147,146.86),(148,147.85), + (149,148.87),(150,149.86),(151,150.86),(152,151.87), + (153,152.84),(154,153.77),(155,154.68),(156,155.57)] + +#the z height where the bed arms are at 90 degrees +square_z=76.97 + +#LENGTH OF ARMS l=150 + +#DISTANCE BETWEEN SHOULDERS L=250 -d=200 #print diameter + +#DISTANCE FROM BED ARM ATTACHMENT TO THE CENTER OF THE BED IN THE Y DIRECTION y_offset=37.5 -mechanical_advantage=9.5 +#Using "G1 X? Y?" to find the machine coordinates that make the arms colinear +straight_forearms=1021 -front_z=float(front_z) -back_z=float(back_z) +machine_z=numpy.array([i for i,j in z_machine_actual]) +actual_z=numpy.array([j for i,j in z_machine_actual]) square_z=float(square_z) l=float(l) L=float(L) -d=float(d) y_offset=float(y_offset) -mechanical_advantage=float(mechanical_advantage) -def testcode(x,y,z): - a,b,c=transform(x,y,z) - return "G1 X"+str(a)+" Y"+str(b)+" Z"+str(c)+" F9000" +straight_forearms=float(straight_forearms) +mechanical_advantage=(straight_forearms/200.0*math.pi+math.asin(1-L/2.0/l)+math.asin(L/4.0/l))/(math.pi-math.acos(1-L/2.0/l)) + + + + + +def interpolate2(v,leftLookup=True): + try: + x=machine_z + y=actual_z + if leftLookup: + f = scipy.interpolate.interp1d(x, y)#, kind='cubic') + else: + f = scipy.interpolate.interp1d(y,x) + return f(v) + except: + return 0 + + """total_weight=0 + new_v=0 + table.sort() + for m,a in table: + if not leftLookup: + m,a=a,m + if m!=v: + weight=1.0/(m-v)**2 + else: + weight=100.0 + new_v+=weight*a + total_weight+=weight + return new_v/total_weight""" +#print interpolate2(0) -"""def transform(x,y,z): - initial_angle=math.acos(L/2/l/2) + + +def machine2reference((x,y,z)): + zprime=interpolate2(z) + def func((i,j)): + (x2,y2,z2)=reference2machine((i,j,0)) + return (x-x2)**2+(y-y2)**2 + xprime,yprime=scipy.optimize.fmin(func,(100,-100),disp=False) + return xprime,yprime,zprime + +def reference2machine((x,y,z)): + try: + zprime=interpolate2(z,False) + initial_angle=math.acos(L/(4*l)) + left_leg=math.sqrt(x*x+y*y) + right_leg=math.sqrt((L-x)*(L-x)+y*y) + left_elbow=math.acos((left_leg*left_leg-2*l*l)/(-2*l*l)) + right_elbow=math.acos((right_leg*right_leg-2*l*l)/(-2*l*l)) + left_small_angle=(math.pi-left_elbow)/2 + right_small_angle=(math.pi-right_elbow)/2 + left_virtual=math.atan(-y/x) + right_virtual=math.atan(-y/(L-x)) + left_drive=left_small_angle+left_virtual-initial_angle + right_drive=right_small_angle+right_virtual-initial_angle + left_stepper=-left_drive+(math.pi-left_elbow)*mechanical_advantage + right_stepper=-right_drive+(math.pi-right_elbow)*mechanical_advantage + return left_stepper*200/math.pi,right_stepper*200/math.pi,zprime + except: + return 0,0,0 + +def refPlane(): + def func((a,b,c,d)): + v=0 + for p in touch_points: + x,y,z=machine2reference(p) + #print x,y,z + v+=(a*x+b*y+c*z+d)**2 + return v + a,b,c,d=scipy.optimize.fmin(func,(1,1,1,1),disp=False) + return a,b,c,d + +print "Finding bed level from touch points. This may take a while." +ap,bp,cp,dp=refPlane() +#print ap,bp,cp,dp + +#print machine2reference((1000,1000,100)) +#print reference2machine((125,125,100)) +def actual2reference((x,y,z)): bed_angle=math.asin((z-square_z)/l) leg_offset=l*math.cos(bed_angle) - ypri20me=+y-y_offset-leg_offset - #print yprime + yprime=y+y_offset-leg_offset xprime=x+L/2 - topz=((d/2-y)/d)*front_z+(1-(d/2-y)/d)*back_z - bed_tilt=math.atan2(-back_z+front_z,d) - yprime-=z*math.sin(bed_tilt) - zprime=topz-z*math.cos(bed_tilt) - a=math.sqrt(xprime**2+yprime**2) - b=math.sqrt(xprime**2+yprime**2+L**2-2*xprime*L) - c=math.acos((2*l**2-a**2)/2/l/l) - dp=math.acos((2*l**2-b**2)/2/l/l) - e=math.atan(-yprime/xprime) - f=math.atan(-yprime/(L-xprime)) - g=e+(math.pi-c)/2-initial_angle - h=f+(math.pi-dp)/2-initial_angle - stepper1=c*mechanical_advantage+g - stepper2=dp*mechanical_advantage+h - return stepper1,stepper2,zprime""" + zero_z=(-dp-ap*xprime+bp*yprime)/cp + zprime=zero_z-z + return xprime,yprime,zprime +#print actual2reference((0,0,0)) + +def reference2actual((x,y,z)): + pass def transform(x,y,z): - initial_angle=math.acos(L/(4*l))# - bed_angle=math.asin((z-square_z)/l)# - leg_offset=l*math.cos(bed_angle) - yprime=y+y_offset-leg_offset - xprime=x+L/2 - topz=((d/2-y)/d)*front_z+(1-(d/2-y)/d)*back_z - bed_tilt=math.atan2(-back_z+front_z,d) - yprime-=z*math.sin(bed_tilt) - zprime=topz-z*math.cos(bed_tilt) - left_leg=math.sqrt(xprime*xprime+yprime*yprime) - right_leg=math.sqrt((L-xprime)*(L-xprime)+yprime*yprime) - left_elbow=math.acos((left_leg*left_leg-2*l*l)/(-2*l*l)) - right_elbow=math.acos((right_leg*right_leg-2*l*l)/(-2*l*l)) - left_small_angle=(math.pi-left_elbow)/2 - right_small_angle=(math.pi-right_elbow)/2 - left_virtual=math.atan(-yprime/xprime) - right_virtual=math.atan(-yprime/(L-xprime)) - left_drive=left_small_angle+left_virtual-initial_angle - right_drive=right_small_angle+right_virtual-initial_angle - left_stepper=-left_drive+(math.pi-left_elbow)*mechanical_advantage - right_stepper=-right_drive+(math.pi-right_elbow)*mechanical_advantage - #print "left_angle: "+str(left_drive)+" left_elbow: "+str(math.pi-left_elbow) - #print "right_angle: "+str(left_drive)+" right_elbow: "+str(math.pi-right_elbow) - return left_stepper*200/math.pi,right_stepper*200/math.pi,zprime + return reference2machine(actual2reference((x,y,z))) +#print transform(0,0,0) + +def testcode(x,y,z): + a,b,c=transform(x,y,z) + #print transform(x,y,z) + return "G1 X"+str(a)+" Y"+str(b)+" Z"+str(c)+" F9000" +#print testcode(0,0,0) def getABC(position1): global coord @@ -92,7 +206,7 @@ def getABC(position1): position['F']=fnew coord=position1 return position - +#print testcode(0,0,0) def distance(start, end): @@ -102,6 +216,8 @@ def distance(start, end): return math.sqrt((x1-x2)**2+(y1-y2)**2+(z1-z2)**2) except: return 0 +#print testcode(0,0,0) + def interpolate(start, end, i, n): x1,y1,z1,e1=start['X'],start['Y'],start['Z'],start['E'] @@ -114,6 +230,7 @@ def interpolate(start, end, i, n): middle[c]=end[c] return middle + def segmentize(start,end,maxLength): l=distance(start,end) if l<=maxLength: @@ -126,7 +243,7 @@ def segmentize(start,end,maxLength): return output -print testcode(0,0,0) + f=file(raw_input("Input File: ")) coord={"X":0,"Y":0,"Z":0, "E":0, "F":0} prefixes="MGXYZESF" @@ -162,7 +279,10 @@ def segmentize(start,end,maxLength): for c in coord: if c in stuff: coord[c]=stuff[c] -for line in program: +for i in range(len(program)): + line=program[i] + if i%100==0: + print str(i*100.0/len(program))+"%" abcline=getABC(line) for letter in prefixes: if letter in abcline and letter in commands: