Everything working! still need to optimize and implement some of the
functions under Autonomous class