#!/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 * 
from zeusteach import *

def main():
    ## 1. 초기 설정② ####################################
    # i611 로봇 생성자
    rb = i611Robot()
    # 좌표계의 정의
    _BASE = Base()
    # 로봇과 연결 시작 초기화
    rb.open()
    # I/O 입출력 기능의 초기화 
    IOinit( rb )
    
    # 2.교시 데이터 생성
    zt          = ZeusTeach() 
    
    ### 교시 데이터 생성####################################
    # Target Joint 위치
    Joint_Start    = zt.TJoint(5)
    Pickup_A       = zt.TPosition(5)
    Place_A        = zt.TPosition(5)

    
    ## 3. 동작 조건 설정 ######################## 
    m = MotionParam( jnt_speed=10, lin_speed=60, pose_speed=100, overlap = 30 )
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )
    
    ## 4. 로봇 동작을 정의 ##############################
    # 작업 시작
    while True:
        try:
            # Move Home 위치 이동
            rb.home()
            # Joint_Start[0] 위치로 이동
            rb.move( Joint_Start[0] )

            # Pickup_A[0] 이동 
            rb.line(Pickup_A[0].offset(dz=100))
            rb.line(Pickup_A[0])
            rb.line(Pickup_A[0].offset(dz=100))

            # Place_A[0] 이동
            rb.line(Place_A[0].offset(dz=100))
            rb.line(Place_A[0])
            rb.line(Place_A[0].offset(dz=100))

        except Exception as e:
            rb.close() 
            print e

    ## 4. 종료 ######################################
    # 로봇과의 연결을 종료
    rb.close()
if __name__ == '__main__':
    main()