ZRA – ex18_Thread.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 * 
import thread

def aa(rb):
    try:
        while True:
            Joint_list = shm_read( 0x3050, 6 ).split( ',' ) 
            Joint_list[0] = math.degrees(float(Joint_list[0])) # Joint 1 위치
            Joint_list[1] = math.degrees(float(Joint_list[1])) # Joint 2 위치
            Joint_list[2] = math.degrees(float(Joint_list[2])) # Joint 3 위치
            Joint_list[3] = math.degrees(float(Joint_list[3])) # Joint 4 위치
            Joint_list[4] = math.degrees(float(Joint_list[4])) # Joint 5 위치
            Joint_list[5] = math.degrees(float(Joint_list[5])) # Joint 6 위치

            print 'Joint',Joint_list[0],Joint_list[1],Joint_list[2],Joint_list[3],Joint_list[4],Joint_list[5]
            rb.sleep(0.1)

            XYZ_list = shm_read( 0x3000, 6 ).split( ',' ) 
            XYZ_list[0] = float(XYZ_list[0]) # XYZ X 위치
            XYZ_list[1] = float(XYZ_list[1]) # XYZ Y 위치
            XYZ_list[2] = float(XYZ_list[2]) # XYZ Z 위치
            XYZ_list[3] = math.degrees(float(XYZ_list[3])) # XYZ Rz 위치
            XYZ_list[4] = math.degrees(float(XYZ_list[4])) # XYZ Ry 위치
            XYZ_list[5] = math.degrees(float(XYZ_list[5])) # XYZ Rx 위치

            print 'XYZ',XYZ_list[0],XYZ_list[1],XYZ_list[2],XYZ_list[3],XYZ_list[4],XYZ_list[5]
            rb.sleep(0.1) 

            #시스템 포트의 상태를 획득
            running  = rb.get_system_port()[0]   #로봇 프로그램 상태 
            svon     = rb.get_system_port()[1]   #서보 상태
            emo      = rb.get_system_port()[2]   #비상 정지 상태
            hw_error = rb.get_system_port()[3]   #시스템 정의 에러 ( 치명적 ) 상태
            sw_error = rb.get_system_port()[4]   #시스템 정의 에러 상태
            abs_lost = rb.get_system_port()[5]   #ABS 소실 상태
            in_pause = rb.get_system_port()[6]   #일시 정지 상태
            error    = rb.get_system_port()[7]   #시스템 에러 상태

            print '상태',running,svon,emo,hw_error,sw_error,abs_lost,in_pause,error 
            rb.sleep(1)

        
    except Robot_emo:
        rb.exit(1)

def main():
    ## 2. 초기 설정② ####################################
    # ZERO 로봇 생성자
    rb = i611Robot()
    # 좌표계의 정의
    _BASE = Base()
    # 로봇과 연결 시작 초기화
    rb.open()
    # I/O 입출력 기능의 초기화 
    IOinit( rb )
    # 교시 데이터 파일 읽기
    data = Teachdata( "teach_data" )
   
    # Thread 시작
    th = threading.Thread(target=aa, args= (rb,))
    th.setDaemon(True) 
    th.start()
    
    ## 1. 교시 포인트 설정 ######################
    p1 = data.get_position( "pos5", 0 )
    p2 = data.get_position( "pos5", 1 )
    ## 2. 동작 조건 설정 ######################## 
    m = MotionParam( jnt_speed=10, lin_speed=10, pose_speed=10, overlap = 30 )
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )
    rb.enable_interrupt(0,True) # 동작 중에 감속 정지 입력 시의 예외 발생을 활성화
    rb.enable_interrupt(1,True) # 동작 중에 비상 정지 입력 시의 예외 발생을 활성화

    ## 3. 로봇 동작을 정의 ##############################
    # 작업 시작
    try:
        while True:
            rb.move(p1) 
            rb.move(p2)

    except Robot_emo:
        print "Robot_emo"
    except KeyboardInterrupt:  # "ctrl" + "c" 버튼 입력
        print "KeyboardInterrupt"   
        rb.home()
    finally:
        print "finally"
        rb.close()
      
    ## 4. 종료 ######################################
    # 로봇과의 연결을 종료
    rb.close()
if __name__ == '__main__':
    main()