Added dstage and dmsg macros to debug.h.
Finished implementing and testing multifinger cartesian greedy controller. The demo program (test_multifinger_greedy_cartesian_planner_main.cpp) allows the user to specify a desired position and finger spread and it distributes the finger goals accordinaly around the point target (always oriented to be pointing forward), and moves to that goal configuration.