Friday, November 09, 2007

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);
}
}

1 comment:

ssk said...

That's cool, but it might be better if you use a timer like this:

bool is_stalled(void)
{
t0 = CurrentTick(void);
ResetTachoCount(OUT_A);
while(t0 + 1000 < CurrentTick(void))
;
int count = MotorTachoCount(OUT_A);
NumOut(0, LCD_LINE4, count, true);

if (abs(count) >= stallThresHold)
{
return false;
}

return true;
}