Code for the Trippy pan-tilt-zoom webcam

The code presented here is written in a C-like notation, while the original code running on the Trippy was written in PIC assembly language. This code is not in any way complete, but rather, it is intended to document the core ideas of the code that ran on the Trippy.

The original code worked well, but note that it was highly optimized, so that the absolute minimum number of machine instructions were involved. As presented here, there are extra if statements that were eliminated in the production code, and there are named temporary variables dedicated to specific purposes where the original code used variables that had other purposes elsewhere.

Also note that this outline does not contain the actual application code that was used on the Trippy to implement the protocol for communicating with a remote computer to set and test the variables that set the goal for pan and tilt moves.

This outline uses physical motor steps to specify both X (tilt) and Y (pan) coordinates with no scale factors.

Global Variables

// table of control vectors for the dual H-bridge controlling each motor
const steptab[] = { 0x8, 0xA, 0x2, 0x6, 0x4, 0x5, 0x1, 0x9 };

Constant tables on the PIC are very strange things because of the Harvard architecture of the processor. They are implemented with a function call that does an indexed jump into an array of return literal instructions.

1000 1010 0010 0110 0100 0101 0001 1001

From one entry to the next, only one bit changes. This 8-step per cycle pattern gives half-step control of the motor.

// Public variables used in motor control (see documentation of the BT protocol)
uint8_t A;     // tilt and pan sensor inputs
uint8_t Ex,Ey; // current tilt and pan velocity 
uint8_t Fx,Fy; // current number of 1/500 sec ticks til next tilt or pan step
int16_t G;     // number and direction of pan steps remaining in current move
int16_t H;     // number and direction of tilt steps remaining in current move
int16_t I;     // current pan coordinate 
int16_t J;     // current tilt coordinate
uint8_t V;     // limits maximum velocity, sets number of atable[] entries
int16_t X;     // goal pan coordinate 
int16_t Y;     // goal tilt coordinate

uint8_t atable[]; // acceleration table (stored after Z)
                  //   note atable[V] is ticks per step at maximum velocity.

In the complete version of the code, the 16-bit variables A through Z plus atable[] were stored consecutively and were addressible through the BT protocol as a single array. Here we only defined the variables used for motion and and for E and F, where the high and low bytes of the variable are used separately, they were named separately with subscripts related to the use of the fields.

// Private variables used in motor control
uint8_t M1dex; // motor 1 (pan) index, values are 0 to 7
uint8_t M2dex; // motor 2 (tilt) index, values are 0 to 7
uint8_t M1val; // motor 1 (pan) control vector (steptab[M1dex])
uint8_t M2val; // motor 2 (tilt) control vector (steptab[M2dex])

Interrupt Service Routines

There are two interrupt service routines involved in motor control. These send two 4-bit control vectors packed into 8-bits out(vector,MOTORS). Each interrupt service routine is triggered by one of the programmable real-time clocks on the microcontroller. The entire control system was based on a 5kHz clock, so the basic unit of time measurement is the 0.2ms clock tick.

The first interrupt service routine is very simple. It is used only to reduce the motor power used to provide holding torque when the camera is holding steady. Power is reduced by using pulse-width modulation, or PWM, with primary interrupt service routine turning on the motor power every 0.2ms and (when PWM is being used) setting a secondary aperiodic (that is, one-time) real-time clock to an appropriate fraction of 0.2ms. The interrupt service routine for that clock simply turns off the power for the remainder of 0.2ms tick.

PWMinterrupt() { // PWM interrupt service routine
    out( 0, MOTORS ); // turn off motor power for remainder of RTC tick
    resetPWMtimer();  // turn off PWM timer unti next setPWMtimer() call
}

The main motion control interrupt service routine was entered 5000 times per second, responding to a periodic real-time clock that was initialized once and allowed to run continuously.

// real-time clock interrupt service routine
//   uses and updates G, H, to set goals for movement
//   updates I, J to best estimate of actual position
//   uses and updates Ex, Ey, Fx, Fy to manage timing of moves
//   uses steptab[] to get the control vectors for the motors
//   runs the motors at full power when stepping
//   and reduces the power to L (out of 100) when idle.
//   uses atable[0] to atable[V]
RTCinterrupt() {
    uint16_t remaining;          // number of steps remaining in move
    uint8_t sensors = in(PORTC); // current tilt and pan sensor values
    resetPWMtimer();             // in case the PWM power is too close to 100%
    if (G != 0) { // if pan move in progress
        if (G > 0) { // postive pan move
            Fx = Fx - 1;  // decrement time until next pan step
            if (Fx == 0) { // make pan move
                remaining = G;
                G = G - 1;   // count steps remaining in move
                I = I + 1;   // track current position
                M1dex = M1dex + 1; // spin both motors
                M2dex = M2dex + 1;
            }
        } else { // negative pan move
	    if (((      A & PANSENSOR) != 0)    // avoid hyseresis by only
	    &&  ((sensors & PANSENSOR) == 0)) { // using 1 to 0 transition
                I = 0; // we are home!
            }
            if ((H == 0) // no tilt move in progress
	    &&  ((      A & TILTSENSOR) != 0)    // avoid hysteresis using
	    &&  ((sensors & TILTSENSOR) == 0)) { // 1 to 0 transition
                J = -I; // we are home!
            }
            Fx = Fx - 1; // decrement time until next pan step
            if (Fx == 0) { // make pan move
                G = G + 1;   // count steps remaining to move
                remaining = -G;
                I = I - 1;   // track current position
                M1dex = M1dex - 1; // spin both motors
                M2dex = M2dex - 1;
            }
        }
        if (Fx == 0) { // we changed something
            M1dex = M1dex & 7; // motor indexes increment and decrement mod 8
            M2dex = M2dex & 7;
            M1val = steptab[M1dex]; // get the control vectors for the motors
            M2val = steptab[M2dex];
            out((M1val << 4) | M2val, MOTORS); // spin the motors

            Fx = atable[Ex]; // get time until next step from velocity
       
            if (remaining <= Ex) { // decelerate if approaching destination
                Ex = Ex - 1;
            } else if (Ex < V) { // accelerate if not up to speed
                Ex = Ex + 1;
            }
        }
    }
    if (H != 0) { // if tilt move in progress
        if (H > 0) { // postive tilt move
            Fy = Fy - 1;  // decrement time until next pan step
            if (Fy == 0) { // make pan move
                H = H - 1;   // count steps remaining in move
                remaining = H;
                J = J + 1;   // track current position
                M2dex = M2dex + 1; // spin just tilt motor
            }
        } else { // negative tilt move
	    if (((oldsensors & TILTSENSOR) != 0)    // avoid hysteresis using
	    &&  ((   sensors & TILTSENSOR) == 0)) { // 1 to 0 transition
                J = -I; // we are home!
            }
            Fy = Fy - 1; // decrement time until next tilt step
            if (Fy == 0) { // make pan move
                remaining = -H;
                J = J - 1;   // track current position
                M2dex = M2dex - 1; // spin just tilt motor
            }
        }
        if (Fy == 0) { // we changed something
            M2val = steptab[M2dex]; // get the new control vector
            out((M1val << 4) | M2val, MOTORS); // spin the motors

            Fy = atable[Ey]; // get time until next step from velocity
       
            if (remaining <= Ey) { // decelerate if approaching destination
                Ey = Ey - 1;
            } else if (Ey < V) { // accelerate if not up to speed
                Ey = Ey + 1;
            }
        }
    }
    if ((G == 0)&&(H == 0)) { // no move in progress
        out((M1value << 4) | M2value, MOTORS); // restore full power to motors
        setPWMtimer( L * 200 * MICROSEC );
    }
    A = sensors;
}

The above code has sections both tilt and pan motion that are essentially independent of each other. For each, positive and negative motion are handled separately. This structure was used to minimize both the number of variables required and the number of instructions per interrupt.

A second complicating factor was the need check the position feedback sensors for 1 to 0 transitions only when the motors are spinning in the negative direction. Resetting the I and J position variables only during moves in the same direction minimizes position confusion caused by hysteresis, both in the sensors and caused by backlash in the gear trains.

Note that pan moves involve moving both the tilt and pan motors in exact synchronization, while tilt moves involve moving only the tilt motor. Pan moves can, therefore, reset the tilt position, but this is only reliable if there is not a simultaneous tilt move.

Note also that simultaneous tilt and pan moves are done without any concern for the odd step patterns tha might result. It is quite possible if the moves are in opposite directions to see alternating increment and decrement moves on the tilt motor. This causes no problems at all because the step table works in half steps, so these alternate increment and decrement steps always leave one motor winding on. As a result, the motor never loses its position. The net result, in practice, was surprisingly smooth diagonal moves.

Note that vertical and horizontal acceleration are handled independently. The logic in each case is that the motor always tries to accelerate if it is not at its maximum speed or approaching its goal. On approach to the goal, it decelerates.

Finally, note that, when the motor is not in motion, the PWM timer is set by this interrupt service routine. The minimum power level practical is determined by the time to return from RTCinterrupt() plus the time to enter PWMinterrupt(). Values close to 100% similarly become 100% if the PWM timer fails to fire before the next entry to RTCinterrupt(). In practice, 5kHz PWM worked quite well, and for reasonable cameras, the audio hum was insignificant. Running at 50% power and pushing hard on the camera to come close to the holding torque of the motors did produce an audible 5kHz tone.

Polling from the application

When the application changes X or Y, the target pan and tilt variables, it needs to figure out if this change starts a move. This is done by calling MotorPoll(), which changes G ad H appropriately. The application should also call MotorPoll() every once in a while in case, during a move, the position feedback sensors changed I or J or in case a move was in progress at the time X or Y were updated.

Whenever the application changes X, Y or the bounds on those variables, the application should check that X and Y are in bounds before calling MotorPoll(). The easiest way to do this is to put all the bounds checks in one block of code that is called after any assignment to any variable by the appliction.

// polling code called when convenient by application to keep goal updated
//   simplified:  This version changes goals only when previous move is done
//   the application is responsible for setting X and Y, the goal for each move
MotorPoll() {
    disableInterrupts(); // avoid interrupts during update
    if (G == 0) G = X - I;
    enableInterrupts();
    disableInterrupts(); // avoid interrupts during update
    if (H == 0) H = Y - J;
    enableInterrupts();
}

Note that each computation is in a critical section. This was particularly necessary on the 8-bit PIC microcontroller, where 16-bit arithmetic was done in two 8-bit halves. It would not do to allow RTCinterrupt() to be called while G or H were only partially updated, nor would it be save to allow RTCinterrupt() to be called between inspecting the two halves of G or H.

Two separate critical sections are used in order to minimize the distortion of the real-time-clock interrupt timing that would occur during a long critical section.

In general, when writing appliction code on an 8-bit processor to handle 16-bit quantities, critical sections will be needed around any assignment to a variable that is inspected by an interrupt service routine, and around any inspection of a variable that is updated by an interrupt service routine.

In the final version of the Trippy code, the application had a number of long critical sections for things like reading or writing the auxiliary EEPROM or sending 40kHz IR or serial data to the camera. These long critical sections were always preceeded by waiting for the current tilt or pan moves to stop and then setting the PWM power level to 100%.

Trippy PTZ Embedded Code

The core idea

Global Variables

Interrupt Service Routines

Polling from the application