diff --git a/src/main.c b/src/main.c index 845cc94..de0638b 100644 --- a/src/main.c +++ b/src/main.c @@ -101,6 +101,13 @@ uint16_t ReadADC(void) { * ############################ */ +static inline uint8_t diff(uint8_t a, uint8_t b) { + if(a > b){ + return a - b; + } + return b - a; +} + void MotorSetSavePos() { uint8_t pos = (uint8_t)(ReadADC() >> 2); eeprom_write_byte((uint8_t *)ROM_SS1_ADR, pos); @@ -115,7 +122,11 @@ void MotorMoveTo(uint8_t target) { uint8_t pos = 0; while (target != pos) { pos = (uint8_t)(ReadADC() >> 2); - if (target > pos) { + if (diff(target, pos) < 8){ + MotorCoast(); + break; + } + else if (target > pos) { MotorMove(1); } else { MotorMove(0);