ZRA – ex08_Pallet.py

				
					#!/usr/bin/python
# -*- coding: utf-8 -*-
## 1. 초기 설정① #######################################
# 라이브러리 가져오기
## 1．초기 설정 ①　모듈 가져오기 ######################
from i611_MCS import *
from teachdata import *
from i611_extend import *
from rbsys import *
from i611_common import *
from i611_io import *
from i611shm import * 

def main():
    
    ### 2. 초기 설정② ####################################
    # i611 로봇
    rb = i611Robot()
    rb.open()
    # I/O 입출력 기능의 초기화 
    IOinit( rb )
    # 좌표계의 정의
    _BASE = Base()

    ## 3. 교시 포인트 설정 #######################
    # 교시 데이터 파일 읽기
    data = Teachdata( "teach_data" )
    pos_0 = Position( -418.30, -398.86, 287.00, 0, 0, -180 )
    pos_1 = Position( -158.54, -395.10, 287.00, 0, 0, -180 )
    pos_2 = Position( -418.89, -200.35, 287.00, 0, 0, -180 )
    # 팔레트 정의(1번째 팔레트)
    pal = Pallet()
    pal.init_3( pos_0, pos_1, pos_2, 5, 4 )
    # 팔레트 조정
    pal.adjust( 4, 3, 0.2, -0.1 )

    # 작업 위치 검색
    p0 = pal.get_pos( 4, 3, 60 )
    p1 = pal.get_pos( 4, 3 )

    p10 = pal.get_pos( 1, 2, 60 )
    p11 = pal.get_pos( 1, 2 )

    ## 4. 동작 조건 설정 ###################################
    m = MotionParam( jnt_speed=10, lin_speed=30, pose_speed=100 )
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )
    
    ## 5. 로봇 동작을 정의 ##############################
    # 작업 시작
    
    # OUT: 50번:0, 49번:0, 48번:0 
    dout(48,'000')
    rb.sleep(0.2)

    rb.home()
    
    print '(4,3) Get'
    rb.move( p0 )
    rb.line( p1 )
    # OUT: 50번:0, 49번:0, 48번:1 
    dout(48,'001')
    rb.sleep(0.5)
    rb.line( p0 )

    print '(1,2) Put'
    rb.move( p10 )
    rb.line( p11 )
    # OUT: 50번:1, 49번:0, 48번:0 
    dout(48,'100')
    rb.sleep(0.5)
    rb.line( p10 )

    rb.home()
    
    ## 6. 종료 ######################################
    # 로봇과의 연결을 종료
    rb.close()

if __name__ == '__main__':
    main()