M a r k   C r o s b i e

You are in: BrickOS Motor Control Library  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 Motor Library

Precise control of motors is the key to making a robot effective in its environment. However, the LEGO motors do not have any feedback mechanism to indicate where they are positioned. Constant speed control and distance measurement is tricky.

I solved some of these problems in other code I wrote, so I decided to pull together some of the techniques I designed in a library which could provide control and measurement of many motors.

Key features I wanted from this library:
  • Support for multiple motors.
  • A layered design, with the lowest layer providing direct motor control, and each higher layer building upon the lower layers.
  • Wheel speed measurement.
  • Wheel RPM measurement.
  • Stall detection with a callback function for your code to react to a wheel stall.
  • Distance computation given the wheel circumference.
  • Constant velocity control of a motor using a feedback loop.
  • A high-level API for direction and turning of a motor.


Rexamining this problem from a different point of view. We should view the different tasks of the control problem as layers in the solutions. In addition, the code should have an idea of how fast it will drive. This means that the wheel diameter, or circumference, must be known. In addition, the wheel-motor rotational ratio must be known.

Once the rotation sensor pulses have been converted into velocity readings, the feedback loop proposed by Jones and Flynn in "Mobile Robots" section 7.8.2 can be applied.

At the lowest layer, the drive layer, the motor is set to drive at a certain speed. The speed is specified as mm/s, and a speed and direction are specified for each motor. In addition, each motor is checked for stall conditions, and a stall flag is updated.

The next layer measures the actual rotational speed for each motor, and computes the current speed in mm/s and direction. It compares the current readings to the desired settings, and adjusts the motor speed.

The highest layer encodes basic driving and turning primitives:
set_speed(speed, dir)Set speed and direction
drive_forward(dist)Drive forward by specified distancen
drive_backward(dist)Drive backwards by specified distancen
turn_left(degree)Turn left by degree amountn
turn_right(degree)Turn right by degree amountn
rotate(degree)Rotate in place by degree amountn

Drive Layer Design

We define two key constants for the large Techic wheels supplied with the Mindstorms set:

WHEEL_CIRC256mmCircumference of the large wheel
MOTOR_WHEEL_RATIO8Ratio of motor/wheel turns


Each time the rotation sensor gives a pulse that indicates that the wheel has rotated by 16mm. 16 pulses x 16mm = 256mm total rotation.

Given these definitions, we can specify speed as a 16 bit integer which gives mm per second (mm/s). Direction can be specified as a boolean flag for forward or backwards.

The settings for each motor are stored in a struct as follows:

typedef enum direction {forward, backward, brake};

typedef struct motor {
	unsigned int desired_speed;	/* in mm/s */
	unsigned int current_speed;	/* in mm/s */
	unsigned int wheel_circ;	/* circumference of wheel mm */
	unsigned int ratio;		/* motor/wheel ratio */
	direction dir;			/* motor direction */
	int feedback;			/* feedback proportion */
	int stalled;			/* is this motor stalled ? */
	(void)(*stallHandler)(int motor); /* stall handler callback */
} motor_t;

motor_t motors[3];

#define A_MOTOR motors[0];
#define B_MOTOR motors[1];
#define C_MOTOR motors[2];


We can initialize each motor's settings by calling
int motor_init(int motorNum, unsigned int wheel_circ, unsigned int
	ratio, (void)(*stallHandler)(int));


Each motor's speed, circumference and ratio settings are initialized to 0. Direction is set to brake. The feedback setting is not changed unless the caller needs to fine tune a motor's response. The stallHandler function pointer points to a callback function to execute if the motor stalls.

Each motor's stall state is updated by running my stall algorithm. Each motor's current rotation sensor count is checked against it's speed setting. If no pulses are found then the stall flag is set. If the desired_speed entry for the motor is 0, then the motor is not expected to run and we can skip checking it for a stall condition.

/* 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


The stall detection thread runs continually for each motor. It takes a sample every STALL_INTERVAL seconds. If the rotation sensor for that motor has not changed since the last measurement, and this occurs STALL_SAMPLES times, the detect_stall thread sets the stall flag in the motor structure, and if it is not NULL, it calls the stallHandler() function. It passes it the id of the motor which has stalled (0, 1 or 2). The stall handler function operates in the context of the stall detection thread.

Feedback Layer Design

The feedback layer is composed of two halves: speed measurement and feedback computation. The speed measurement layer must compute the current speed of each motor being controlled.

Speed Measurement:
For each motor loop
	Reset the rotation counter
	Sleep for SAMPLE_INTERVAL milliseconds
	Measure the number of rotation sensor pulses.
	Distance = pulses * (WHEEL_CIRC / 16) * (1 / SAMPLE_INTERVAL)
end loop


The distance calculation multiplies the distance travelled for one pulse by the number of pulses counted. It then multiplies by the number of sampling intervals in one second to convert to mm/s.

The feedback layer is responsible for computing the deviation from the desired speed setting for each motor. Given the speed measurement computed by the speed measurement thread, it applies a proportional amount of feedback to adjust the motor's speed setting and get the motor closer to the desired speed.

If the lower-level stall detection thread has determined that the motor has stalled then the speed measurement thread will not compute the speed. The feedback loop will also skip the motor until corrective action is taken to reverse the stall condition. Otherwise we would continue to increase power to the motor, but get nowhere...

I'm going to leverage the work done on the Handyboard by Jones and Flynn pg 257 (2nd edition) for this algorithm. Figure 7.37 in their book shows exactly the feedback control loop I'll use here. As we now have velocity measurements, rather than rotation sensor pulses, to work with, their methodology will work.

Distance Measurement

The other nice thing about all of this is that we can now build a distance measurement thread that can update the distance traveled by each wheel.

Discovered that BrickOS 0.2.5 contains code in the kernel to measure rotation sensor velocities. VELOCITY_X returns the number of rotation "ticks" per second for a particular sensor. Can I use this code in my library? Probably yes, but then the code would only work on 0.2.5 and forward. So I think I will stick with the code I have and then investigate the VELOCITY_X change.

I realized that there is no definition in BrickOS for a motor, only explicit function calls e.g. motor_A_dir(). So I can't pass a definition for a motor into initMotor, instead, I'll just map motor output A to array slot 0 etc. So now you can use A_MOTOR as an index into the get/set functions.

Recoded all functions to use motor_t * and had to change initMotor too.

Debugged execi issue: args[] array must be global so that the thread can see its arguments! stallThread now executes correctly. Added VAL macro to debug variable values.

Doh! ds_active() takes SENSOR_X *not* ROTATION_X! Add an extra argument to initMotor.

The distSpeedThread needs signed ints not unsigned. Fixed this and now I get a reasonable display of the current motor rpm. Compared to tacho.c and it looks good. So now we can call this version 0.2!

You can download the C source file and the header file

/* 
 * layered.c
 *
 * Control motors using proportional feedback with stall detection
 *
 * Version:
 *   0.1: initial version for 0.2.4 BrickOS kernel
 *
 * Assumes:
 *   Not much; motor definitions are held in an array  so you
 *   can set them up at run time.
 *
 * External Interfaces:
 *    motor_init
 *    detectStallThread
 *    measureSpeedThread
 *    
 * Internal Interfaces:
 *    initModule
 *
 * Description:
 *   This first version contains the speed measurement code and the
 *   stall detection thread. Most of the base variable and structure
 *   definitions are in place.
 *
 *   A sample test main loop is in place to exercise the code
 *
 * TO DO:
 *   Seperate the layered motor driver code into a library module,
 *   split up the header file to expose the interfaces and keep the
 *   module internals hidden. Modify the Makefile to link the main
 *   module with the library module.
 *
 * Mark Crosbie  1/23/2001  mark@mastincrosbie.com
 */

#include 
#include 

#include 
#include 
#include 
#include 

#include "layered.h"

#define DEBUG

#ifdef DEBUG
static int progress = 0;   /* for debugging */
#define MAKEPROGRESS lcd_int(progress++);
#else
#define MAKEPROGRESS
#endif

/*********************************************************************
 * Contruction specific definitions
 ********************************************************************/
#define WHEEL_CIRC	256   /* mm Circumference of the wheel */
#define MOTOR_WHEEL_RATIO 8   /* Ratio of sensor/wheel turns 
			       * Rotation sensor turns 8 times for
			       * every one wheel revolution */

/*********************************************************************
 * static global variables for this module
 ********************************************************************/
motor_t motors[MAXMOTOR];  /* all the motors we can control */
int errno;  /* your friend errno; what was the last error value? */
char *args[2]; /* for passing to threads */


/*********************************************************************
 * local thread declarations (if any)
 ********************************************************************/


/********************************************************************* * local function declarations for this module (if any)
 ********************************************************************/
static void initModule(void);    /* basic module initialization */
void handleStall(motor_t *m);

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

  MAKEPROGRESS;

  initModule();

  MAKEPROGRESS;

  /* define a motor */
  initMotor(&A_MOTOR,       /* motor output A */
	    256,            /* circumference is 256 mm */
	    8,              /* ratio of motor/wheel turns is 8 */
	    &ROTATION_3,    /* rotation sensor is on input 3 */
	    &SENSOR_3,      /* for initialization */
	    handleStall     /* stall function */
	    );           

  MAKEPROGRESS;

  motor_a_speed(MAX_SPEED);
  motor_a_dir(fwd);

  setDist(&A_MOTOR, 0L);

  while(1) {
#if 1
    lcd_int(getDist(&A_MOTOR));
#endif

    msleep(100);
  }
   
}

void handleStall(motor_t *m) {
  m->stalled = 0; /* reset stalled flag */
  return;
}

/*
 * panic
 *
 * Input Parameters:
 *    None
 *
 * Output Parameters:
 *    None
 *
 * Globals:
 *    errno
 *
 * Algorithm: 
 *    Halt in an infinite loop displaying errno and progress values
 */
void panic(void) {

  while(1) {
    lcd_int(errno);
    sleep(1);
#ifdef DEBUG
    lcd_int(progress);
    sleep(1);
#endif
  }
}

/*
 * initModule
 *
 * Input Parameters:
 *    None
 *
 * Output Parameters:
 *    None
 *
 * Globals:
 *
 * Algorithm: 
 *
 */
static void initModule(void) {

  errno = 0;

  /* initialize the sensors */
  ds_init(); 

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

/* 
 * Name:
 *    initMotor
 *
 * Description:
 *    Initialize a motor's entry in the motor definition array
 *
 * Parameters:
 *    motor (in):      ptr to motor entry in motors[] array
 *    wheel_circ (in): circumference of wheel in mm
 *    ratio (in):      ratio of motor/wheel turns
 *    rotSensorPort (in): what port the rotation sensor for this motor
 *                         is connected to
 *    rotSensor (in):  to initialize rotational sensor processing
 *    stallHandler (in): function to call if motor stalls
 *                       may be NULL
 *
 * Returns:
 *    0 if all values valid and changes are made
 *    -1 an error occurred. errno is set
 *
 * Algorithm:
 *    Each motor's speed, circumference and ratio settings are initialized
 *    to 0. Direction is set to brake. The feedback setting is not changed
 *    unless the caller needs to fine tune a motor's response. The
 *    stallHandler function pointer points to a callback function to execute
 *    if the motor stalls.
 * 
 *    Start a thread to measure the distance and speed of the motor
 *    given its rotation sensor, and a thread to detect stalls on the
 *    motor.
 *
 *    The mapping of motors to motors[] array entries is simple: motor
 *    output A is in entry 0, B in 1 and C in 2
 *    The constants A_MOTOR, B_MOTOR and C_MOTOR are pre-defined in
 *    the layered.h file to simplify calling this function
 *
 * Example:
 *    initMotor(&A_MOTOR,       motor on output A
 *              256,            circumference is 256 mm
 *              8,              ratio of motor/wheel turns is 8
 *              &ROTATION_1,    rotation sensor is on input 1
 *              &SENSOR_1,      must agree with prev argument
 *              foo             call foo if the motor stalls
 *             );           
 */
int initMotor(motor_t *motor, 
	      unsigned int wheel_circ, 
	      unsigned int ratio, 
	      volatile int *rotSensorPort,
              volatile int *rotSensor,
	      void (*stallHandler)(struct motor *motor)) {


  errno = 0;     /* no errors yet */

  motor->distance = 0;
  motor->desired_speed = 0;
  motor->current_speed = 0;
  motor->ratio = ratio;
  motor->wheel_circ = wheel_circ;
  motor->dir = off;
  motor->feedback = 10;
  motor->stalled = 0;
  motor->rotSensorPort = rotSensorPort;
  motor->stallHandler = stallHandler;

  /* initialize the mutex on the structure */
  sem_init(&(motor->mutex), 0, 1);

  /* initialize the rotation sensor for this motor */
  ds_init(); 
  ds_active(rotSensor);

  /* ok, start sensor processing */
  ds_rotation_set(rotSensor, 0);
  ds_rotation_on(rotSensor);

  /* put the motor ptr into argv[0] for the thread */
  args[0] = (char *)motor;
  args[1] = (char *)NULL;

  /* start an instance of the distance thread for this motor */
  motor->distancePid = execi(distSpeedThread, 2,
				    args, 
				    PRIO_NORMAL,DEFAULT_STACK_SIZE); 

  /* start an instance of the stall detection thread for this motor */
  motor->stallPid = execi(stallThread, 2,
				    args, 
				    PRIO_NORMAL,DEFAULT_STACK_SIZE); 

  return 0;
}


/* 
 * Name:
 *    stallThread
 *
 * Description:
 *    Monitor the rotation sensor and detect if it has stopped
 *    changing. If it has, set the flag in the motor's array entry and
 *    call the stall handler function if defined.
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) ptr to array entry of wheel to monitor for a stall
 *
 * Returns:
 *    Does not return - runs until killed
 */
int stallThread(int argc, char **argv) {

  int value;
  int sampleCount;
  int sensorReading;
  motor_t *motor;
  volatile int *sensor;

  motor = (motor_t *)(argv[0]);
  sensor = motor->rotSensorPort; /* take an initial reading */
  value = *sensor;
  sampleCount = 0;      /* no samples have been taken so far */

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

    sensorReading = *sensor;   /* take a reading */

    msleep(STALL_INTERVAL);

    /* if the motor is currently disabled then don't bother to process
     * stall data
     */
    if( (motor->dir == off) || (motor->desired_speed == 0) )
      continue;

    if(value != sensorReading) {
      /* change is good. It means we're moving, so update value and
       * reset sampleCount
       */
      value = sensorReading;
      sampleCount = 0;
    } else {
      /* ok, time to take samples. If this is the last sample of the
       * set, then we call it a stall
       */
      if(sampleCount == STALL_SAMPLES) {
	cputs("stall");
	value = sensorReading;
	sampleCount = 0;
	motor->stalled = 1;
	/* call the stall handler */
	motor->stallHandler(motor);
      } else {
	/* update the sampleCount */
	sampleCount++;
      }
    }
  }
}


/* 
 * Name:
 *    distSpeedThread
 *
 * Description:
 *    Given the circumference of a wheel and the ratio of rotation
 *    sensor turns to wheel turns, compute the distance travelled by a
 *    wheel, and the speed at which it is turning.
 *
 * Parameters:
 *    argc: (in) ignored
 *    argv: (in) argv[0] contains ptr to motor to measure
 *
 * Returns:
 *    Does not return - runs until killed
 *
 * Algorithm:
 *    Within each SPEED_INTERVAL milliseconds, count the number of
 *    rotation sensor pulses and compute the distance traveled.
 *
 *    Computational efficiency is gained by using local variables so
 *    that the motors[] array is not continually defreferenced.
 *    
 *    NOTE: distance is *not* vectored so travelling forwards by x 
 *    distance and then reverse by x results in 2x distance, not 0 
 *    distance
 *
 *    LIMITATIONS: 
 *      Will underflow if wheel circumference < 2mm
 *      If rate of rotation is less than 1/16th of a revolution every
 *      SPEED_INTERVAL milliseconds then zero is computed. Adjust
 *      SPEED_INTERVAL to a lower value if this is a problem.
 */
int distSpeedThread(int argc, char **argv) {

  motor_t *motor;
  int val1, val2, speed, rpm, circumference;
  unsigned int x, avgspeed, avgrpm;
  unsigned long avgdist;
  volatile int *sensor;

  /* pre-compute some values */
  motor = (motor_t *)argv[0];
  circumference = motor->wheel_circ;
  sensor = motors->rotSensorPort;
  avgdist = avgspeed = avgrpm = 0;

  while(1) {

    /* take a sensor measurement */
    val1 = *sensor;
    msleep(SPEED_INTERVAL);
    val2 = *sensor;

    /* 
     * each unit of difference represents 1/16 th of a revolution if
     * we get 16 units of absolute difference, that equals one
     * revolution. Multiply by 1000/SPEED_INTERVAL to scale to seconds
     * 
     * Fixed point math is used to avoid division underflow. 
     * The RPM calculation is scaled by 60 in the numerator so that
     * division by 16 does not lead to an underflow error.
     *
     */
    x = ABS(val2 - val1); /* take absolute difference */

    if(x) { /* avoid division by zero! */
      speed = ((x * circumference) * (1000/SPEED_INTERVAL))/16;
      rpm = (x * 60 * (1000/SPEED_INTERVAL))/16; /* revs per minute */
    } else {
      rpm = speed = 0;
    }

    /* compute averages over two readings for smoother results */
    avgspeed = (avgspeed + speed)/2;
    avgrpm = (avgrpm + rpm) / 2;

    /* distance = speed * time */
    avgdist = avgdist + ((x * circumference)/16);

    /* update the motor's entry and only acquire the lock once */
    sem_wait(&(motor->mutex));  /* get the mutex */
    motor->current_rpm = avgrpm;
    motor->current_speed = avgspeed;
    motor->distance = avgdist;
    sem_post(&(motor->mutex));  /* release the mutex */
  }
}

/* 
 * Name:
 *    getRPM
 *
 * Description:
 *    Return current rotational speed of a motor in RPM
 *
 * Parameters:
 *    motor (in): ptr to motor to measure
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Current measured rotational speed
 *
 * Algorithm:
 *    Return the current value for this motor. No need to acquire mutex.
 */
unsigned int getRPM(motor_t *motor) {

  return(motor->current_rpm);
}

/* 
 * Name:
 *    getSpeed
 *
 * Description:
 *    Return current rotational velocity of the motor in mm/s
 *
 * Parameters:
 *    motor (in): ptr to motor to measure
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Current measured speed value
 *
 * Algorithm:
 *    Return the current value for this motor. No need to acquire mutex.
 */
unsigned int getSpeed(motor_t *motor) {

  return(motor->current_speed);
}

/* 
 * Name:
 *    getDist
 *
 * Description:
 *    Return current measured distance traveled by the wheel
 *
 * Parameters:
 *    motor (in): ptr to motor to measure
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Current measured distance value
 *
 * Algorithm:
 *    Return the current value for this motor.  No need to acquire mutex.
 */
unsigned long getDist(motor_t *motor) {

  return(motor->distance);
}

/* 
 * Name:
 *    setDist
 *
 * Description:
 *    Set the distance measurement for a wheel to a fixed value 
 *
 * Parameters:
 *    motor (in): ptr to motor to set
 *    value (in): value to set current distance to
 *
 * Globals:
 *    motors
 *
 * Returns:
 *    Nothing.
 *
 * Algorithm:
 *    Sets the current distance measurement. Acquires mutex on array entry
 */
void setDist(motor_t *motor, long value) {

  sem_wait(&(motor->mutex));  /* get the mutex */
  motor->distance = value;
  sem_post(&(motor->mutex));  /* release the mutex */

}



© 2002-2004 Mark Crosbie   shareright © 2002 Phlash