Fixed lag compensation errors

Fixed problems in calculations of speed for lag compensation, and made
the units go back to their current positions if you click on an invalid
position for them to move