M a r k   C r o s b i e

You are in: BrickOS Stall detector  Home  Resume  Linux  LEGO  Electronics  Movies  Mac OS X 


Details
BrickOS Overview Stall detector Tachometer Motor control BrickOS Links Someday soon
Contact Mark at mark@mastincrosbie.com

BrickOS Stall Detector

Sample Stall detector I decided to use my new rotation sensor code to try a different approach to collision detection for robots: stall detection. One thread samples the output of a rotation sensor periodically. If it gathers enough samples that shows no movement on the rotation sensor it decides that a stall has occurred.

I've gone a step further from the previous two projects and implemented a thread based solution. The rotation sensor is checked periodically by the detect_stall() thread. The main() function starts the thread executing using the execi() BrickOS call. You can initialize as many threads as you wish using execi() and they will all start running when the tm_start() kernel call starts the task scheduler.

The detect_stall() function loops forever and monitors the rotation sensor value every STALL_INTERVAL microseconds. We must observe STALL_SAMPLES iterations through the loop with no change in the rotation sensor reading before we decide that a stall has occurred. If the value has changed since the last sample point then we reset the sampleCount counter to 0 to indicate that we are not stalled.

If the sampleCount counter reaches STALL_SAMPLES then we make a beep sound and call stop_and_reverse() to reverse the motor direction.

HINTS
  • The sensitivity of the stall detector can be tuned by changing the value of STALL_SAMPLES. A greater value will decrease the sensitivity to stalls.
  • It is best to include a rubber-band drive step into a robot using this code. If the drive train from the motor to the wheels is all gear driven then the gears will be stressed when the robot runs into an obstacle. The rubber bands provide some slippage in the drive mechanism to allow the stall sensor to trigger.


Click here to download the source code.

/* 
 * stall.c
 *
 * Detect a robot stalling when it hits an object
 * Constantly monitors the rotation sensor as a background thread and
 * if it detects a stall then beep and stop and reverse the motors
 *
 * Assumes: rotation sensor on sensor port 1
 *          motor output on port A
 *
 * Mark Crosbie  4/8/00  mark@mastincrosbie.com
 */


#include 
#include 

#include 
#include 
#include 
#include 

#define ROTSENSOR SENSOR_1
#define DRIVEMOTOR MOTOR_A
#define FORW 0
#define REV 1

/* how many samples should we take before calling a stall? */
#define STALL_SAMPLES 3   

/* time interval between each sample point in ms */
#define STALL_INTERVAL 100

int detect_stall(int argc, char **argv);
void stop_and_reverse(void);

int dir;  /* motor direction */

int main(int argc, char *argv[]) {

  int stallpid;

  /* we start out going forward */
  dir = FORW;

  /* initialize the sensor */
  ds_init(); 
  ds_active(&ROTSENSOR);

  /* set the sensor's count to 0 */
  ds_rotation_set(&ROTSENSOR, 0);

  /* ok, start sensor processing */
  ds_rotation_on(&ROTSENSOR);

  /* declare the thread to detect a stall */
  stallpid = execi(detect_stall, 0, (char **)NULL, PRIO_NORMAL,
		   DEFAULT_STACK_SIZE);

  /* start task manager processing */
  tm_start();

  motor_a_speed(MAX_SPEED);
  motor_a_dir(fwd);
  cputs("fwwd ");

  while(1) {

  }
}

/* 
 * Name:
 *    detect_stall
 *
 * Description:
 *    Monitor the rotation sensor and detect if it has stopped
 *    changing. If it has, stop the motors and reverse direction
 *
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) ignored
 *
 * Returns:
 *    0 if no change in value
 *    1 if value of ROTSENSOR has changed since last poll
 */
int detect_stall(int argc, char **argv) {

  int value;
  int sampleCount;

  value = ROTATION_1;   /* take an initial reading */

  sampleCount = 0;      /* no samples have been taken */

  /* we live forever, or at least until the main loop kills us :-) */
  while(1) {

    msleep(STALL_INTERVAL);

    if(value != ROTATION_1) {
      /* change is good. It means we're moving, so update value and
       * reset sampleCount
       */
      value = ROTATION_1;
      sampleCount = 0;
    } else {
      /* ok, time to take samples. If this is the last sample of the
       * set, then we call it a stall and reverse out of here
       */
      if(sampleCount == STALL_SAMPLES) {
	/* beep so we know it  */
	dsound_system(DSOUND_BEEP);
	stop_and_reverse();
	value = ROTATION_1;
	sampleCount = 0;
      } else {
	/* update the sampleCount */
	sampleCount++;
      }
    }
  }
}

/* 
 * Name:
 *    stop_and_reverse
 *
 * Description:
 *    Stop the motor and reverse direction
 *
 * Parameters:
 *
 * Returns:

 */
void stop_and_reverse(void) {

  motor_a_dir(brake);

  /* toggle direction */
  if(dir == FORW) {
    dir = REV;
    motor_a_dir(rev);
    cputs("rev ");
  } else {
    dir = FORW;
    motor_a_dir(fwd);
    cputs("fwwd ");

  }

  motor_a_speed(MAX_SPEED);
}




© 2002-2004 Mark Crosbie   shareright © 2002 Phlash