gin66 / FastAccelStepper

A high speed stepper library for Atmega 168/328p (nano), Atmega32u4, Atmega 2560, ESP32, ESP32S2, ESP32S3, ESP32C3 and Atmel SAM Due
MIT License
283 stars 67 forks source link

relative move() after stopMove() continues old move #172

Closed microtronics closed 1 year ago

microtronics commented 1 year ago

I encountered a bug that becomes present when switching from 0.29.0 to 0.29.1: When performing a move that is being interrupted by stopMove() (called from an interrupt) and then performing a relative move(1) the driver will continue the stopped move.

move(-1000);
interrupt -> stopMove(); //light barrier triggered
delay(10000); //wait time doesn't matter
move(1);  //motor moves in negative direction until reaching -1000 first

I currently can't replicate the issue without interrupts

gin66 commented 1 year ago

A similar issue could be reproduced and the test code is in examples. If the move() is called, while the motor is still decelerating as part of the stopMove(), then the former target position has been used as reference. The fix was to use the current position at queue end as reference, which is closer to the expectation. The test is included in the automatic test suite of github actions.

"similar" because this does not fit perfectly to your description:

So please check, if the pre-0.29.3 fixes your issue.

microtronics commented 1 year ago

The bug is still present using the master branch. The delay really doesn't matter here. I can even hear the stepper driver switching to hold current, so the motor has really come to a full stop.

It also seems to be tightly related to Interrupts:

constexpr uint8_t SW_PIN =        2;

volatile int pinState;
void swInterrupt() {
    pinState = digitalRead(SW_PIN);
    if (pinState) {
        stepper->stopMove();
    }
}

void setup() {
    pinMode(SW_PIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(SW_PIN), swInterrupt, CHANGE);
    pinState = digitalRead(SW_PIN);
    sei();
}

void babyStepOut(void) {
    while (pinState)
    {
        stepper->move(1);
        delay(1);
    }
}

After the light barrier is hit, the babyStepOut() function is used, to slowly move out of it again. Even when i just comment out the while (pinState) statement it works fine. 0.29.0 behaves as expected. Very strange!

Any recent changes regarding interrupts and enabling or disabling them? Race conditions?!

gin66 commented 1 year ago

There have been lately some changes in regard to communication between API and the stepper task. Instead of pushing a chunk of data during interrupts disabled on any change, the stepper tasks pulls that chunk of data with interrupts enabled on a regular schedule. This has two advantages: interrupts are disabled less and frequent changes with higher rate than the stepper task's cycle time are collected into one update, which is then pulled once per cycle. Just I cannot see, how this relates to your issue.

Anyway, could you please try for the baby step instead:

void babyStepOut(void) {
    while (pinState)
    {
        stepper->move(1, true);
    }
}

BTW: which uC do you use ? From the sei() I guess it is an avr-derivate.

microtronics commented 1 year ago

Arduino Uno, currently.

Using the blocking move in babyStep it works as expected on the master branch. Also, when I increase the delay in babyStep to 4 ms the bug is mostly gone. Performing a single step should be a lot faster though, right?

Measuring the millis() until stepper-> isRunning() becomes false gives values like these: 3 1 5 3 1 5 3 3 1 So this appears to be the cause. However, this also means that the second move(1) triggers the bug and brings up a move that is even further back in history, because the first move(1) did not.

gin66 commented 1 year ago

Good analysis. I have modified the test (examples/Issue172) to issue two move() directly after each other and it fails. So I have a test case to working on debugging

gin66 commented 1 year ago

New fix has passed all pc_based and simavr_based tests. The rework has been more intensive than expected. stopMove() sets again a flag, which will be reset in the stepperTask and the targetPosition adjusted as needed for a stop with deceleration. Any following move() will use that adjusted targetPosition. Looks more reasonable to me.

There is still a race condition: If move()/moveTo() is called and then stopMove() is invoked (e.g. from Interrupt) before the stepperTask could process. The move()/moveTo() before that stopMove()will be processed as if the sequence has been stopMove() and then move()/moveTo(). As fixing this would be quite difficult, I leave this as it is.

Please try, if the new version works for you.

microtronics commented 1 year ago

Sorry, busy times here. It works as expected again. I wonder if the remaining bug you describe is of practical relevance. Currently, I cannot think of a real world example that would stop a move after starting it and before the motor actually turned