diff --git a/ardumower/robot.cpp b/ardumower/robot.cpp index 02bc34a..b476956 100644 --- a/ardumower/robot.cpp +++ b/ardumower/robot.cpp @@ -4394,12 +4394,21 @@ void Robot::checkBumpers() { motorLeftRpmCurr = motorRightRpmCurr = 0 ; motorLeftPWMCurr = motorRightPWMCurr = 0; setMotorPWM( 0, 0, false ); - if (bumperLeft) { - ShowMessageln("Bumper left trigger"); - reverseOrBidir(LEFT); - } else { - ShowMessageln("Bumper right trigger"); - reverseOrBidir(RIGHT); + if (stateCurr == STATE_PERI_OUT_STOP) + { + odometryRight = stateEndOdometryRight; + odometryLeft = stateEndOdometryLeft; + } + else + { + if (bumperLeft) + { + ShowMessageln("Bumper left trigger"); + reverseOrBidir(LEFT); + } else { + ShowMessageln("Bumper right trigger"); + reverseOrBidir(RIGHT); + } } } @@ -5753,6 +5762,7 @@ void Robot::loop() { case STATE_PERI_OUT_STOP: motorControlOdo(); + checkBumpers(); if (((odometryRight >= stateEndOdometryRight) && (odometryLeft >= stateEndOdometryLeft))) if (motorLeftPWMCurr == 0 && motorRightPWMCurr == 0) { //wait until the 2 motors completly stop because rotation is inverted setNextState(STATE_PERI_OUT_REV, rollDir);