Detecting Motor Stalls in NBC/NXC

So I stayed up all night again playing with the Mindstorms kit. I re-built my car model with a shorter, sturdier frame and a more precise steering. I kept the brick in the back and the differential transmission.

Since that went well into the wee hours, I had little time left to program the thing before the sunrise (yeah, I had to stretch that Transylvanian connection again).

Anyway, the detection of obstacles and steering around them is quite imperfect, and the vehicle bumps into things more often than I would like. So I have decided to compensate with a hack, and searched the Internet for ways to detect motor stalls. The search turned a couple of solutions, one in RobotC and another one in the graphical NXT-G language.

I could not find any NXC code snippet, but after some experimentation I came up with the code below. It checks whether the rotation count drops under a specified threshold for three consecutive times (to avoid spurious, false positives).

#define OUT_DRIVE       OUT_C
#define STALL_THRESHOLD 6 // may depend on battery charge level?


bool is_stalled()
{
    for (int i = 0; i != 3; ++i)
    {
        ResetRotationCount(OUT_DRIVE);
        Wait(10);

        const int count = MotorRotationCount(OUT_DRIVE);
        NumOut(0, LCD_LINE1, count, true);
        
        if (abs(count) >= STALL_THRESHOLD)
        {
            return false;
        }
    }
    return true;
}

Here is another variation:

bool is_stalled()
{
    ResetRotationCount(OUT_DRIVE);
    
    for (int i = 0; i != 3; ++i)
    {
        Wait(10);

        const int count = MotorRotationCount(OUT_DRIVE);
        NumOut(0, LCD_LINE1, count, true);
        
        if (abs(count) >= stallThreshold * (i + 1))
        {
            return false;
        }
    }
    TextOut(0, LCD_LINE1, "stalled", true);
    return true;
}

I have yet to build a robot with the motors attached directly to the wheels, and I wonder if the false positive that I get are related to the fact that my wheels are powered through a differential transmission.

And the stall threshold depends on the surface the robot runs on, so I added the following code to read it from the user:

void input_stall_threshold()
{
    bool done = false;
    int count = 0;
    
    stallThreshold = INIT_STALL_THRESHOLD;
    until (done)
    {
        Wait(300);
        ReadButtonEx(BTNCENTER, true, done, count);
        
        bool pressed = false;
        int n = 0;
        
        ReadButtonEx(BTNLEFT, true, pressed, n);
        if (pressed)
        {
            if (stallThreshold > 0)
                --stallThreshold;
        }
        pressed = false;
        ReadButtonEx(BTNRIGHT, true, pressed, n);
        if (pressed)
        {
            ++stallThreshold;
        }
        TextOut(0, LCD_LINE1, "stall thresh=", true);
        NumOut(78, LCD_LINE1, stallThreshold);
    }
}