Modularize functions

[skulumani]

It might make sense to separate the functionality of ROS and the non-ROS related functions.

For example, the controller functions should simply accept the state, desired state, and constants and output the required Moment or Force.

A separate function should then output this value to ROS and the hardware.

This will enable easier testing using gtest