Create main.c

Main code for MCU. Small pseudocode. Implementation needed.