/* ***************************************************************

Author: Thomas Braunl, 22.05.98, UWA
Editor: Peter Vanopulos 03.11.1999 
Filename: gym.c (gymnastics)

Purpose:
  For Johnny and Jack. At first the robot stands in a balanced upright
  position. When go is hit the robot does squats where the robot bends 
  the hips, knees and ankles (in Jacks case ankles are negligible) down
  and up continuously.

********************************************************************/

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

#define STEPS 100    /* constants */
ServoHandle serv[9]; /* global variables */

/** moveit().
    For every servo made to perform the squats get the current servo
    position and determine from 
    the new position the difference in movement for interations.
    This function is used to move the leg in a direction.
    For example, move leg up or down.
    @param old - the old position we are currently at
    @param new - the new position we want to move to
    @param speed - the speed to set the servo's to move the leg   
*/
void moveit(int pold[], int pnew[], int speed)
{ int i,j;
  int steps = STEPS;          /* between old and new positions make STEPS steps */
  float now[9], diff[9];

  for (j=0; j<9; j++)         /* update every servo from the old to new positions */
  { now[j]  = (float) pold[j]; /* current servo position */
    diff[j] = (float) (pnew[j] - pold[j]) / (float) steps;
                              /* determine interation "diff" from old to new position */
    LCDPrintf("%lf", diff[j]); /* float "diff" prints servo change */
  }

  for (i=0; i<steps; i++)
  {
    for (j=0; j<9; j++)
    { now[j] +=  diff[j];    /* update the current position moving from old to new */

      SERVOSet(serv[j], (int) now[j]); 
        /* move the j'th servo to the new calculated position now[j] */
    }
    OSWait(speed);
  }
}

/************* MAIN ******************/
int main ()
{ int i,angle, speed;
  int k;
  typedef enum{ rHipT,rHipB,rKnee,rAnkle,torso,lAnkle,lKnee,lHipB,lHipT} link;
  int   up[9] = {127, 127,  127,   127,   127,  127,   127,  127,  127};
        /*  up position for leg */
  int down[9] = {127,  80,  200,   80,    127,  200,    80,  200,  127};
        /*  down position for leg to squat */

  LCDPutString("Here is Johnny...\n");

  /* get parameters */
  LCDMenu("+","-"," ","CON");
  angle = 60;  /* this is how deep we squat down, initialise to 60 degrees */
  do           /* the greater the angle the deeper the squat */
  { LCDPrintf("angle %d\n", angle);
    k = KEYGet();
    if (k == KEY1 && angle <= 65) angle += 5;
    if (k == KEY2 && angle >= 40) angle -= 5;
  } while (k != KEY4);


  speed = 2;  /* initialse the speed the servo's work at to 2 */
  do          /* the greater the speed the greater the rate at squating
                 and the greater the possibilty of falling over */
  { LCDPrintf("speed %d\n", speed);
    k = KEYGet();
    if (k == KEY1 && speed <= 9) speed += 1;
    if (k == KEY2 && speed >= 1) speed -= 1;
  } while (k != KEY4);

  /* set positions */
  down[rHipT]  = 127;            down[rHipB]  = 127 - angle; 
  down[rKnee]  = 127 + 2*angle;  down[rAnkle] = 127 + angle;
  down[torso]  = 127; 
  down[lAnkle] = 127 - angle;    down[lKnee]  = 127 - 2*angle;
  down[lHipB]  = 127 + angle;    down[rHipT]  = 127; 

  /* allocate handels for all servos */
  serv[0]=SERVOInit(RHipT);  serv[1]=SERVOInit(RHipB);  
  serv[2]=SERVOInit(RKnee);  serv[3]=SERVOInit(RAnkle);  
  serv[4]=SERVOInit(Torso);  
  serv[5]=SERVOInit(LAnkle); serv[6]=SERVOInit(LKnee);  
  serv[7]=SERVOInit(LHipB);  serv[8]=SERVOInit(LHipT);  

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

  LCDMenu("GO"," "," ","END");
  k = KEYWait(KEY1);    /* wait until key1 is hit before start gym exercises*/

  while (k != KEY4)           /* do gym exercises until the key4(end) is hit  */
  { moveit(up,down, speed);   /* move leg up */
    moveit(down,up, speed);   /* move leg straight */
    k = KEYRead();            /* check for the end key (key4) */
  }

  /* release handles to servo's */
  for (i=0; i<9; i++) SERVORelease(serv[i]);
  return 0;
}