/*
  Michael Simmons
  Master Project: gRAPI: an indoor four-wheeled robot API for a gumstix connex
  
  Last Modified: 17 Feb 2009
  
  Srf10 class
*/

#include "Srf10.h"

// Constructor
Srf10::Srf10(int handle, int addr, char rt )
{
  readReg = 2;
  pingBuf[0] = 0;
  gainBuf[0] = 1;
  rangeBuf[0] = 2;
  i2cHandle = handle;
  i2cAddr = addr;
  i2cBusWait = 10;

  setResultType(rt);
} // Srf10

void Srf10::ping()
{
  ioctl(i2cHandle, I2C_SLAVE, i2cAddr);
  usleep(i2cBusWait);

  write(i2cHandle, pingBuf, 2);
  usleep(i2cBusWait);
} // ping

int Srf10::readS()
{
  ioctl(i2cHandle, I2C_SLAVE, i2cAddr);
  usleep(i2cBusWait);

  write(i2cHandle, &readReg, 1);
  usleep(i2cBusWait);
  read(i2cHandle, readBuf, 2);
  usleep(i2cBusWait);

  return readBuf[0]*256 + readBuf[1];
} // readS

void Srf10::setGain(int g)
{
  ioctl(i2cHandle, I2C_SLAVE, i2cAddr);
  usleep(i2cBusWait);

  gainBuf[1] = g;
  write(i2cHandle, &gainBuf, 2);
  usleep(i2cBusWait);
} // setGain

void Srf10::setRange(int r)
{
  ioctl(i2cHandle, I2C_SLAVE, i2cAddr);
  usleep(i2cBusWait);

  rangeBuf[1] = r;
  write(i2cHandle, &rangeBuf, 2);
  usleep(i2cBusWait);
} // setRange

void Srf10::setResultType(char rt)
{
  switch(rt)
  {
    case 'i':
    case 'I':
      pingBuf[1] = 80;
      break;
    case 'c':
    case 'C':
      pingBuf[1] = 81;
      break;
    case 's':
    case 'S':
      pingBuf[1] = 82;
      break;
    default:
      pingBuf[1] = 81;
      break;
  }
} // setResultType

/*
  There are 16 addresses available to the srf10 range sensor (0-15).  The
  changeAddress method will take a number from 0 - 15 and perform the
  necessary commands to change the address of that sensor.
*/
void Srf10::changeAddress(int newAddress)
{
  //newAddress is the exact Hex address in the srf10 manual
  int bufVal1 = 0xA0;
  int bufVal2 = 0xAA;
  int bufVal3 = 0xA5;

  ioctl(i2cHandle, I2C_SLAVE, i2cAddr);
  usleep(i2cBusWait);

  pingBuf[1] = 0xA0;
  write(i2cHandle, &pingBuf, 2);
  usleep(i2cBusWait);

  pingBuf[1] = 0xAA;
  write(i2cHandle, &pingBuf, 2);
  usleep(i2cBusWait);

  pingBuf[1] = 0xA5;
  write(i2cHandle, &pingBuf, 2);
  usleep(i2cBusWait);

  pingBuf[1] = (newAddress*2) + 0xE0;
  write(i2cHandle, &pingBuf, 2);
  usleep(i2cBusWait);
} // changeAddress