/* ********************************************* */
/* Simplified Walking Program for 6legged Walker */
/* Thomas Braunl, Tong An, Dec. 2000             */
/* ********************************************* */
#include "eyebot.h"
 
#define NumServos 12

ServoHandle servohandles[12];           /* Semantics for servos */
int semas[12]  ={SER_LFUD,SER_LFFB,SER_RFUD,SER_RFFB,SER_LMUD,SER_LMFB,
                 SER_RMUD,SER_RMFB,SER_LRUD,SER_LRFB,SER_RRUD,SER_RRFB};


#define MAXF  50
#define MAXU  60
#define CNTR 128      
#define UPWD (CNTR+MAXU)
#define DOWN (CNTR-MAXU)  
#define FORW (CNTR-MAXF)  
#define BACK (CNTR+MAXF)

#define GaitForwardSize 6
int GaitForward[GaitForwardSize][12]= {
 {DOWN,FORW, UPWD,BACK, UPWD,BACK, DOWN,FORW, DOWN,FORW, UPWD,BACK},
 {DOWN,FORW, DOWN,BACK, DOWN,BACK, DOWN,FORW, DOWN,FORW, DOWN,BACK},
 {UPWD,FORW, DOWN,BACK, DOWN,BACK, UPWD,FORW, UPWD,FORW, DOWN,BACK},
 {UPWD,BACK, DOWN,FORW, DOWN,FORW, UPWD,BACK, UPWD,BACK, DOWN,FORW},
 {DOWN,BACK, DOWN,FORW, DOWN,FORW, DOWN,BACK, DOWN,BACK, DOWN,FORW},
 {DOWN,BACK, UPWD,FORW, UPWD,FORW, DOWN,BACK, DOWN,BACK, UPWD,FORW},
};

#define GaitTurnRightSize 6
int GaitRight[GaitTurnRightSize][12]= {
 {DOWN,FORW, UPWD,FORW, UPWD,BACK, DOWN,BACK, DOWN,FORW, UPWD,FORW},
 {DOWN,FORW, DOWN,FORW, DOWN,BACK, DOWN,BACK, DOWN,FORW, DOWN,FORW},
 {UPWD,FORW, DOWN,FORW, DOWN,BACK, UPWD,BACK, UPWD,FORW, DOWN,FORW},
 {UPWD,BACK, DOWN,BACK, DOWN,FORW, UPWD,FORW, UPWD,BACK, DOWN,BACK},
 {DOWN,BACK, DOWN,BACK, DOWN,FORW, DOWN,FORW, DOWN,BACK, DOWN,BACK},
 {DOWN,BACK, UPWD,BACK, UPWD,FORW, DOWN,FORW, DOWN,BACK, UPWD,BACK},
};

#define GaitTurnLeftSize 6
int GaitLeft[GaitTurnLeftSize][12]= {
 {DOWN,BACK, UPWD,BACK, UPWD,FORW, DOWN,FORW, DOWN,BACK, UPWD,BACK},
 {DOWN,BACK, DOWN,BACK, DOWN,FORW, DOWN,FORW, DOWN,BACK, DOWN,BACK},
 {UPWD,BACK, DOWN,BACK, DOWN,FORW, UPWD,FORW, UPWD,BACK, DOWN,BACK},
 {UPWD,FORW, DOWN,FORW, DOWN,BACK, UPWD,BACK, UPWD,FORW, DOWN,FORW},
 {DOWN,FORW, DOWN,FORW, DOWN,BACK, DOWN,BACK, DOWN,FORW, DOWN,FORW},
 {DOWN,FORW, UPWD,FORW, UPWD,BACK, DOWN,BACK, DOWN,FORW, UPWD,FORW},
};


int PosInit[12]=
 {CNTR,CNTR, CNTR,CNTR, CNTR,CNTR, CNTR,CNTR, CNTR,CNTR, CNTR,CNTR};


void set_joint(int pos1[12])
/* set all servos to given position */
{ int servo;
  for (servo=0; servo<NumServos; servo++)
    SERVOSet(servohandles[servo], pos1[servo]);
}


void move_joint(int pos1[12], int pos2[12], int speed)
/* speed between 1.. 100 */
{ int steps = 50;
  int i,servo;
  float size[12];

  for (servo=0; servo<NumServos; servo++)
    size[servo] = (float) (pos2[servo]-pos1[servo]) / (float) steps;

  for (i=0;i<steps;i++)
  { for(servo=0; servo<NumServos; servo++)
      SERVOSet(servohandles[servo], pos1[servo]+ (int)((float) i *size[servo]));
    OSWait(10/speed);
  }
}


void gait(int g[][12], int size, int speed)
{ int i;
  for (i=0; i<size; i++)
    move_joint(g[i], g[(i+1)%size], speed);
}


/* main walker program                */
int main ()
{ int  i,k;
  int  walksteps=5, turningsteps=4, speed=10;
  char z;

  /* allocate handels for all servos */
  for(i=0;i<=12;i++) servohandles[i]=SERVOInit(semas[i]);  

  LCDPrintf("\n6 legged robot Demo\n");
  LCDMenu("For","Lef","Rig","End");
  set_joint(PosInit);

  do{ switch (z=KEYGet())
      { case KEY1: move_joint(PosInit, GaitForward[0], speed);
                   for (k=0; k<walksteps; k++)
                     gait(GaitForward, GaitForwardSize, speed);
                   move_joint(GaitForward[0], PosInit, speed);
                   break;
	
	case KEY2:move_joint(PosInit, GaitLeft[0], speed);
		  for (k=0; k<turningsteps; k++)
	            gait(GaitLeft, GaitTurnLeftSize, speed);
		  move_joint(GaitLeft[0], PosInit,speed); 
                  break;
	
	case KEY3:move_joint(PosInit, GaitRight[0], speed);
                  for (k=0; k<turningsteps; k++)
	            gait(GaitRight, GaitTurnRightSize, speed);
		  move_joint(GaitRight[0], PosInit,speed); 
                  break;

	
	default:   break;
     }
  } while (z!=KEY4);

  for(i=1;i<=12;i++) SERVORelease(servohandles[i]);
  PSDRelease();
  return 0;
}