Fixed minor bugs after testing with robot