Update of /cvsroot/playerstage/code/player/server/drivers/mixed/chatterbox
In directory 
sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv15262/server/drivers/mixed/chatterbox

Added Files:
        Makefile.am cb_driver.cc cb_i2c.c 
Log Message:
added chatterbox driver, fixed build bug in libplayerc/dev_vectormap.c, fixed 
shallow copy bug in libplayerc/dev_ir.c

--- NEW FILE: cb_driver.cc ---
 /*
 *  Chatterbox robot Player plugin driver
 *  Copyright (C) 2008
 *    Richard Vaughan
 *
 *  Based on Player's multidriver.cc example by Andrew Howard
 *                        
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/*
 * Author: Richard Vaughan
 * Date: 25 January 2008
 * CVS: $Id: cb_driver.cc,v 1.1 2008/02/07 02:23:00 rtv Exp $
 */

#include <unistd.h>
#include <string.h>
#include <netinet/in.h>
#include <math.h>

extern "C" { 
#include "cb_i2c.h" // Jens's library to talk to the CB board over i2c.
}

#include <libplayercore/playercore.h>
//#include <libplayercore/player_interfaces.h>

const unsigned int IRCOUNT = 6;
const unsigned int LEDCOUNT = 5;

////////////////////////////////////////////////////////////////////////////////
// The class for the driver
class ChatterboxDriver : public Driver
{
  public:
    
    // Constructor; need that
    ChatterboxDriver(ConfigFile* cf, int section);

    // Must implement the following methods.
    virtual int Setup();
    virtual int Shutdown();
    virtual int ProcessMessage(QueuePointer & resp_queue, 
                               player_msghdr * hdr, 
                               void * data);

  private:
    // Main function for device thread.
    virtual void Main();

    // sonar interface
    player_devaddr_t ir_addr;

    // lights interface
    player_devaddr_t blinkenlight_addr;
  
  // perform a colorful display
  void LightshowColors();
  
  // perform a Cylon display
  void LightshowCylon();
  
  // program to use to play audio files
  char* audioplayer;

  // play an audio file
  void PlayAudioFile( char* wavfile );
};


// A factory creation function, declared outside of the class so that it
// can be invoked without any object context (alternatively, you can
// declare it static in the class).  In this function, we create and return
// (as a generic Driver*) a pointer to a new instance of this driver.
Driver* ChatterboxDriver_Init(ConfigFile* cf, int section)
{
  // Create and return a new instance of this driver
  return ((Driver*) (new ChatterboxDriver(cf, section)));
}

// A driver registration function, again declared outside of the class so
// that it can be invoked without object context.  In this function, we add
// the driver into the given driver table, indicating which interface the
// driver can support and how to create a driver instance.
void Chatterbox_Register(DriverTable* table)
{
  table->AddDriver("chatterbox", ChatterboxDriver_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Extra stuff for building a shared object.

// plugin code

// /* need the extern to avoid C++ name-mangling  */
// extern "C"
// {
//   int player_driver_init(DriverTable* table)
//   {
//     Chatterbox_Register(table);
//     return(0);
//   }
// }


////////////////////////////////////////////////////////////////////////////////
// Constructor.  Retrieve options from the configuration file and do any
// pre-Setup() setup.
ChatterboxDriver::ChatterboxDriver(ConfigFile* cf, int section)
    : Driver(cf, section)
{
  puts( "Autolab Chatterbox" );

  // establish connection with the CB board
  init(); 

  // Create my IR interface
  if (cf->ReadDeviceAddr(&(this->ir_addr), 
                         section, 
                         "provides", 
                         PLAYER_IR_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);
    return;
  }
  
  if (this->AddInterface(this->ir_addr))
  {
    this->SetError(-1);    
    return;
  }
  
  
  if (cf->ReadDeviceAddr(&(this->blinkenlight_addr), 
                         section, 
                         "provides", 
                         PLAYER_BLINKENLIGHT_CODE, -1, NULL) != 0)
    {
      this->SetError(-1);
      return;
    }
  
  if (this->AddInterface(this->blinkenlight_addr))
    {
      this->SetError(-1);    
      return;
    }
  

  // configure the sound player and play an audio greeting if requested
  audioplayer = strdup( cf->ReadString( section, "audioplayer", 0 ));
  char* audiogreeting = strdup( cf->ReadString( section, "audiogreeting", 0 ));
  if( audioplayer && audiogreeting )
    PlayAudioFile( audiogreeting );
  
  // play a lightshow greeting if requested
  char* lightgreeting = strdup( cf->ReadString( section, "lightgreeting", 0  
));      
  if( lightgreeting )
    {
      if( strcmp( lightgreeting, "cylon" ) == 0 )
        LightshowCylon();
      else if( strcmp( lightgreeting, "colors" ) == 0 )
        LightshowColors();
    }
}

void ChatterboxDriver::LightshowColors()
{
  unsigned long sleeptime = 100000L;
  
  // fade up and down red
  for( double u=0; u<M_PI; u+=0.3 )
    {       
      for( int l=0; l<LEDCOUNT; l++ )
        {
          setLed( l, sin(u)*255.0,0,0 );       
          usleep(sleeptime);
        }
    }
  
  // fade up and down blue
  for( double u=0; u<M_PI; u+=0.3 )
    {       
      for( int l=0; l<LEDCOUNT; l++ )
        {
          setLed( l, 0,0,sin(u)*255.0 );       
          usleep(sleeptime);
        }
    }

  // black
  for( uint8_t l=0; l<LEDCOUNT; l++ )
    setLed( l, 0,0,0 );

  // random green
  for( int b=0; b<50; b++ )
    {
      int now = random()%LEDCOUNT; 
      setLed( now, 0,255,0 );
      usleep(10000);
      setLed( now, 0,0,0 );
    }       

  // white
  for( uint8_t l=0; l<LEDCOUNT; l++ )
    setLed( l, 255,255,255 );
  usleep( 1000000 );

  // black
  for( uint8_t l=0; l<LEDCOUNT; l++ )
    setLed( l, 0,0,0 );
}

void ChatterboxDriver::LightshowCylon()
{
  unsigned long sleeptime = 100000L;

  for( uint8_t l=0; l<LEDCOUNT; l++ )
    setLed( l, 0,0,0 );

  for(int sweeps=0; sweeps<4; sweeps++ )
    {
      setLed( 4, 255,0,0 );
      usleep(sleeptime); 
      setLed( 4, 0,0,0 ); 
      setLed( 0, 255,0,0 );
      usleep(sleeptime); 
      setLed( 0, 0,0,0 ); 
      setLed( 1, 255,0,0 );
      usleep(sleeptime); 
      setLed( 1, 0,0,0 ); 
      setLed( 2, 255,0,0 );
      usleep(sleeptime); 
      setLed( 2, 0,0,0 ); 
      setLed( 1, 255,0,0 );
      usleep(sleeptime); 
      setLed( 1, 0,0,0 ); 
      setLed( 0, 255,0,0 );
      usleep(sleeptime); 
      setLed( 0, 0,0,0 ); 
    }

  for( uint8_t l=0; l<LEDCOUNT; l++ )
    setLed( l, 0,0,0 );
}

void ChatterboxDriver::PlayAudioFile( char* wavfile )
{
  if( ! audioplayer )
    return;
  
  char buf[256];
  snprintf( buf, 255, "%s %s &", audioplayer, wavfile ); 
  system( buf );
}

////////////////////////////////////////////////////////////////////////////////
// Set up the device.  Return 0 if things go well, and -1 otherwise.
int ChatterboxDriver::Setup()
{   
  puts("Chatterbox: Setup");

  // turn on the rangefinders
  enableIr( 1 ); 
    
  puts("Chatterbox: starting thread.");

  // Start the device thread; spawns a new thread and executes
  // ChatterboxDriver::Main(), which contains the main loop for the driver.
  this->StartThread();

  puts("Chatterbox: setup done.");

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device
int ChatterboxDriver::Shutdown()
{
  puts("Shutting down Chatterbox driver...");
  
  // turn off the LEDs
  for( unsigned int l=0; l<LEDCOUNT; l++ )    
    setLed( l, 0,0,0 );        

  // Stop and join the driver thread
  this->StopThread();

  puts("done.");

  return(0);
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void ChatterboxDriver::Main() 
{
  float ranges[IRCOUNT];
  float voltages[IRCOUNT];

  player_ir_data_t irdata;
  irdata.ranges_count = IRCOUNT;
  irdata.ranges = ranges;
  irdata.voltages_count = IRCOUNT;
  irdata.voltages = voltages;

  // The main loop; interact with the device here
  for(;;)
  {
    // test if we are supposed to cancel
    pthread_testcancel();

    // Process incoming messages.  Calls ProcessMessage() on each pending
    // message.
    this->ProcessMessages();

    // read ranges from the IRs    
    float d, v;
    for( unsigned int i=0; i<IRCOUNT; i++ ) 
      {
        if (readDistance(i, &d, &v) == 1) 
          {
            irdata.ranges[i] = d;
            irdata.voltages[i] = v;
          }
        else
          {
            irdata.ranges[i] = -1; // indicate bad data
            irdata.voltages[i] = -1; 
          }
      }
 
    // publish the new range data to Player to deliver to clients
    this->Publish(this->ir_addr, 
                  PLAYER_MSGTYPE_DATA, 
                  PLAYER_IR_DATA_RANGES,
                  (void*)&irdata, 
                  0, 
                  NULL);    

    // TODO - gather and publish more data?
  }
  return;
}


int ChatterboxDriver::ProcessMessage(QueuePointer & resp_queue, 
                                player_msghdr * hdr, 
                                void * data)
{
  // a bit ugly, polling for each light in turn. might be a better way to do 
it...
  for( unsigned int l=0; l<LEDCOUNT; l++ )
    {
      if(Message::MatchMessage(hdr, 
                               PLAYER_MSGTYPE_CMD, 
                               PLAYER_BLINKENLIGHT_CMD_COLOR, 
                               this->blinkenlight_addr ))
        {
          player_blinkenlight_cmd_color_t *cmd = 
            (player_blinkenlight_cmd_color_t*)data;             
          
          //if( cmd->id >= 0 && cmd->id < LEDCOUNT )
          setLed( cmd->id, cmd->color.red, cmd->color.green, cmd->color.blue ); 
       

          return 0; // handled OK
        }

//       if(Message::MatchMessage(hdr, 
//                             PLAYER_MSGTYPE_CMD, 
//                             PLAYER_BLINKENLIGHT_CMD_POWER, 
//                             this->blinkenlight_addr ))
//      {
//        player_blinkenlight_cmd_power_t *cmd = 
//          (player_blinkenlight_cmd_power_t*)data;             

          
//        setLed( l, cmd->color.red, cmd->color.green, cmd->color.blue );       
 
//        return 0; // handled OK
//      }
    }
      

  if(Message::MatchMessage(hdr, 
                           PLAYER_MSGTYPE_REQ, 
                           PLAYER_IR_REQ_POSE, 
                           this->ir_addr))
    {
      // reply with the poses of the 6 sensors
      
      player_pose3d_t poses[IRCOUNT];
      bzero( poses, IRCOUNT*sizeof(player_pose3d_t));

      poses[0].px = 0;
      poses[0].py = 0;
      poses[0].pyaw = 0;

      poses[1].px = 0;
      poses[1].py = 0.076;
      poses[1].pyaw = DTOR(20);

      poses[2].px = -0.040;
      poses[2].py =   0.076;
      poses[2].pyaw = DTOR(90);

      poses[3].px = -0.085;
      poses[3].py = 0;
      poses[3].pyaw = DTOR(180);

      poses[4].px = -0.040;
      poses[4].py =  -0.076;
      poses[4].pyaw = DTOR(270);

      poses[5].px = 0;
      poses[5].py = -0.076;
      poses[5].pyaw = DTOR(340);

      
      player_ir_pose_t reply;
      reply.poses = poses;
      reply.poses_count = IRCOUNT;
      
      Publish( this->ir_addr, resp_queue, 
               PLAYER_MSGTYPE_RESP_ACK, 
               PLAYER_IR_REQ_POSE,
               (void*)&reply, sizeof(reply), NULL );
      
      return(0);
    }
  
  else if( Message::MatchMessage(hdr, 
                        PLAYER_MSGTYPE_REQ, 
                        PLAYER_IR_REQ_POWER, 
                        this->ir_addr))
    {
      puts( "TODO: handle IR power request" );
      return -1;
    }
  
  // TODO handle more messages

  //  else
  // Don't know how to handle this message.
  printf( "Warning: Chatterbox plugin doesn't support msg with type/subtype 
%d/%d\n",
               hdr->type, hdr->subtype);
  return(-1);
}


--- NEW FILE: cb_i2c.c ---


// include Dave Hyland's Robostix interface code
#include "AvrInfo.h"
#include "i2c-dev.h"
#include "i2c-api.h"
#include "i2c-io-api.h"

#include "cb_i2c.h"

// i2c device
static int i2cDev = 0;

// address data structure
static led_t led[5][3];

//-----------------------------------------------------------------------------
// Functions
//-----------------------------------------------------------------------------

//----------------------------------------------------------------------------
/**
 * Initializes a pca9634 chip
 * @param addr i2c address of chip
 * @return 1 if successful, 0 otherwise
 */
int initPCA9634(unsigned char addr)
{ 
  unsigned char data[2];

  I2cSetSlaveAddress(i2cDev, addr, I2C_NO_CRC);

  // enable clock
  data[0] = 0x00;   // register address
  data[1] = 0x0f;   // register value
  if (I2cSendBytes(i2cDev, data, 2) != 0) {
    printf("Error initializing chip %d \n", addr);
    return 0; // error
  }

  // enable led output A
  data[0] = 0x0c;   // register address
  data[1] = 0xff;   // register value
  if (I2cSendBytes(i2cDev, data, 2) != 0) {
    printf("Error initializing chip %d \n", addr);
    return 0; // error
  }

  // enable led output B
  data[0] = 0x0d;   // register address
  data[1] = 0xff;   // register value
  if (I2cSendBytes(i2cDev, data, 2) != 0) {
    printf("Error initializing chip %d \n", addr);
    return 0; // error
  }

  return 1; // success  
}

//-----------------------------------------------------------------------------
/**
 * Resets all PCA9634 chips on the i2c bus
 * @return 1 if successful, 0 otherwise
 */
int resetPCA9634()
{
  unsigned char data[2];

  // Call ALL address
  I2cSetSlaveAddress(i2cDev, 0x03, I2C_NO_CRC);

  // reset all
  data[0] = 0xa5;   
  data[1] = 0x5a;   
  if (I2cSendBytes(i2cDev, data, 2) != 0) {
    printf("Error resetting pca9634 chips \n");
    return 0; // error
  }

  return 1; // success  
}
//----------------------------------------------------------------------------
/**
 * Set the 7 segment display to a given byte
 * @param value [0..255]
 * @return 1 if successful, 0 in case of an error
 */
int set7SegDisplay(unsigned char value)
{
  I2cSetSlaveAddress( i2cDev, ROBOSTIX_ADDR, I2C_USE_CRC );
  if (I2C_IO_WriteReg8(i2cDev, PORTA, value) == 0)
    return 0; // error

  return 1; // success
}
//-----------------------------------------------------------------------------
/**
 * Initializes the driver and hardware. Note this function has to be called
 * first to things to work properly
 * @return 1 if successful, 0 in case of an error
 */
int init()
{ 
  I2C_IO_Info_t   info;

  // open I2C device
  if (( i2cDev = open( I2C_DEV_NAME, O_RDWR )) < 0 ) {
    printf("Error  opening '%s': %s\n", I2C_DEV_NAME, strerror( errno ));
    return 0;  // error
  }
 
  //*****************************************
  // Robostix stuff

  // lets talk to robostix
  I2cSetSlaveAddress( i2cDev, ROBOSTIX_ADDR, I2C_USE_CRC );

  // check if software version robostix is high enough
  if ( !I2C_IO_GetInfo( i2cDev, &info )) {
    printf("Unable to retrieve information from i2c address %d\n", 
            ROBOSTIX_ADDR );
    return 0; // error
  }
  if ( info.version < 2 ) {
    printf("i2c-io.hex on the robostix is too old and needs to be updated\n" );
    return 0;
  }

  // configure DDRA port as output 
  if (I2C_IO_WriteReg8(i2cDev, DDRA, 0xFF) == 0)
    return 0; // error

  // clear 7 seg display
  set7SegDisplay(0);

  // configure ir enable pin as output
  if (I2C_IO_SetGPIODir( i2cDev, 2, 4, 4 ) == 0)
    return 0; // error

  //****************************************
  // RGB stuff

  led[0][RED].chipAddr   = LED0_R_ADDR;
  led[0][RED].ledAddr    = LED0_R_PORT;
  led[0][GREEN].chipAddr = LED0_G_ADDR;
  led[0][GREEN].ledAddr  = LED0_G_PORT;
  led[0][BLUE].chipAddr  = LED0_B_ADDR;
  led[0][BLUE].ledAddr   = LED0_B_PORT;

  led[1][RED].chipAddr   = LED1_R_ADDR;
  led[1][RED].ledAddr    = LED1_R_PORT;
  led[1][GREEN].chipAddr = LED1_G_ADDR;
  led[1][GREEN].ledAddr  = LED1_G_PORT;
  led[1][BLUE].chipAddr  = LED1_B_ADDR;
  led[1][BLUE].ledAddr   = LED1_B_PORT;

  led[2][RED].chipAddr   = LED2_R_ADDR;
  led[2][RED].ledAddr    = LED2_R_PORT;
  led[2][GREEN].chipAddr = LED2_G_ADDR;
  led[2][GREEN].ledAddr  = LED2_G_PORT;
  led[2][BLUE].chipAddr  = LED2_B_ADDR;
  led[2][BLUE].ledAddr   = LED2_B_PORT;

  led[3][RED].chipAddr   = LED3_R_ADDR;
  led[3][RED].ledAddr    = LED3_R_PORT;
  led[3][GREEN].chipAddr = LED3_G_ADDR;
  led[3][GREEN].ledAddr  = LED3_G_PORT;
  led[3][BLUE].chipAddr  = LED3_B_ADDR;
  led[3][BLUE].ledAddr   = LED3_B_PORT;

  led[4][RED].chipAddr   = LED4_R_ADDR;
  led[4][RED].ledAddr    = LED4_R_PORT;
  led[4][GREEN].chipAddr = LED4_G_ADDR;
  led[4][GREEN].ledAddr  = LED4_G_PORT;
  led[4][BLUE].chipAddr  = LED4_B_ADDR;
  led[4][BLUE].ledAddr   = LED4_B_PORT;

  // reset all chips and then initialize them
  if (resetPCA9634() == 0)
    return 0; // error

  if (initPCA9634(CHIP_A_ADDR) == 0)
    return 0; // error

  if (initPCA9634(CHIP_B_ADDR) == 0)
    return 0; // error

  return 1; // succes

}

//----------------------------------------------------------------------------
/**
 * Set the rgb value for a given led. The layout is
 *  0 - front right
 *  1 - front left
 *  2 - left
 *  3 - back
 *  4 - right
 * @param id of led to set
 * @param red [0..255]
 * @param green [0..255]
 * @param blue [0..255]
 * @return 1 successfull, 0 error
 */
int setLed(unsigned char id, unsigned char red, unsigned char green, 
           unsigned char blue)
{
  unsigned char data[2];

  if (id > 4) {
    printf("Error: led id out of bounds [0..4] %d \n",id);
    return 0; // error
  }

  //*************************************
  // send red channel data
  I2cSetSlaveAddress(i2cDev, led[id][RED].chipAddr, I2C_NO_CRC);

  data[0] = led[id][RED].ledAddr;
  data[1] = red;
  if (I2cSendBytes(i2cDev, data, 2) != 0) {
    printf("Error setting color red of led %d to %d \n", id, red);
    return 0; // error
  }

  //*************************************
  // send green channel data
  I2cSetSlaveAddress(i2cDev, led[id][GREEN].chipAddr, I2C_NO_CRC);

  data[0] = led[id][GREEN].ledAddr;
  data[1] = green;
  if (I2cSendBytes(i2cDev, data, 2) != 0) {
    printf("Error setting color red of led %d to %d \n", id, red);
    return 0; // error
  }

  //*************************************
  // send blue channel data
  I2cSetSlaveAddress(i2cDev, led[id][BLUE].chipAddr, I2C_NO_CRC);

  data[0] = led[id][BLUE].ledAddr;
  data[1] = blue;
  if (I2cSendBytes(i2cDev, data, 2) != 0) {
    printf("Error setting color red of led %d to %d \n", id, red);
    return 0; // error
  }

  return 1; // success
}

//-----------------------------------------------------------------------------
/**
 * Sets the 7 segment display to show a given decimal and the dot
 * @param n decimal to display [0..9]
 * @param dot 1 display dot otherwise do not display dot
 * @return 1 if successful, 0 in case of an error
 */
int set7SegNumber(unsigned char n, char dot)
{
  unsigned char byteCode;

  switch (n) {
  case 0:
    byteCode = 0xFC;
    break;
  case 1:
    byteCode = 0x60;
    break;
  case 2:
    byteCode = 0xDA;
    break;
  case 3:
    byteCode = 0xF2;
    break;
  case 4:
    byteCode = 0x66;
    break;
  case 5:
    byteCode = 0xB6;
    break;
  case 6:
    byteCode = 0xBE;
    break;
  case 7:
    byteCode = 0xE0;
    break;
  case 8:
    byteCode = 0xFE;
    break;
  case 9:
    byteCode = 0xF6;
    break;

  default:
    printf("Error: number out of bounds [0..9] \n");
    return 0; // error

  } // switch

  // should we turn the dot on ?
  if (dot == 1)
    byteCode |= 0x01;


  return set7SegDisplay(byteCode);
}
//-----------------------------------------------------------------------------
/**
 * Shows a rotating segment bar and progresses it one position
 * @return 1 if successful, 0 in case of an error
 */
int rotateStep()
{
  unsigned char byteCode;
  static int state = 0;

  switch (state) {
  case 0:
    byteCode = 0x80;
    break;
  case 1:
    byteCode = 0x40;
    break;
  case 2:
    byteCode = 0x20;
    break;
  case 3:
    byteCode = 0x10;
    break;
  case 4:
    byteCode = 0x08;
    break;
  case 5:
    byteCode = 0x04;
    state = -1;
    break;
  default:
    byteCode = 0;
    state = 0;

  } // switch

  state++;

  return set7SegDisplay(byteCode);
}
//-----------------------------------------------------------------------------
/**
 * Enables or disables the ir sensors
 * @param enable = 1 enable, disable otherwise
 * @return 1 if successful, 0 in case of an error
 */
int enableIr(char enable)
{ 
  I2cSetSlaveAddress( i2cDev, ROBOSTIX_ADDR, I2C_USE_CRC );
  
  if (enable == 1) {
    if ( I2C_IO_SetGPIO(i2cDev, 2, 2, 1) == 0) {
      printf("Error: while enabling IR \n");
      return 0; // error
    }
  }
  else {
    if ( I2C_IO_SetGPIO(i2cDev, 2, 2, 0) == 0) {
      printf("Error: while disabling IR \n");
      return 0; // error
    }
  }

  return 1; // success
}
//-----------------------------------------------------------------------------
/**
 * Checks if the IR sensors are enabled
 * @return 1 enabled, 0 disabled, -1 error
 */
int isIrEnabled()
{
  unsigned char pinVal;

  I2cSetSlaveAddress( i2cDev, ROBOSTIX_ADDR, I2C_USE_CRC );
  if (I2C_IO_GetGPIO( i2cDev, 2, &pinVal) == 0) {
    printf("Error: failed to read ir status \n");
    return -1; // error
  }
 
  return pinVal & 0x02;
 
}
//-----------------------------------------------------------------------------
/**
 * Reads the 10bits from the adc
 * @param id of ADC [0..7]
 * @return 10 bit value or -1 in case of an error
 */
int readAdc(unsigned char id)
{
  unsigned short adcVal;

  if (id > 7) {
    printf("ADC id out of bounds [0..7] %d \n", id);
    return -1;
  }
  I2cSetSlaveAddress( i2cDev, ROBOSTIX_ADDR, I2C_USE_CRC );

  if ( I2C_IO_GetADC( i2cDev, id, &adcVal ) == 0) {
    printf("Error while reading from ADC %d \n", id);
    return -1;
  }
  return adcVal;
}
//-----------------------------------------------------------------------------
/**
 * Reads the distance from an ir sensor. The layout is:
 *  0 - front center
 *  1 - front left
 *  2 - left
 *  3 - back center
 *  4 - right
 *  5 - front right
 * @param id of ir sensor [0..5]
 * @return distance [m]
 * @return voltage [V]
 * @return 1 if successfull, 0 otherwise
 */
int readDistance(unsigned char id, float* distance, float* voltage)
{
  float u;
  float v;
  float range;
  unsigned char adcId = 0;

  if (id > 5) {
    printf("IR id out of bounds [0..5] %d \n", id);
    return 0;  // error
  }

  switch (id) {
  case 0:      // front center
    adcId = 7;
    break;
  case 1:      // front left
    adcId = 4;
    break;
  case 2:      // left side
    adcId = 3; 
    break;
  case 3:      // back center
    adcId = 6; 
    break;
  case 4:      // right side
    adcId = 2; 
    break;
  case 5:      // front right
    adcId = 5;
    break;
  } // switch

  u = (float)(readAdc(adcId)) / 1024.0 * VREF;
  
  v = u;
  range = K5;
  range += K4 * v;
  v = v * u; 
  range += K3 * v;
  v = v * u; 
  range += K2 * v;
  v = v * u; 
  range += K1 * v;
  v = v * u; 
  range += K0 * v;

  *distance = range;
  *voltage  = u;
  return 1; // success
}

--- NEW FILE: Makefile.am ---
noinst_LTLIBRARIES =
if INCLUDE_CHATTERBOX
noinst_LTLIBRARIES += libchatterbox.la
endif

ROBOSTIX=/home/gumstix/vR1078experimental/robostix

# NASTY HACK! RTV
AM_CPPFLAGS = -Wall -I$(top_srcdir) -I$(ROBOSTIX)/Shared 
-I$(ROBOSTIX)/gumstix/Common -I$(ROBOSTIX)/gumstix/roomba

libchatterbox_la_SOURCES = cb_driver.cc cb_i2c.c cb_i2c.h

# this stuff is the right way to do it, but it's not respected by 
libplayerdrivers
# so instead the missing library is inserted in libplayerdrivers's Makefile.am
#libchatterbox_la_LIBADD = -lroboi2c
#libchatterbox_la_LDFLAGS = -L$(ROBOSTIX)/gumstix/roomba

-------------------------------------------------------------------------
This SF.net email is sponsored by: Microsoft
Defy all challenges. Microsoft(R) Visual Studio 2008.
http://clk.atdmt.com/MRT/go/vse0120000070mrt/direct/01/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit

Reply via email to