// Mako Basic Movement Demo, M. Finn May 2018
// extended by T. Braunl, June 2018
#include "eyebot.h"

// Thruster IDs.
#define LEFT   1
#define FRONT  2
#define RIGHT  3
#define BACK   4

#define PSD_DOWN 6   // PSD direction

void drive(int l_speed, int r_speed)
{ MOTORDrive(LEFT,  l_speed);
  MOTORDrive(RIGHT, r_speed);
}

int main()
{ BYTE img[QVGA_SIZE];
  char key;
  
  LCDMenu("FORWARD", "LEFT", "RIGHT", "END");
  CAMInit(QVGA);
  do { LCDSetPrintf(19,0, "Dist to Front:%6d\n", PSDGet(PSD_FRONT));
       CAMGet(img);
       LCDImage(img);

       switch(key=KEYRead())
       { case KEY1: drive( 100,  100);  break;
         case KEY2: drive(-100,  100);  break;
         case KEY3: drive( 100, -100);  break;
       }
     } while (key != KEY4);
  return 0;  
}