Change Sonar Reading to unit MM
Workable Alttutide Control Code

The UpdatePID is not workable here, reason unfound yet