ZRA – ex12_IO_Control.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 * 

def main():
    ## 2. 초기 설정② ####################################
    # ZERO 로봇 생성자
    rb = i611Robot()
    # 좌표계의 정의
    _BASE = Base()
    # 로봇과 연결 시작 초기화
    rb.open()
    # I/O 입출력 기능의 초기화 
    IOinit( rb )
    # 교시 데이터 파일 읽기
    data = Teachdata( "teach_data" )
    
    ## 1. 교시 포인트 설정 ######################
    p1 = data.get_position( "pos1", 0 )
    p2 = data.get_position( "pos1", 1 )
    p3 = data.get_position( "pos1", 2 )

    ## 2. 동작 조건 설정 ######################## 
    m = MotionParam( jnt_speed=10, lin_speed=100, pose_speed=100, overlap = 100 )
    #MotionParam 형으로 동작 조건 설정
    rb.motionparam( m )

    dout(20, '0000')

    ## 3. 로봇 동작을 정의 ##############################
    # 작업 시작
    while True:
        din_4 = din(4)
        din_5 = din(5)
        din_6 = din(6)
        din_7 = din(7)
                
        # input 0 신호가 '1' 경우
        if din_4 == '1':
            # OUT: 23번:0, 22번:0, 21번:0, 20번:1 
            dout(20, '0001')
            rb.move( p1)
        # input 0 신호가 '0' 경우 
        elif din_5 == '1':
            # OUT: 23번:0, 22번:0, 21번:1, 20번:0  
            dout(20, '0010')
            rb.move( p2)

        elif din_6 == '1':
            # OUT: 23번:0, 22번:1, 21번:0, 20번:0    
            dout(20, '0100')
            rb.move( p3)

        elif din_7 == '1':
            # OUT: 23번:1, 22번:0, 21번:0, 20번:0  
            dout(20, '1000')
            rb.home()

            dout(20, '0000')

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