BeeSoft User's Guide and Reference

7.4 A Sample Program Using pantiltServer

This sample program shows you how you can make use of pantiltServer commands. It simply points your robot’s cameras at someone, and "tracks" that person as he or she walks around your robot. The program looks at sonar readings from the robot, and points the cameras at a point 40 cm above the pan/tilt head, in the direction of the sonar sensor that gave the reading indicating it is closest to the "obstacle" — the person it’s keeping track of.

/*===========================================*/
/*===========================================*/
panTest.c
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <baseClient.h>
#include <pantiltclient.h>
#include <rai.h>
#include <bUtils.h>
/*===========================================*/
RaiModule* demoModule;
void
demoPoll (RaiModule * demoModule)
{
     float newTilt, newPan;
     int ii, closest;
     closest = bRobot.sonar_cols[0]/2;
     for (ii=0; ii<bRobot.sonar_cols[0]; ii++) { 
          if (sonars[ii].value < sonars[closest].value) {
               closest = ii;
          }
     }
     newTilt = atan2(40.0, (float)sonars[closest].value
         / 10.0);
     newPan = bSonarAngle(closest);
     ptMoveTo (newPan, newTilt);
     fprintf (stderr, "%s:%s( ) - sonar %3d = %5.1f,
        newPos = (%6.1f, %6.1f) \ n",
        __FILE__, __FUNCTION__,
        closest, (float)sonars[closest].value/10.0,
        newPan * 180.0 / PI, newTilt * 180.0 / PI);
     return;
}
/*===========================================*/
void
createDemoModule( )
{
     /* Ask that polling function be run */
     /* every 500 msecs */
     demoModule=makeModule (panTest", NULL);
     addPolling (demoModule, demoPoll, 500);
}
/*===========================================*/
void
commShutdown( )
{
     printf ("Somebody died. Exiting. \n");
     RaiShutdown( );
     }
/*===========================================*/
int
main (int argc, char** argv)
{
     struct bParamList * params = NULL;
     fprintf (stderr, "starting main\n");
     /* Add some parameter files. */
     params = bParametersAddFile(params,
         "etc/beeSoft.ini");
     /* Add some environment variables. */
     params = bParametersAddEnv(params, " " ,
         "TCXHOST");
     /* Add command line arguments. */
     bParametersAddArray(params, " ", argc, argv);
     /* Fill the global parameter structure. */
     bParametersFillParams (params);
     registerBaseclient( );
     ptregister( );
     /* Set up func. to be called if baseServer dies. */
     initClient ("panTest", commShutdown);
     /* Hook up to running server. */ 
     findBaseServer( );
     ptConnect ( );
     /* Initialize (but do not start) Scheduler. */
     RaiInt( );
     catchInterrupts( );
     /* Set up modules to do communication for you. */
     initClientModules( );
     /* Set up your own module to move pan/tilt head. */
     createDemoModule( );
     /* Start scheduling! */
     Raistart ( );
     /* Note: this will terminate only when /*
     /* RaiShutdown is called, or on CTRL-C. */
     return(0);
}
/*===========================================*/ 

In This Chapter You’ve Learned...

....how to control the speed, direction and acceleration of your robot’s pan/tilt head, and how to interact with BeeSoft’s pantiltServer. You also worked with a simple test program that introduced a few of pantiltServer’s capabilities. Now, use this simple program as a template to experiment further with your robot’s pan/tilt mechanism.