/*
  testSpeedFwRv.cpp

  moves wheels minForwd
  then moves wheels minReverse
*/

#include <iostream>
using namespace std;

#include <stdlib.h>

#include "Motor.h"
#include "Servo.h"

const int minForward = 12;
const int minReverse = 20;

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, 3);
  Servo turn(i2cHandle, 1100, 1900, 3);  // centers wheels

  go.forward(minForward);
  sleep(3);
  
  go.stop();
  sleep(1);
  
  go.reverse(minReverse);
  sleep(3);
  
  go.stop();

  cout << "Test complete" << endl;
  
  return 0;
}