Added util assert and addressed some TODO's such as checking for null values and making turn speed a field.