#!/usr/bin/python
# -*- coding: utf-8 -*-

## 1. 초기 설정① #######################################
# 라이브러리 가져오기
from i611_MCS import *
from i611_extend import *
from i611_io import *
from teachdata import *
from rbsys import *
from i611_common import *
from i611shm import * 

def main():
  
    ## 2. 초기 설정② ####################################
    # i611 로봇 생성자
    rb = i611Robot()
    # 좌표계의 정의
    _BASE = Base()
    # 로봇과 연결 시작 초기화
    rb.open()
    # I/O 입출력 기능의 초기화 (I/O 미사용시 생략 가능 )
    IOinit( rb )

    data = Teachdata( "teach_data" )
    # 오버라이드
    rb.override( 50 )
  
    ## 3. 교시 포인트 설정 ######################
    p1 = Position( 95, -280, 425, -120, 84, -28 )
    p2 = Position( 95, -280, 240, 154, 80, -114 )
    p3 = Position( 300, -280, 240, 159, 86, -156 )
    p4 = p3.offset( dz=40 )
    j1 = Joint( 230, -1, -92, 90, 5, 89 )
 
    ## 4. 동작 조건 설정 ##################################
    #MotionParam 생성자에서 동작 조건 설정
    m = MotionParam( jnt_speed=10, lin_speed=70 )
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )
    ## 5. 로봇 동작을 정의 ##############################
    # 작업 시작
    rb.home()
    rb.move( p1.offset(dz=30) )
    rb.line( p2, p3, p4 )
    rb.move( j1 )
    rb.home()
 
    ## 6. 종료 ######################################
    # 로봇과의 연결을 종료
    rb.close()

if __name__ == '__main__':
    main()