Main code for CCSR robot project