#include "eyebot.h"
#define MIN(a,b) (((a)<(b))?(a):(b))
#define MAX(a,b) (((a)>(b))?(a):(b))

int main()
{ int pos[5] = {128,128,128,128,128};
  int i,k, leg=0;

  LCDMenu("Leg+", "+", "-", "END");
  do
  { switch(k=KEYGet())
    { case KEY1: leg = (leg+1)%5; break;
      case KEY2: pos[leg] = MIN(pos[leg]+5, 255); break;
      case KEY3: pos[leg] = MAX(pos[leg]-5,   0); break;;
    }

    for (i=0; i<5; i++) 
    { LCDPrintf("S%d pos %d, ", i+1, pos[i]);
      SERVOSet(i+1, pos[i]);
    }
    LCDPrintf("\n");
  } while (k != KEY4);
}
