Firmware skeleton outlined.

Registers map added.
Some inititalization implemented.
Basic logic behind all algorithm created.