//----------------------------------------------------------------------
// Description:  Motor Encoder testing
// Author:       Thomas Braunl UWA 2020
//---------------------------------------------------------------------- 

#include "eyebot.h"
#include <stdio.h>
#include <math.h>

int main ()
{ int e1=0, e2=0, key=0, last=0, print;

  LCDMenu("FWD/STOP","BACK/STOP","TURN/STOP","END");
  do
  { key = KEYRead();
    if (key == last || key == KEY4) // stop at 2nd press of same key
    { MOTORDrive(1, 0);
      MOTORDrive(2, 0);
      last=0; 
    }
    else switch(key)
    { case KEY1: MOTORDrive(1, 50);
                 MOTORDrive(2, 50);
                 last = key;
                 break;
      case KEY2: MOTORDrive(1, -50);
                 MOTORDrive(2, -50);
                 last = key;
                 break;
      case KEY3: MOTORDrive(1, +50);
                 MOTORDrive(2, -50);
                 last = key;
    }

    print = e1 != ENCODERRead(1) || e2 != ENCODERRead(2);
    e1 = ENCODERRead(1);
    e2 = ENCODERRead(2);
    if (print)    // don"t print all the time
      LCDPrintf("enc1 %d enc2 %d \n", e1, e2);
    OSWait(100);  // 0.1 sec 
  } while(key != KEY4);
}
