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

#define emic_tx 6
#define emic_rx 7

int main()                                    // main function
{
  pause(1000);
  adc_init(21, 20, 19, 18);
  fdserial *Emic2 = fdserial_open(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");
      dprint(Emic2, "s");
      dprint(Emic2, "Up up and away!\n");
      servo_angle(16, 900);
      pause(500);
      servo_angle(16, 1025);
      pause(500);
    }
    else if (ud < -10)
    {
      print("Down \n");
      dprint(Emic2, "s");
      dprint(Emic2, "We're going down!\n");
      servo_angle(16, 1200);
      pause(500);
      servo_angle(16, 1025);
      pause(500);
    }
    if (lr > 10)
    {
      print("Right \n");
      dprint(Emic2, "s");
      dprint(Emic2, "To the right, to the right!\n");
      servo_angle(17, 900);
      pause(500);
      servo_angle(17, 1020);
      pause(500);
    }
    else if (lr < -10)
    {
      print("Left \n");
      dprint(Emic2, "s");
      dprint(Emic2, "To the left, to the left!\n");
      servo_angle(17, 1200);
      pause(500);
      servo_angle(17, 1020);
      pause(500);
    }
    pause(100);
  }
}