#include <iostream>
using namespace std;

#include <stdlib.h>

//#include "Srf10.h"
//#include "Servo.h"
//#include "Motor.h"
#include "Cmps03.h"

int main()
{
  int i2cHandle;
  double deg;
  double bob;
double junk = 15.43;

  if((i2cHandle = open("/dev/i2c-0", O_RDWR)) < 0)
  {
    cout << "Open Failed" << endl;
    exit(1);
  }
while(1)
{
  Cmps03 compass(i2cHandle);
usleep(65000);
  deg = compass.readC();
  bob = deg/10.0;
cout << junk << endl;
//  cout << "Deg: " << deg << " deg:" << bob << endl;
  printf("Deg: %.1f  deg: %.1f\n", deg, bob);
}
/*
  int i2cHandle;
  int dist1, dist2;

  if((i2cHandle = open("/dev/i2c-0", O_RDWR)) < 0)
  {
    cout << "Open Failed" << endl;
    exit(1);
  }

//  Srf10 sensor1(i2cHandle, 0x71, 'i');
  Srf10 sensor2(i2cHandle, 0x72, 'i');
//  Srf10 sensor3(i2cHandle, 0x73, 'i');

  Servo turn(i2cHandle, 1100, 1900, 3);
  Motor go(i2cHandle, 1100, 1900, 4);

  while(1)
  {
    sensor2.ping();
    usleep(130000);
    dist2 = sensor2.readS();

    if(dist2 > 10)
    {
      turn.center();
      go.forward(15);
    }
    else
    {
      turn.left(50);
      go.stop();
    }
  }
*/
  return 0;
}