ZRA – ex24_circle_move.py

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

from i611_extend import *
from i611_io import *
from i611_MCS import *
from i611shm import shm_read 

def main(): 
    ## 1. 초기 설정 ################ 
    rb = i611Robot()
    rb.open()
    IOinit(rb)
    rb.enable_interrupt(0,True)     # 동작 중의「 감속 정지」입력 시의 예외 발생 활성화
    rb.enable_interrupt(1,True)     # 실행중인 비상 정지 입력시 예외 발생을 활성화하려면

    # 오버라이드
    rb.override( 70 )

    ## 2. Tool 초기 설정 ################ 
    #rb.settool( id=1, offx=0.0, offy=0.0, offz=100.0, offrz=0.0, offry=20, offrx=20 )   # Tool 설정 
    #rb.changetool( tid=1 )

    ## 3. 교시 포인트 설정 #############
    StartSpeed  = MotionParam(jnt_speed = 40.0, lin_speed = 50.0, acctime = 0.4, dacctime = 0.4, overlap = 1.0, pose_speed = 80) # Speed 설정
    StartJoint  = Joint(0, -30, -90, 0, -60, 0)     # 시작 설정
    rb.motionparam(StartSpeed)                      # Speed 셋팅  

    ## 4. 동작 조건 설정 #############
    rb.move(StartJoint)                         # Start 위치 이동  
    start_pos = rb.Joint2Position(StartJoint)   # coordinate 변경 
    start_pos = start_pos.offset(dy=-100)       # offset 적용

    try:
        while True:
            ## 5. 로봇 동작의 정의 ########## 
            p1 = start_pos.offset(dx=100,dy=50) # circle  경로1번 설정
            p2 = start_pos.offset(dx=50,dy=200) # circle  경로2번 설정

            print 'mode = 0'
            rb.circlemove(p1,p2,0)  # circle 그리기, (월드 좌표 고정)
            rb.move(StartJoint)     # 시작 위치 이동

            print 'mode = 1'
            rb.circlemove(p1,p2,1)  # circle 그리기, (rz,ry,rx 자세 유지)
            rb.move(StartJoint)     # 시작 위치 이동 

    ## 6. 종료 ################## 
    except Robot_emo:
        print "Program is finish."

    except KeyboardInterrupt:
        rb.abort()
        print "Stop by Ctrl+C"

    finally:
        rb.close()
        
if __name__ == "__main__":
    main()