Added drive squares and finetuned parameters such that driving around works more than 1 hour