Updated code, robot should work