#include <iostream>
using namespace std;

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

int main()
{
  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, 4);
  Motor go(i2cHandle, 1100, 1900, 4);

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

    if(dist2 > 10)
    {
//      turn.center();
      go.forward(30);
    }
    else
    {
//      turn.left(50);
      go.reverse(20);
    }
  }

  return 0;
}