 Program
   Init Variables
   Robot Program
     GET_POSITION≔read_input_boolean_register(0)
     moveJ≔read_input_boolean_register(1)
     moveL≔read_input_boolean_register(2)
     J_1≔read_input_float_register(0)
     J_2≔read_input_float_register(1)
     J_3≔read_input_float_register(2)
     J_4≔read_input_float_register(3)
     J_5≔read_input_float_register(4)
     J_6≔read_input_float_register(5)
     TCP_X≔read_input_float_register(6)
     TCP_Y≔read_input_float_register(7)
     TCP_Z≔read_input_float_register(8)
     TCP_RX≔read_input_float_register(9)
     TCP_RY≔read_input_float_register(10)
     TCP_RZ≔read_input_float_register(11)
     write_output_integer_register(0, actual_state)
     Switch actual_state
       Case 1
         MoveJ
           Home_positon
         actual_state≔2
       Case 2
         If GET_POSITION≟ True 
           actual_state≔3
       Case 3
         If moveJ≟ False  and moveL≟ True 
            movel(p[TCP_X,TCP_Y,TCP_Z,TCP_RX,TCP_RY,TCP_RZ])
           actual_state≔4
         ElseIf moveJ≟ True  and moveL≟ False 
            movej([J_1,J_2,J_3,J_4,J_5,J_6])
           actual_state≔4
       Case 4
         If GET_POSITION≠ True 
           actual_state≔2
