Скорее всего алгоритм неверный.
Что производишь при выставлении компиком кода матрицы по X/Y ?
Там от контроллера надо то: от изменения Х по прерыванию взять адрес в таблице и выдать ее ячейку в выходной порт (Y). Пяток тактов на все про все. А вот поддержание таблицы вариантов порта Y актуальной - уже действительно требует времени и непростого алгоритма. Но это то делается непрерывно и неспеша спокойно прерываясь на что угодно.