def armSpace(x1,y1,z1,x2,y2,z2):
#print x1,y1,z1,',',x2,y2,z2
#Find base angles
angles1 = jointAngles(x1,y1,z1)
angles2 = jointAngles(x2,y2,z2)
theta1 = radians(angles1[0])
theta2 = radians(angles2[0])
#Convert to mm and find extension distance
x1 = ticToMm(x1)
y1 = ticToMm(y1)
z1 = ticToMm(z1)
x2 = ticToMm(x2)
y2 = ticToMm(y2)
z2 = ticToMm(z2)
dist1 = sqrt(x1**2 + y1**2 + z1**2)
dist2 = sqrt(x2**2 + y2**2 + z2**2)
length1 = dist1 + 40 #length from base to end of arm
length2 = dist2 + 40 #length from base to end of arm
#print "Len 1:", length1
#print "Len 2:", length2
pos1 = armLocation(length1,theta1,[x1,y1])
pos2 = armLocation(length2,theta2,[x2,y2])
if theta1 == theta2:
return pos1
elif theta1 < theta2:
return buildWorkspace(pos1, pos2)
elif theta1 > theta2:
return buildWorkspace(pos2, pos1)
def closingArea(x1,y1,z1,x2,y2,z2):
"""
"""
prevTime = int(round(time()*1000)) #ms
print 'start time', prevTime
workSpace = armSpace(x1,y1,z1,x2,y2,z2)
speed = radians(90.0)/1500.0 #Rad/ms
theta = 0
oldx = x1
oldy = y1
#Find initial parameters
l1 = lineLength([x1, y1], [0, 0])
print 'start length', l1
l2 = lineLength([x2, y2], [0, 0])
l3 = lineLength([x1, y1], [x2, y2])
psy = acos((l1**2 + l2**2 - l3**2)/(2*l1*l2)) #Angle Origin-Point1-Point2
phi = psy + radians(jointAngles(x1,y1,z1)[0]) #Angle of line off vertical from start to end points
print 'phi', phi
while True:
currentTime = int(round(time()*1000)) #ms
print 'current time', currentTime
deltaTime = currentTime - prevTime #ms
print 'delta time', deltaTime
theta += speed*deltaTime #Degrees
print 'angle change', theta
totalTravel = (l1*sin(theta))/sin(pi - theta - psy)
print 'total travel', totalTravel
newx = oldx + totalTravel*sin(phi)
newy = oldy + totalTravel*cos(phi)
print 'new coords', newx, newy
workSpace = armSpace(newx,newy,z1,x2,y2,z2)
#Publish workspace
oldx = newx
oldy = newy
prevTime = currentTime #ms
