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

#include "Cmps03.h"
#include <iostream>

using namespace std;

// Constructor
Cmps03::Cmps03(int handle)
{
  readReg = 2;
  i2cHandle = handle;
  i2cAddr = 0x60;
  i2cBusWait = 10;
} // Cmps03

double Cmps03::readC()
{
  float value;

  ioctl(i2cHandle, I2C_SLAVE, i2cAddr);

  write(i2cHandle, &readReg, 1);
  usleep(i2cBusWait);
  read(i2cHandle, readBuf, 2);
  usleep(i2cBusWait);
  value = readBuf[0]*256 + readBuf[1];

  value = value/10.0;

  return value;
} // readC

int Cmps03::calibrate()
{
  int i,
      direction;
  uint8_t calReg = 15;
  uint8_t writeBuf[2];

  writeBuf[0] = calReg;
  writeBuf[1] = 255;

  for(i = 4; i > 0; i--)
  {
    do
    {
      switch(i)
      {
        case 4: cout << "Point vehicle North and enter a 4" << endl;
                break;
        case 3: cout << "Point vehicle East and enter a 3" << endl;
                break;
        case 2: cout << "Point vehicle South and enter a 2" << endl;
                break;
        case 1: cout << "Point vehicle West and enter a 1" << endl;
                break;
      }
      cin >> direction;
    } while (i != direction);

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

    write(i2cHandle, &writeBuf, 2);
    usleep(i2cBusWait);
  }
} // calibrate