Added ultrasonic capabilites

Make new commands that display sensors and drives to wall
WIP: DrivetoWallPID

Need to write PIDWrite, IsFinished, tune PID, modify PID tuning code