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

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>  /* array need this */

/* global variables */
ServoHandle serv[9];

/* constants */
#define STEPS 100


/** 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 old[], int new[], 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) old[j];                             /* current
servo position */
    diff[j] = (float) (new[j] - old[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 position
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 */
  SERVORelease(RHipT);  SERVORelease(RHipB);  
  SERVORelease(RKnee);  SERVORelease(RAnkle);  
  SERVORelease(Torso);  
  SERVORelease(LAnkle); SERVORelease(LKnee);  
  SERVORelease(LHipB);  SERVORelease(LHipT);  
  return 0;
}

