#!/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. 초기 설정② ####################################
    # ZERO 로봇 생성자
    rb = i611Robot()
    # 좌표계의 정의
    _BASE = Base()
    # 로봇과 연결 시작 초기화
    rb.open()
    # I/O 입출력 기능의 초기화 
    IOinit( rb )
    # 교시 데이터 파일 읽기 Pallet Inits
    #         -----------             
    #         |i0j2|i1j2|            
    #         -----------             
    #         |i0j1|i1j1|             
    #         -----------           
    #         |i0j0|i1j0|            
    #         -----------            
    #        
    #           | zero |
    #            ------
    data = Teachdata( "teach_data" ) 
    # Pallet 좌표 
    pos0 = data.get_position( "pos17", 0 )
    pos1 = data.get_position( "pos17", 1 )
    pos2 = data.get_position( "pos17", 2 )

    # Pallet 초기화  ( '주의 i,j  확인 필요' )
    pal1 = Pallet()
    pal1.init_3(pos0,pos1,pos2,5,4) 

    ## 2. 동작 조건 설정 ######################## 
    m = MotionParam( jnt_speed=50, lin_speed=100, pose_speed=100, overlap = 20 )
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )
    
    ## 3. 로봇 동작을 정의 ##############################
    # 작업 시작
    rb.home()

    while True:
    # for 문 i,j 
        for i in range(5):
            for j in range(4):
                print 'Pallet' ,i,j
                pallet = pal1.get_pos(i,j)
                rb.move(pallet.offset(dz=10))
                rb.line(pallet)
                rb.line(pallet.offset(dz=10))
    rb.home()
    ## 4. 종료 ######################################
    # 로봇과의 연결을 종료
    rb.close()
if __name__ == '__main__':
    main()