Added encoder logic to measure the number and direction of steps of the left and right motors. One function added to firmware to read out the values