Encoder distance calculations
Communication working properly