/*
 * AConvert.c
 *
 * Elliot Nicholls <nicho-ej@ee.uwa.edu.au>
 *
 * Program to allow the j5bot biped to walk with a preset gait
 *
 **********************************************************************/

#include <eyebot.h>

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

typedef enum{ rHipT,rHipB,rKnee,rAnkle,torso,lAnkle,lKnee,lHipB,lHipT} LINK;
int offset[9] = {128, 128, 128, 128, 128, 128, 128, 128, 128};
int sFact[9] = {-512, -133, -133, 133, -427, -133, 133, 133, -512};
BYTE AConvert(LINK , int);
ServoHandle servo[9];

int main () {

  int i,angle, speed = 2;
  int test;
  char k;

  int   up[9] =  {0, 0, 0, 0, 0, 0, 0, 0, 0};
  int down[9] =  {0, 0, 90, 0, 0, 0, 0, 0, 0};

  /* initialization */
  LCDPrintf("Here is Johnny..\n");

  /* get parameters */
  LCDMenu("+","-","TST","CON");

  /* allocate handles for all servos */
  servo[rHipT]=SERVOInit(RHipT);LCDPrintf("%d-", rHipT);
  servo[rHipB]=SERVOInit(RHipB);  
  servo[rKnee]=SERVOInit(RKnee);
  servo[rAnkle]=SERVOInit(RAnkle);  
  servo[torso]=SERVOInit(Torso);  
  servo[lAnkle]=SERVOInit(LAnkle);
  servo[lKnee]=SERVOInit(LKnee);  
  servo[lHipB]=SERVOInit(LHipB);
  servo[lHipT]=SERVOInit(LHipT);LCDPrintf("%d\n", lHipT);

  /* test middle positions using AConvert() */
  do { 
    LCDPrintf("test %3d\n", test);
    k = KEYGet();
    if (k == KEY1) test += 1;
    if (k == KEY2) test -= 1;
    if (test < 0) test = 0;
    if (test > 8) test = 8;
    if (k == KEY3) {
      LCDPrintf("%d: %d\n", test,AConvert(test, 0));
      SERVOSet(servo[test], AConvert(test, 0));
      KEYWait(ANYKEY);
    }
  } while (k != KEY4);

  /* middle position */
  LCDPrintf("Servos mid-pos..\n");
  for(i=0;i<9;i++) SERVOSet(servo[i], AConvert(i, up[i]));

  LCDMenu("END"," "," "," ");
  k = KEYWait(KEY1);

  SERVORelease(RHipT);  SERVORelease(RHipB);  
  SERVORelease(RKnee);  SERVORelease(RAnkle);  
  SERVORelease(Torso);  
  SERVORelease(LAnkle); SERVORelease(LKnee);  
  SERVORelease(LHipB);  SERVORelease(LHipT);  
}


BYTE AConvert(LINK name, int angle) {

  return (angle * sFact[name])/100 + offset[name];

}