cleaning up
updated joints and corrected some calculation issues
cleaning up
corrected speed issues