Beginning Matlab functions for the controlling of Billy. Functions need refining and combining. Further work that needs to be done: a PID controller for the feedback of the rotation encoders received by getRTC(), and then the motion dependent upon the converted angles from inverseKin() BUT relative to the angles in getRTC().