示例#1
0
def main():
    #manual_control()

    kinematics = LinearDeltaKinematics(model)
    comms = DeltaComms()
    comms.microstep = 8
    comms.delay = 0.00001


    pygame.init()
    size = width,height = 800,640
    screen = pygame.display.set_mode(size)
   
    current = [0,0,0]
    target =  [0,0,0]

#    font = pygame.font.Font(None, 36)
#    text = font.render("Click and Drag in the Black Square", 1, (10, 10, 10))
#    textpos = text.get_rect()
#    textpos.centerx = background.get_rect().centerx
#    background.blit(text, textpos)


 
    while 1:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                sys.exit()

        if pygame.mouse.get_pressed()[0]:
            mouse = pygame.mouse.get_pos()
            target[0] = float(mouse[0]-width/2)/10.0
            target[1] = float(mouse[1]-height/2)/10.0


        if pygame.mouse.get_pressed()[2]:
            print "changing z"
            if target[2] == 0:
                target[2] = 10

            elif target[2] == 10:
                target[2] = 0

        print target[2]
        steps = kinematics.zzz_optimal_motion_creator(current,target)
        comms.sendCommands(steps)
        current = list(target)
示例#2
0
def manual_control():

    kinematics = LinearDeltaKinematics(model)
    comms = DeltaComms()
    comms.microstep = 8
    comms.delay = 0.0001


    start =(0,0,0)
    while True:
        command = raw_input()
        x,y,z = command.split()
        stop = (int(x),int(y),int(z))
#        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        zzz = kinematics.xyz_to_zzz(stop)

        z0_steps =[]
        z1_steps =[]
        z2_steps =[]

        if zzz[0] <0:
            z0_steps = [(-1,0,0)]*abs(int(zzz[0]/kinematics.model["z_stepsize"]))
        if zzz[0] >0:
            z0_steps = [(1,0,0)]*abs(int(zzz[0]/kinematics.model["z_stepsize"]))
        if zzz[1] <0:
            z1_steps = [(-1,0,0)]**bs(int(zzz[1]/kinematics.model["z_stepsize"]))
        if zzz[1] >0:
            z1_steps = [(1,0,0)]*abs(int(zzz[1]/kinematics.model["z_stepsize"]))
        if zzz[2] <0:
            z2_steps = [(-1,0,0)]*abs(int(zzz[2]/kinematics.model["z_stepsize"]))
        if zzz[2] >0:
            z2_steps = [(1,0,0)]*abs(int(zzz[2]/kinematics.model["z_stepsize"]))

        comms.sendCommands(z0_steps)
        comms.sendCommands(z1_steps)
        comms.sendCommands(z2_steps)

        quit()
示例#3
0
def main():
    #manual_control()

    kinematics = LinearDeltaKinematics(model)
    comms = DeltaComms()
    comms.microstep = 8
    comms.delay = 0.0001

    greader = GCodeReader()
    #xyz = greader.processfile("lenna.ngc")
    #xyz = greader.processfile("cal3.ngc")
    xyz = greader.processfile("maker.ngc")
    #xyz = greader.processfile("test_file.ngc")

#    xyz = [(0,0,0),\
#           (10,10,0),\
#           (10,-10,0),\
#           (-10,-10,0),\
#           (-10,10,0),\
#           (10,10,0),\
#           (0,0,0)\
#          ]

    #switch the z axis
    for i in range(len(xyz)):
        xyz[i] = (xyz[i][0],xyz[i][1],xyz[i][2])
 
    delays = [0.005,0.001,0.0005,0.0001]

    print xyz
#    quit()

    start =xyz[0]
    stop = (0,0,0)
    for index in range(len(xyz)-1):
        stop = xyz[index+1]
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        print str(start) + " "+str(stop)
        comms.sendCommands(steps)
        start = stop
    stop = (0,0,0)
    steps = kinematics.zzz_optimal_motion_creator(start,stop)
    comms.sendCommands(steps)
    quit()


    for delay in delays:
        comms.delay = delay

        start = (0,0,-60)
        stop = (0,0,0)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)
        
        start = (0,0,0)
        stop = (0,0,-10)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)

        start = (0,0,-10)
        stop = (-10,10,0)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)
    
        start = (-10,10,0)
        stop = (10,10,0)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)

        start = (10,10,0)
        stop = (10,-10,0)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)

        start = (10,-10,0)
        stop = (-10,-10,0)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)

        start = (-10,-10,0)
        stop = (-10,10,0)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)

        start = (-10,10,0)
        stop = (0,0,-10)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)


        start = (0,0,-10)
        stop = (0,0,-60)
        steps = kinematics.zzz_optimal_motion_creator(start,stop)
        comms.sendCommands(steps)
    quit()
示例#4
0
文件: draw.py 项目: pobot/Delta-Robot
def main():
    cv.NamedWindow("w1", cv.CV_WINDOW_AUTOSIZE)
    pathgen = PathGen()
    camera_index =0 
    capture = cv.CaptureFromCAM(camera_index)
    kinematics = LinearDeltaKinematics(model)
    comms = DeltaComms()
    comms.microstep = 8 
    comms.delay = 0.0002

    while True:
        #global capture #declare as globals since we are assigning to them now
        #global camera_index
        in_img = cv.QueryFrame(capture)
        #in_img = cv.LoadImageM("moon.jpg")
        
        #in_img = cv.LoadImageM("lenna.bmp")
        #print in_img.width
        
        grey_img = cv.CreateImage(cv.GetSize(in_img), cv.IPL_DEPTH_8U, 1)
        cv.CvtColor(in_img,grey_img,cv.CV_RGB2GRAY)

        blur_img = cv.CreateImage(cv.GetSize(in_img), cv.IPL_DEPTH_8U, 1)
        cv.Smooth(grey_img,blur_img)

        result_img = cv.CreateImage(cv.GetSize(in_img), cv.IPL_DEPTH_8U, 1)
        cv.Canny(blur_img,result_img,100,220)
       

 
        
        cv.ShowImage("w1", result_img)
        c = cv.WaitKey(10)
        
        if(c==32): #the code for space
            print "Capturing"
            path = pathgen.generatePath(result_img)

            for i in range(len(path)):
                x,y,z = path[i]
                #condition the points
                path[i] = ((float(x)-160)/3.0,(float(y))/3.0,float(z))
            print path
        #    quit()

            start =(0,0,0)
            print "start"
            print start
            #quit()
            stop = path[0]

            test_img  = cv.CreateImage(cv.GetSize(in_img), cv.IPL_DEPTH_8U, 1)
            cv.SetZero(test_img)    


            for index in range(len(path)-1):
                


                x,y,z = path[index+1]
                t  = (int(float(x)+160.0/3),int(float(y)),-int(z))
                cv.Set2D(test_img,t[1],t[0],(255,0,0))
                cv.ShowImage("w1",test_img)
                cv.WaitKey(20)

                steps = kinematics.zzz_optimal_motion_creator(start,stop)
                print str(start) + " "+str(stop)
                print "---->>    " + str(index)
                comms.sendCommands(steps)
                start = stop
                stop = path[index+1]

            start = stop
            stop = (0,0,0)
            steps = kinematics.zzz_optimal_motion_creator(start,stop)
            print str(start) + " "+str(stop)
            comms.sendCommands(steps)
            while True:
                pass #noodle away
            #quit()





        if (c == 113): # the code for "q"
            quit()