#include "eyebot.h"
pthread_mutex_t rob;;

void *cam(void *arg)
{ int i=0;
  QVGAcol img;

  while(1)
  { pthread_mutex_lock(&rob);
      CAMGet(img);
      LCDImage(img);
      LCDSetPrintf(19,0, "%4d Image ", i++);
    pthread_mutex_unlock(&rob);
    sleep(1);  // sleep for 1 sec
  }
  return NULL;
}


void *psd(void *arg)
{ int j=0, d;
  while(1)
  { pthread_mutex_lock(&rob);
      d = PSDGet(PSD_FRONT);
      LCDSetPrintf(20,0, "%4d Dist=%4d ", j++, d);
    pthread_mutex_unlock(&rob);
    usleep(50);  // sleep for 0.1 sec
  }
}


int main()
{ pthread_t t1, t2;

  XInitThreads();
  pthread_mutex_init(&rob, NULL);
  CAMInit(QVGA);
  LCDMenu("", "", "", "END");

  pthread_create(&t2, NULL, cam, (void *) 1);
  pthread_create(&t1, NULL, psd, (void *) 2);
  KEYWait(KEY4);
  pthread_exit(0); // will terminate program
}
