#include <iostream>
using namespace std;

#include <stdlib.h>

#include "Motor.h"
#include "Servo.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);
  Servo turn(i2cHandle, 1100, 1900, 3);
  Srf10 sensor2(i2cHandle, 0x72, 'i');

  int inc, dist2;
  char direction;

  cout << "Which direction do you want to test(f, b)?" << endl;
  cin >> direction;

  if(direction == 'f')
    go.forward(0);
  else
    go.reverse(0);

  turn.right(50);

  while(1)
  {
    cout << "Enter increase amount, zero to stop: " << endl;
    cin >> inc;

    if(inc > 0)
      go.increase(inc);
    else
      go.stop();
  }
  return 0;
}