#include "eyebot.h"
#define SCANSIZE 180
#define SAFE     250
#define PSD_FL     5
#define PSD_FR     6
#define PSD_FFL    7
#define PSD_FFR    8

int main ()
{ int i, min_pos, scan[SCANSIZE], F,L,R,FL,FR,FFL,FFR;

  while (KEYRead()!=KEY4)
  { LCDClear();
    LCDMenu("", "", "", "END");
    LIDARGet(scan);
    min_pos = 0;
    for (i=0; i<SCANSIZE; i++)
    { if (scan[i] < scan[min_pos]) min_pos = i;
      LCDLine(i,250-scan[i]/100, i,250, BLUE);
    }

    F=  PSDGet(PSD_FRONT);
    L=  PSDGet(PSD_LEFT); R  =PSDGet(PSD_RIGHT);
    FL =PSDGet(PSD_FL);   FR =PSDGet(PSD_FR);
    FFL=PSDGet(PSD_FFL);  FFR=PSDGet(PSD_FFR);

    if  (F<SAFE) VWSetSpeed(0, -90);
      else if (L<SAFE || FL<SAFE || FFL<SAFE) VWSetSpeed(150, -20);  // slope right
        else if (R<SAFE || FR<SAFE || FFR<SAFE) VWSetSpeed(150, +20); // slope left
          else VWSetSpeed(300, 90 - min_pos);  // drive in adapted direction
    OSWait(200); // 0.2 sec
  }
}
