Initial code

Initial code to be tested once hardware finished