Updated Motor control with proper parameters and bug fixes.  Tested and works on Hardware.