#include "simpletools.h"                      // Include simpletools header
#include "adcDCpropab.h"                             // Include ping header
#include "servo.h"

#define EMIC_TX 6
#define EMIC_RX 7

int main()                                    // main function
{
  pause(1000);
  adc_init(21, 20, 19, 18);
  //FdSerial_Start(EMIC_RX, EMIC_TX, 0, 9600);

  float lrV, udV;
  int ud, lr;

  while(1)                                    // Repeat indefinitely
  {
    udV = adc_volts(2);
    lrV = adc_volts(3);

    ud = 200*(udV-2.5)/5;
    lr = 200*(lrV-2.5)/5;

    if (ud > 10)
    {
      print("Up \n");
      servo_angle(16, 900);
      pause(3000);
    }
    else if (ud < -10)
    {
      print("Down \n");
      servo_angle(16, 0);
      pause(3000);
    }
    if (lr > 10)
    {
      print("Right \n");
      servo_angle(17, 900);
      pause(3000);
    }
    else if (lr < -10)
    {
      print("Left \n");
      servo_angle(16, 0);
      pause(3000);
    }
    pause(100);
  }
}