First working mechatronic code