Added methods to Robot class, changed main function
Added switches and implemented changes in FindAngle()