code more organized, started developing functionality for arm joint position control