Ну вначале нужна инициализация портов/переменных и etc.

Для работы процедуры нужны как минимум две статических переменных. а)
Предыдущее состояние кнопок и б)
предыдущее устойчивое состояние кнопок.
1. С выбранным периодом (у вас это 20мс) , который должен быть не меньше времени дребезга контактов считываем/сканируем
текущее состояние кнопок. Для удобства представляем состояния кнопок так, чтобы нажатая кнопка давала лог.1 , а не нажатая лог.0. При необходимости инвертируем состояния во время сканирования.
2. XORим
текущее состояние с
предыдущим состоянием? Если нуль, то состояния совпадают (устойчивое состояние) -> начинаем анализ. Если не нуль (состояния не совпадают,
неустойчивое), то записываем
текущее состояние как
предыдущее и на п5 (выход из процедуры).
3. Первым делом проверяем нажата ли хоть одна кнопка (опять сравнение
текущего с нулем). Если нуль (ни одна не нажата), то и обрабатывать нечего переход на п4.
Если иначе, то XORим
текущее состояние с
предыдущим устойчивым состоянием. Если не нуль (т.е. в устойчивом состоянии произошли изменения), то побитно в цикле обрабатываем состояние кнопок и складываем коды нажатых (при необходимости и отжатых тоже) в буфер клавиатуры. Обработку делаем как циклический сдвиг битовой маски и накладывание ее на
текущее состояние проXORенное с
предыдущим устойчивым состоянием. Этим мы устраняем повторную обработку уже нажатых и обработанных в предыдущей процедуре клавиш. При таком способе можно генерировать специальные коды при нажатии и удержании комбинации клавиш и отдельно фиксировать нажатие и отпускание нескольких клавиш.
4. Записываем текущее состояние как
предыдущее устойчивое состояние.
5. Выход из процедуры.
Вот такой примерно алгоритм я когда-то реализовывал. Кроме собственно обработки нажатия клавиш у меня был реализован автоповтор последней нажатой клавиши.