Did a huge revamp on Wally's segmentize. It is super slow but the out…

…put is hugely improved. AFAIK the only thing still wrong with it is that I haven't optimized it to run fast. Speed scaling issues are gone. Bed arc adjustment works. Calibration is improved.

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: