#include <iostream>
using namespace std;

#include <stdlib.h>

#include "Motor.h"
#include "Srf10.h"

int main()
{
  int i2cHandle;

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

  Motor go(i2cHandle, 1100, 1900, 4);
  Srf10 sensor2(i2cHandle, 0x72, 'i');

  int inc, dist2, stopDist;

  cout << "Enter desired speed: " << endl;
  cin >> inc;

  cout << "Sensor stop distance: ";
  cin >> stopDist;
  cout << endl;

//  go.increase(inc);

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

    if(dist2 > stopDist)
      go.forward(inc);
    else
      go.stop();
  }
  return 0;
}