// Drive as "remote control" using buttons
// At the same time record images and steering angle
// Thomas Bräunl, UWA 2019
#include "eyebot.h"
#define  LSPEED 50

int main ()
{ BYTE img[QVGA_PIXELS];
  int i=0, frame=0, v=0, w=0, key=0;
  char filename[100], cmd[100];

  LCDMenu("GO/STOP", "LEFT", "RIGHT", "END");
  CAMInit(QVGA);

  while((key=KEYRead()) != KEY4)
  { CAMGetGray(img);    // demo
    LCDImageGray(img);  // only
    switch (key)
    { case KEY1: v = LSPEED - v; break; // toggle 
      case KEY2: w += 5; break;
      case KEY3: w -= 5; break;
    }
    LCDSetPrintf(16,0, "frame=%4d,  v=%4d,  w=%4d  ", frame, v,w);
     VWSetSpeed(v,w);
    if (v>0 && i%5==0) // write file - not too often
    { sprintf(filename, "images/%04d.pgm", frame);
      IPWriteFileGray(filename, img);
      sprintf(cmd, "sed -i'' -e 's/^P5/P5 \\# %d/' %s", w, filename);
      system(cmd);
      frame++;
    }
    i++;
    OSWait(10);
  } // while
  VWSetSpeed(0,0); // stop
  return 0;
}
