Added encoder and gyroscope code to the auto_nav2 finalized program