Cleaned up the mapping from speed to angle. Also added some comments and formatting.