Final Code for ELEC 299 Robot Competition - Reached Finals