Found some constants. Added some functions. Fixed motor setup so that ultrasound is in front.