Updated Position

Created Function to initialize home positions. Removed unnecessary
parts of main() function.