Example #1
0
def moveto(target):
    '''moves the arm to a target coordinate'''
    pathresolution = 2
    path = pp.plan(target, pathresolution)
    anglesbuffer = []
    for coords in path:
        angles = ik.calcInverse(coords)
        anglesbuffer.append(angles)
    for angles in anglesbuffer:
        if angles:
            s1.movea(angles[0])
            s2.movea(angles[1])
            s3.movea(angles[2])
            time.sleep(max(s1.diff, s2.diff, s3.diff) / 3000.0)
Example #2
0
def drawfig(fig):
    table = -163
    lift = 10
    pathresolution = 0.5
    lines = fig.lines
    targets = []
    paths = []
    anglesbuffer = []
    
    targets.append([lines[0][0], lines[0][1], table + lift])

    for i in range(len(lines) - 1):
        targets.append([lines[i][0], lines[i][1], table])
        targets.append([lines[i][2], lines[i][3], table])

        if lines[i + 1][0] != lines[i][2] or lines[i + 1][1] != lines[i][3]:
            targets.append([lines[i][2], lines[i][3], table + lift])
            targets.append([lines[i + 1][0], lines[i + 1][1], table + lift])

    targets.append([lines[len(lines) - 1][0], lines[len(lines) - 1][1], table])
    targets.append([lines[len(lines) - 1][2], lines[len(lines) - 1][3], table])

    targets.append([lines[len(lines) - 1][2], lines[len(lines) - 1][3], table + lift * 2])

    for target in targets:
        paths.append(pp.plan(target, pathresolution))
    for path in paths:
        for coords in path:
            angles = ik.calcInverse(coords)
            anglesbuffer.append(angles)
    for angles in anglesbuffer:
        if angles:
            s1.movea(angles[0])
            s2.movea(angles[1])
            s3.movea(angles[2])
            time.sleep(max(s1.diff, s2.diff, s3.diff) / 2000.0)
        else:
            print "invalid angle"