Improved diffdrive and diffdrive_ultrasonic

Updated README