Separate code into .h/.c files

Oh, thank you. I was actually wondering, why it does not work, when I have already done it before.

Okay, compiled. I will burn it to my MCU tomorrow, and I will let you now if I run to any additional problems.