Servo motors, Linux/TRAI and other fun stuff LG #96

...making Linux just a little more fun!

Servo motors, Linux/TRAI and other fun stuff
By Pramode C.E

Servo motors, Linux/RTAI and other fun stuff

Introduction

In last month's article, I had explored the problems associated with writing timing sensitive code under Linux and looked at how RTAI solves them in an elegant manner. In this issue, I present an interesting application developed by a few of my students as part of a Computer Vision project. I would also like to share with you a few programs which I wrote in my effort to understand RTAI better:

A simple program which measures pulse width of a PWM signal
A frequency counting program which uses interrupts under RTAI
A program which tries to demonstrate the effect of priority inversion in multithreaded code and the use of resource semaphores under RTAI (The reader is expected to have an understanding of basic semaphore operations to follow this part).

The Project

In his article Creating a web page controlled 2-axis movable camera with RTLlinux/Pro, Cort Dougan presents the software and hardware involved in the construction of an interesting device which allows him to always keep an eye on his pet cat, Kepler. An ordinary web cam is controlled by two servo motors - one makes the cam move up and down and the other makes it sweep 180 degrees. The movement of both the servos is controlled by real time tasks.

Motivated by the article, a few of my students tried their hand at designing such a system. Here is what they came up with:

The top and bottom servo's should be clearly visible - the one at the bottom serves to rotate the platform resting on its axis (the platform on which the webcam is mounted). The whole thing is made out of transparent plastic. Here is a closer view of the bottom part:

Controlling the servo

A hobby servo motor(like the Futaba S2003 used in this project) runs off 5V DC, has high torque for its size and can be positioned at points along a 180 degree arc (or a little bit more - the servo doesn't rotate the full 360 degrees, there is some kind of mechanical `stop' built into it - which can of course be removed if you are seriously into servo hacking). The control wire (normally white colour) of the servo should be continuously fed with a digital signal whose period is about 20ms - the period need not be very accurate, and can be lesser than 20ms, but the on time should be precisely controlled, it is what decides where the servo will move to - in this case, it was seen that the servo moves for about 170 degrees for an on time from 2.2ms down to 0.5ms. Here is a picture of the control pulse:

Generating this control pulse with `normal' Linux is a difficult issue - other activities going on in the system can seriously disturb the waveform - with the result that the servo will start reacting violently. So, hard real time RTAI tasks are used.

Two real time tasks are used to control the servos - one controlling the top servo and the other one, the bottom servo. The servo position is communicated to the real time tasks from user space with the help of two real time fifos. Let's first look at some macro/variable definitions (the actual rotation angles have not been accurately measured - so users of the Futaba S2003 need not be worried about any numerical discrepancy):


#define BOTTOM_SERVO_FIFO 0
#define TOP_SERVO_FIFO 1

#define FIFO_SIZE 1024

#define BOTTOM_SERVO 0
#define TOP_SERVO 1

#define BOTTOM_SERVO_PIN 2 /* Bit D1 of parport o/p register */
#define TOP_SERVO_PIN 4 /* Bit D2 of parport o/p register */

#define TICK_PERIOD 1000000 /* 1 ms */
#define STACK_SIZE 4096

#define MIN_ON_PERIOD 500000 /* .5 ms */
#define NSTEPS 35
#define STEP_PERIOD 50000 /* 50 micro seconds */

#define TOTAL_PERIOD 20000000 /* 20ms */

#define ONE_SHOT

RTIME on_time[2], off_time[2];
RT_TASK my_task1, my_task2;

The bottom servo is connected to pin 3 of the parallel port and the other one, to pin 4 - the macro's BOTTOM_SERVO_PIN and TOP_SERVO_PIN define the values to be written to the parallel port data register to set these pins (bit D0 of parallel port data register controls pin number 2, bit D1 pin number 3 and so on - the macro's are definitely poorly named and meant to confuse readers). The minimum on period of the control pulse is defined to be 500000 nano seconds (.5 ms). Because we get a servo rotation of 170 degree for pulse widths from 0.5ms to 2.2ms, a 50 micro second change in pulse width gives us 5 degree of rotation. We define the `zeroth step' of the servo as the position to which it moves when it sees a pulse of 0.5ms duration and the `thirty-fourth step' as the position it moves to when it sees a pulse whose on time is 0.5ms + (50 micro second * 34), ie, 2.2ms. So, in a total of 35 steps, each step about 5 degree, we get full 170 degree rotation. What the user program communicates with the real time tasks through the fifos is this step number.

The total period is defined to be 20ms. The zeroth element of the on_time array stores the current on time of the pulse which controls the bottom servo and the first element, that of the top servo. Similar is the case with the off_time array.

Let's now look at the code for the tasks which control the servo's:


static void servo_task(int pin)
{
	while(1) {
		outb(inb(0x378) | pin, 0x378);
		rt_sleep(on_time[(pin >> 1) - 1]);
		outb(inb(0x378) & ~pin, 0x378);
		rt_sleep(off_time[(pin >> 1) - 1]);
	}
}

The argument to the task is BOTTOM_SERVO_PIN or TOP_SERVO_PIN, depending on which servo it controls. The servo number can be obtained from these values by shifting them right once and subtracting one. First, the corresponding parallel port pin is made high and a sleep is executed - then, the pin is made low and the task goes to sleep once again.

Let's now look at the fifo handler code - user programs communicate with the real time tasks through two fifo's - one for each servo. The fifo handler code, which gets executed when either of them is written to from a user space program, is the same. User programs communicate a `step number', ie, an integer in the range 0 to 34.


int fifo_handler(unsigned int fifo)
{
	int n, r;
	unsigned int on;
	r = rtf_get(fifo, &n, sizeof(n));
	rt_printk("fifo = %d, r = %u, n = %u\n", fifo, r, n);
	if((n < 0) && (n > 34)) return -1;
	on = MIN_ON_PERIOD + n*STEP_PERIOD;
	on_time[fifo] = nano2count(on);
	off_time[fifo] = nano2count(TOTAL_PERIOD - on);
	return 0;
}

The code should be easy to understand, it simply reads a step number, converts it into a corresponding `on time' and stores it into the proper slot in the on_time array. The argument to the handler is the number of the fifo to which data was written to from the user space program.

We now come to the module initialization part, the code should be easy to understand.


int init_module(void)
{
	RTIME tick_period;
	RTIME now;

	rtf_create(BOTTOM_SERVO_FIFO, FIFO_SIZE);
	rtf_create_handler(BOTTOM_SERVO_FIFO, fifo_handler);
	rtf_create(TOP_SERVO_FIFO, FIFO_SIZE);
	rtf_create_handler(TOP_SERVO_FIFO, fifo_handler);
	
#ifdef ONE_SHOT
	rt_set_oneshot_mode();
#endif
	rt_task_init(&my_task1, servo_task, BOTTOM_SERVO_PIN, STACK_SIZE, 0, 0, 0);
	rt_task_init(&my_task2, servo_task, TOP_SERVO_PIN, STACK_SIZE, 0, 0, 0);
	tick_period = start_rt_timer(nano2count(TICK_PERIOD));
	on_time[BOTTOM_SERVO] = nano2count(MIN_ON_PERIOD);
	on_time[TOP_SERVO] = on_time[BOTTOM_SERVO];
	off_time[BOTTOM_SERVO] = nano2count(TOTAL_PERIOD - MIN_ON_PERIOD);
	off_time[TOP_SERVO] = off_time[BOTTOM_SERVO];
	now = rt_get_time() + tick_period;
	rt_task_make_periodic(&my_task1, now, tick_period);
	rt_task_make_periodic(&my_task2, now, tick_period);
	
	return 0;
}

And here comes the module cleanup:


void cleanup_module(void)
{
	stop_rt_timer();
	rt_busy_sleep(10000000);
	rtf_destroy(BOTTOM_SERVO_FIFO);
	rtf_destroy(TOP_SERVO_FIFO);
	rt_task_delete(&my_task1);
	rt_task_delete(&my_task2);
}

The real time tasks were found to control the servo's perfectly. The system was heavily loaded by running multiple kernel compiles, copying and untarring large files and several other tricks - but the real time tasks were found to perform satisfactorily.

Using Interrupts

It is easy to measure the frequency of a low-frequency square wave. Simply apply it to the parallel port interrupt pin, write an interrupt service routine and increment a count within the routine. This count may be transferred to a user space program through a real time fifo.

A real time application may not tolerate interrupts getting missed or the interrupt handling code getting executed a long time after the interrupt is asserted. With a `normal' Linux kernel, this is a real problem. The Linux kernel may disable interrupts when it executes critical sections of code - during this time, the system remains unresponsive to external events. In an RTAI patched kernel, even if Linux asks for interrupts to be disabled, interrupts don't really get disabled; only thing is RTAI does not let the Linux kernel see the interrupt. A real time task will still be able to handle interrupts undisturbed.

Here is a small program which counts the number of interrupts coming on the parallel port interrupt input pin. I tested it out by using a function generator to generate a square wave at various low frequencies. A simple 555 timer based circuit should also do the job.


#include <linux/module.h>
#include <rtai.h>
#include <rtai_sched.h>
#include <rtai_fifos.h>
#include <asm/io.h>

#define FIFO_R 0
#define FIFO_W 1
#define TIMERTICKS 1000000000 /* 1 second */
#define STACK_SIZE 4096
#define FIFO_SIZE 1024

static int prev_total_count = 0;
static int new_total_count = 0;
static RT_TASK my_task;

static void fun(int t)
{
	while(1) {
		prev_total_count = new_total_count;
		new_total_count = 0;
		rt_task_wait_period();
	}
}

int fifo_handler(unsigned int fifo)
{
	char c;
	rtf_get(FIFO_R, &c, sizeof(c));
	rtf_put(FIFO_W, &prev_total_count, sizeof(prev_total_count));
	return 0;
}
	

static void handler(void)
{
	new_total_count++;
}

int init_module(void)
{
	RTIME tick_period, now;

	rt_set_periodic_mode();
	rt_task_init(&my_task, fun, 0, STACK_SIZE, 0, 0, 0);
	tick_period = start_rt_timer(nano2count(TIMERTICKS));
	now = rt_get_time();
	rt_task_make_periodic(&my_task, now + tick_period, tick_period);

	rtf_create(FIFO_R, FIFO_SIZE);
	rtf_create(FIFO_W, FIFO_SIZE);
	rtf_create_handler(FIFO_R, fifo_handler);
	
	rt_request_global_irq(7, handler);
	rt_enable_irq(7);
	outb(0x10, 0x37a); 
	return 0;
}

void cleanup_module(void)
{
	stop_rt_timer();
	rt_busy_sleep(10000000);
	rt_task_delete(&my_task);
	rt_disable_irq(7);
	rt_free_global_irq(7);
	rtf_destroy(FIFO_R);
	rtf_destroy(FIFO_W);
}

The rt_request_global_irq and rt_enable_irq functions together instruct the RTAI kernel to service IRQ 7 (which is the parallel port interrupt). The interrupt handler simply increments a count. Every 1 second (the frequency of our input doesn't change fast, and we are only interested in observing a long cumulative count) a real time task wakes up and stores this count value to another variable (called prev_total_count) and also clears the counter. User programs can access prev_total_count through a FIFO.

A user program reads the count by writing a dummy value to FIFO_R and reading from FIFO_W. Writing to FIFO_R will result in fifo_handler getting executed, which will place the count onto FIFO_W; it can then be read by the user program. There should surely be a better way to do this - only trouble is, I don't know how. Here is the user program:


/* User space test program */

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <assert.h>

#define FIFO_W "/dev/rtf0"
#define FIFO_R "/dev/rtf1"

main()
{
	int fd1, fd2, dat,r;

	fd1 = open(FIFO_W, O_WRONLY);
	fd2 = open(FIFO_R, O_RDONLY);
	assert(fd1 > 2);
	assert(fd2 > 2);
	
	write(fd1, "a", 1);
	r = read(fd2, &dat, sizeof(dat));
	assert(r == 4);
	printf("interrupt count = %d\n", dat);
}

Measuring Pulse Width

Let's say we wish to measure the off-time of a pulse of total width 3ms with an accuracy of not more than 0.1ms. We start a periodic task with period 0.1 ms. At each `awakening' of this periodic task, it checks whether the signal is low or high. The number of times the signal is low, out of a total of 30 samples, is recorded. Here is the code which implements this procedure:

#include <linux/module.h>
#include <rtai.h>
#include <rtai_sched.h>

#define LPT1_BASE 0x378
#define LPT1_STATUS 0x379
#define ACK 6

#define STACK_SIZE 4096
#define TIMERTICKS 100000 /*  0.1 milli second */
#define TOTAL_SAMPLES 30 /* Take 30 samples at 0.1 ms each */

static RT_TASK my_task;
static int old_off_samples, new_off_samples;

static void fun(int t)
{
	static int count = 0;
	while(1) {
		new_off_samples = new_off_samples + ((inb(LPT1_STATUS) >> ACK) & 0x1);
		if(++count == TOTAL_SAMPLES) {
			count = 0;
			old_off_samples = new_off_samples;
			new_off_samples = 0;
		}
		rt_task_wait_period();
	}
}

int init_module(void)
{
	RTIME tick_period, now;

	rt_set_periodic_mode();
	rt_task_init(&my_task, fun, 0, STACK_SIZE, 0, 0, 0);
	tick_period = start_rt_timer(nano2count(TIMERTICKS));
	now = rt_get_time();
	rt_task_make_periodic(&my_task, now + tick_period, tick_period);
	return 0;
}

void cleanup_module(void)
{
	stop_rt_timer();
	rt_printk("old = %u, new = %u\n", old_off_samples, new_off_samples);
	rt_busy_sleep(10000000);
	rt_task_delete(&my_task);
}

The periodic task checks the D6th bit of the parallel port status register. It is 1 if the input signal on the 10th pin is low. The count could be, as usual, transferred to a user space program through a fifo.

Semaphores and Priority inversion

Programmer's use semaphore's to synchronize the activity of multiple threads. RTAI has a function:

void rt_sem_init(SEM *sem, int value);

Which can be used to create and initialize semaphore objects. There is an interesting problem called `priority inversion' associated with the use of locking mechanisms in an an environment which allows preemptive execution of tasks with varying priorities. It seems that the problem was brought to the attention of the real time design community by certain glitches encountered during the Mars Pathfinder Mission (a Google search would yield more information). I shall try to describe the problem first (the description is based on information obtained from the Net on the Pathfinder mission failure).

Suppose we have a very high priority task, Task-H which periodically accesses a buffer to write data to it (or read data from it). Another task, a very low priority and very infrequently running task, which always runs only for a short amount of time (let's call it Task-L), also might need to access the buffer to write to or read from it. Both these tasks would have to successfully grab a lock before one of them is able to access the buffer; the lock can be acquired by only one task at a time. Let's say Task-L grabs the lock and starts accessing the buffer. In between, suppose an interrupt causes the scheduling of the very high priority task, Task-H. Now, Task-H also would try to grab the lock, but blocks because it is currently held by Task-L. In the normal case, Task-L would finish very soon and release the lock, allowing Task-H to continue. But suppose a medium priority task, Task-M gets scheduled. Now, as long as Task-M does not finish, the OS will not allow Task-L to continue. Only if Task-L finishes doing whatever it has to do and releases the lock will Task-H be able to continue - the result is Task-H gets delayed by the time Task-M would take to complete. Suppose the system designer overlooks this and builds a watchdog timer which would time out and reboot the system if Task-H does not run for a short amount of time - the result would be system reboots whenever Task-M comes in between Task-H and Task-L.

Here is a small program which tries to demonstrate this problem.


#include <linux/module.h>
#include <rtai.h>
#include <rtai_sched.h>

#define LPT1_BASE 0x378
#define STACK_SIZE 4096
#define TIMERTICKS 100000000 /*  .1 second */

#define HIGH_PRIO 0
#define MEDIUM_PRIO 1
#define LOW_PRIO 2

#define PORTB 0x61
#define PIT_CTRL 0x43
#define PIT_DATA 0x42

static RT_TASK my_task1, my_task2, my_task3;
SEM flag;
RTIME tick_period;
unsigned char c = 0;

void speaker_on(void)
{
	unsigned char c;
	c = inb(PORTB)|0x3;
	outb(c, PORTB);
}

void speaker_off(void)
{
	unsigned char c;
	c = inb(PORTB)&~0x3;
	outb(c, PORTB);
}

void generate_tone(void)
{
	/* Counter 2, low and high, mode 3, binary */
	outb(0xb6, PIT_CTRL);
	outb(152, PIT_DATA);
	outb(10, PIT_DATA);
}


static void my_delay(unsigned int i)
{
	while(i--);
}

/* Highest priority */
static void info_bus_task(int t)
{
	speaker_on();
	rt_sem_wait(&flag);
	rt_printk("info bus task got mutex...\n");
	rt_sem_signal(&flag);
	speaker_off();

}

/* Medium Priority */
static void comm_task(int t)
{
	my_delay(0xffffffff);
	my_delay(0xffffffff);
	my_delay(0xffffffff);
}

/* Low priority */
static void weather_task(int t)
{
	rt_sem_wait(&flag);
	rt_sleep(30*tick_period);
	rt_sem_signal(&flag);
}


int init_module(void)
{
	RTIME  now;

	//rt_typed_sem_init(&flag, 1, RES_SEM);
	rt_sem_init(&flag, 1);
	rt_set_periodic_mode();
	generate_tone();
	rt_task_init(&my_task1, info_bus_task, 0, STACK_SIZE, HIGH_PRIO, 0, 0);
	rt_task_init(&my_task2, comm_task, 0, STACK_SIZE, MEDIUM_PRIO, 0, 0);
	rt_task_init(&my_task3, weather_task, 0, STACK_SIZE, LOW_PRIO, 0, 0);

	tick_period = start_rt_timer(nano2count(TIMERTICKS));
	now = rt_get_time();
	rt_task_make_periodic(&my_task1, now + 2*tick_period, tick_period);
	rt_task_make_periodic(&my_task2, now + 3*tick_period, tick_period);
	rt_task_make_periodic(&my_task3, now + tick_period, tick_period);
	return 0;
}

void cleanup_module(void)
{
	stop_rt_timer();
	rt_busy_sleep(10000000);
	rt_sem_delete(&flag);

	rt_task_delete(&my_task1);
	rt_task_delete(&my_task2);
	rt_task_delete(&my_task3);
}

The information bus, communication and weather tasks are respectively the high, medium and low priority tasks. RTAI lets us set priorities to tasks during rt_task_init. The weather task starts first. It grabs a semaphore (the semaphore is initialized to 1 in rt_sem_init) and then goes to sleep for 30 ticks (3 seconds, as each tick is 0.1 second). The information bus task runs at the next timer tick (the second argument to rt_task_make_periodic is the point of time at which the task is to be started); it turns on the PC speaker and then attempts to do a `down' operation on the semaphore and in the process, gets blocked. We expect the weather task to complete its sleep, release the semaphore and let the information bus task run to completion, thereby stopping the noise coming out of the speaker. But the trouble is that before the weather task comes out of its sleep, a medium priority `communication' task gets scheduled and starts executing a series of busy loops (which on my Athlon XP system generates a combined delay of about 17 seconds when compiled with -O2 - it would be better if you time the delay loop in a userland program before you plug it into the kernel - remember, as long as that delay loop is running, your machine will be in an unusable state - so be careful with what you do). The operating system can't bring the weather taks out of its sleep even after 3 seconds is over because the medium priority task is busy executing a loop - after about 17 seconds, the communication task would end, thereby letting the weather task continue with its execution. The weather task perform an `up' operation on the semaphore bringing the information bus task out of the block and letting it stop the speaker. We note that the high priority information bus task is getting delayed by the communication task.

RTAI supports `resource semaphores' - they can be used to solve the above problem. Had we initialized our semaphore like this:

rt_typed_sem_init(&flag, 1, RES_SEM);

the task which originally `acquired' the semaphore (the weather task) would have `inherited' the priority of the high priority information bus task blocked on it. This would have resulted in the RTAI scheduler preempting the communication task and giving control back to the weather task exactly after 3 seconds of sleep.

Acknowledgements

The webcam-servo motor setup was implemented by Krishna Prasad, Mahesh and friends as part of a Computer Vision project; they wish to acknowledge the influence of Cort Dougan's implementation of the idea on RTLinux. RTAI comes with good documentation, and lots of example code. I would like to thank all those people who took the pains not only to build a great system, but also document it well.

About the Author

I have been teaching GNU/Linux and elementary Computer Science since 1997. If you are sure that you really wish to waste your time, you might drop in on my home page pramode2.tripod.com

[BIO] I am an instructor working for IC Software in Kerala, India. I would have loved becoming an organic chemist, but I do the second best thing possible, which is play with Linux and teach programming!