Fixed various bugs got bot driving
Got ultrasounds all working and stuff