Program Examples - Robot Pick and Place (P&P)

From SoftMC-Wiki
Jump to: navigation, search
Language: English  • 中文(简体)‎

Here are 2 examples of doing Robot Pick and Place. One using Cartesian coordinates, second using robot's Joints for movement. We use puma 6 axis robot in this example.

Pick & Place - Cartesian

PnPCartesianPositions.JPG


P2.JPG P11.JPG vid prev.png
P2 P11 Click image for 360° video
'------------------------------------------------------------------------------
' File:        PnPTrn.prg
' Purpose:     Robot Pick & Place
' Version:     1.00
' Author:      Or Karni
' History:     17.JAN.2017    -    created
'------------------------------------------------------------------------------

' module global "constants"


' module global variables
dim shared ZeroPosition as joint of xyzypr = {0 , 0 , 0 , 0 , 0 , 0}                'Moving as joint - for initial positioning
dim shared P1Position as joint of xyzypr = {120,45,0,0,45,0}                        '{J1,J2,J3,J4,J5,J6}

dim shared P1    as location of xyzypr = #{-650 , 1130 , -150 , 0 , 180 , -120}     '{X,Y,Z,Y,P,R}
dim shared P11   as location of xyzypr = #{-650 , 1130 , -50 , 0 , 180 , -120}
dim shared P22   as location of xyzypr = #{650 , 1130 , -50 , 0 , 180 , -60}
dim shared P2    as location of xyzypr = #{650 , 1130 , -150 , 0 , 180 , -60}

dim shared RobotVelocity as double = 40.0

program

    with Puma                                'set default group element, no need to explicitly indicate the motion element
        Attach                               'Attach the task to motion element (with Puma)
            En = TRUE
            Sleep 100
            Vcruise = RobotVelocity          'Set robot's cruise velocity 
            while NOT En
                Sleep 100
            end while
            Move ZeroPosition                'moving to zero position
            WaitForMotion Puma
              call pickPlaceXYZ              'execute Pick and Place function
            Move ZeroPosition
            WaitForMotion Puma
        Detach
    end with    

end program


sub pickPlaceXYZ

    dim i as long

    Move PUMA P1Position
    WaitForMotion Puma
    Puma.BlendingMethod = 2                  'Type of motion blending - 2 is SuperPosition blending - in order to optimize movement, so that it will be as fast as possible
    Puma.BlendingFactor = 80                 'Percentage of the movement’s length that will not be blended with the next movement
    Puma.Cp = 10                             'At continous path blednign, sets the blend radious value - bigger equal smoother
    for i = 1 to 10
        Moves Puma P11 
        WaitForMotion Puma
        Moves Puma P1
        WaitForMotion Puma
        Moves Puma P11
        WaitForMotion Puma
        Moves Puma P22
        WaitForMotion Puma
        Moves Puma P2
        WaitForMotion Puma
        Moves Puma P22
        WaitForMotion Puma
    next i
    Move PUMA ZeroPosition
    WaitForMotion Puma

end sub

Pick & Place - Joints

PnPJnt.PRG:

'------------------------------------------------------------------------------
' File:        PnPJnt.prg
' Purpose:     Robot Pick & Place
' Version:     1.00
' Author:      Eran Korkidi
' History:     10.DEC.2015    -    created
'------------------------------------------------------------------------------

dim shared jntZeroPosition as joint of xyzypr = {0,0,0,0,0,0}

dim shared jntPickHigh  as joint of xyzypr = {120,45,0,0,45,0}                                      'P11
dim shared jntPickLow   as joint of xyzypr = {120 , 49.9504 , 0.352258 , 0 , 39.6973 , 0}           'P1
dim shared jntPlaceHigh as joint of xyzypr = {60,45,0,0,45,0}                                       'P22
dim shared jntPlaceLow  as joint of xyzypr = {60 , 49.9504 , 0.352258 , 0 , 39.6973 , 0}            'P2

dim shared dVelSlow as double = 40.0
dim shared dVelFast as double = 80.0

program

    Sys.Vrate = 100.0
    with Puma
        Attach
            En = TRUE
            Sleep 100
            while NOT En
                Sleep 100
            end while
            Move jntZeroPosition Vcruise=dVelSlow
            call waitMotion
              call pickPlaceJoint
            Move jntZeroPosition Vcruise=dVelSlow
            call waitMotion
        Detach
    end with    

end program


sub pickPlaceJoint

    dim i as long
    dim dVelFac as double = 0.9
    
    Move PUMA jntZeroPosition Vcruise=dVelSlow
    call waitMotion
    Move PUMA jntPickHigh Vcruise=dVelSlow
    call waitMotion
    Puma.BlendingMethod = 1
    Puma.Cp = 50
    puma.J1.PeMax = 3.0 ' 0.2
    for i = 1 to 10
        Move Puma jntPickHigh Vcruise = PUMA.Vmax * dVelFac
        Move Puma jntPickLow Vcruise = PUMA.Vmax
        call waitMotion
        Move Puma jntPickHigh Vcruise = PUMA.Vmax
        Move Puma jntPlaceHigh Vcruise = PUMA.Vmax * dVelFac
        Move Puma jntPlaceLow Vcruise = PUMA.Vmax
        call waitMotion
        Move Puma jntPlaceHigh Vcruise = PUMA.Vmax
    next i
    Move PUMA jntZeroPosition Vcruise = dVelSlow
    call waitMotion

end sub

sub waitMotion
    while puma.IsMoving
        Sleep 1
    end while
end sub