#include "eyebot.h"
#define JNT 6

int main ()
{ int angles[JNT] = {0,0,0,0,0,0};
  int joint = 0, done  = false;

  LCDMenu("JOINT","+","-","END");
  while (!done)
  { LCDSetPrintf(0,0, "Joint %1d - Angles %4d %4d %4d %4d %4d %4d %4d ",
    joint+1, angles[0],angles[1],angles[2],angles[3],angles[4],angles[5]);
    switch (KEYGet())
    { case KEY1: joint = (joint+1)%JNT; break;
      case KEY2: angles[joint] +=10;    break;
      case KEY3: angles[joint] -=10;    break;
      case KEY4: done = true;
    }
    if (angles[joint]>255) angles[joint]=255;
    if (angles[joint]<  0) angles[joint]=  0;
    SERVOSet(joint+1, angles[joint]); // drive to new angle
  }
}
