/*
  testServoRL.cpp
  
  Moves wheels to max right, then center, then max left.
  Then moves incrementally from max left to max right.
*/

#include <iostream>
using namespace std;

#include <stdlib.h>

#include "Servo.h"

const int maxLeft = 100;
const int maxRight = 100;
const int inc = 1;


int main()
{
  int i2cHandle;
  int i;

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

  Servo turn(i2cHandle, 1100, 1894, 3);  // centers wheels

  turn.right(maxRight);
  sleep(2);

  turn.center();
  sleep(2);

  turn.left(maxLeft);
  sleep(2);

  // move wheels from max left to center
  for(i = maxLeft; i > 0; i-=inc)
  {
    turn.left(i);
    usleep(1000);
  }

  // move wheels from center to max right
  for(i = 0; i <= maxRight; i+=inc)
  {
    turn.right(i);
    usleep(1000);
  }
  sleep(3);
  turn.center();

  return 0;
}