Final code tweaks - robot is more or less competition ready