complete sorting code
grip angle determination
more visualisation
added 'p' command to park and powerdown robot