#include "eyebot.h"
#define SAFE    300
#define HORIZON 120

int main ()
{ BYTE img[QVGA_PIXELS], sob[QVGA_PIXELS];
  int l, f, r, k, angle=0;

  LCDMenu("MANUAL", "AUTO", "", "END");
  CAMInit(QVGA);

  k=KEYGet();
  if (k==KEY1) // Manual
  { LCDMenu("LEFT", "RIGHT", "", "END");
    while (k != KEY4)
    { CAMGetGray(img);
      IPSobel(img, sob);
      LCDImageGray(sob);
      LCDLine(0, HORIZON, 319, HORIZON, RED); // horizon
      k=KEYRead();
      if (k==KEY1)      angle += 10;
      else if (k==KEY2) angle -= 10;
      LCDSetPrintf(18,0, "Curve %4d", angle);
      VWCurve(100, angle, 50); // moderate speed
      OSWait(100); // wait 0.1 s
    }
  }

  else if (k==KEY2)
  while(KEYRead() != KEY4)
  { CAMGetGray(img);
    IPSobel(img, sob);
    LCDImageGray(sob);
    LCDLine(0, HORIZON, 319, HORIZON, RED); // horizon
    l = PSDGet(PSD_LEFT);
    f = PSDGet(PSD_FRONT);
    r = PSDGet(PSD_RIGHT);
    LCDSetPrintf(18,0, "PSD L%3d F%3d R%3d", l, f, r);
  } // while
  return 0;
}
