Микроконтроллеры

А вот это ты зря :-) Тебе надо было сюда кинуть, скажем, только верхний комментарий из того кода и словесное описание "чего надо" и предложить решить задачу. А то мне приходится восстанавливать задачу из кода, а два перевода туда-сюда не всегда дают тождественный результат :-)