예제 #1
0
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)
예제 #2
0
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