/* ***************************************************************
MODULE:    walker
AUTOR:     Rainer Leonhardt, Uni Stuttgart, 12. May  1997
CHANGES:   Thomas Braunl,    UWA,           30. Oct. 1997
           Thomas Braunl,    UWA,           28. Feb. 2000
FUNCITION: This module contains a accumulation of routines to control the walker
           and always one more routine just to display a menu suitable to the
           current controllroutine of the walker.
basic procedures:
              layer2:
                -ultrasonicLeft and ultrasonicRight are two routines, 
                 each returns a distance-value for it's side. 
                 Parameters are not necessery
                -liftleg is a funktion that moves a leg of the 6-legged walker
		 to a specified angle in vertical dimension, in a direction, 
                 independent on which side or position it occures.
		 Parameters are the Side, Posiotion and Angle:
                 All are scalar variables
                -movleg is a funktion that moves a leg of the 6-legged walker
                 to a specified angle in horizontal layer, in a direction, 
                 independent on which side or position it occures.
		 Parameters are the Side, Posiotion and Angle: 
                 All are scalar variables
		-CheckUltra is a funktion that checks the left and the right 
                 ultrasound-sensor, and if there is no obstacle in a certain 
                 distance it returns a true value.
	      layer3:
                -oneStep,turnRight,turnLeft,backStep are control-datafields, 
                 necessery for making those steps.
                -make_n_steps is a funktion, that moves the legs in such 
                 sequences that steps are made.
		 Parameters are one of those controll fields, the length
                 (in states-usually 8) and the numer of runs through this 
                 controll field.
		 No Obstaclecheck is made -> Useful for turns
                -make_n_stepsC is a funktion, that moves the legs in such 
                 sequences that steps are made.
		 Parameters are one of those controll fields, the length
                 (in states-usually 8) and the numer of runs through this 
                 controll field.
                 The value returne is the number of runs that could be made,
		 an obstacle in the way can interrupt the whole run.
		-basing on this two items the routines for the layer four are 
                 implemented:
		-beetle_steps, beetle_right, beetle_left, beetle_back,
                 beetle_hi_steps, beetle_hi_right
		 beetle_hi_left, beetle_hi_back
		 There allways exists just one Parameter:
                 The numer of this steps, that is wanted to be made
		-stillstand, stillhistand are procedures, that make the walker
                 stay on his startposition
	      layer4:
	        -gotoxy this routine shall manage that the walker goes from the
                 from-positions to the to-positions (because of the resolution
                 the positions have to be multiplied with 256),
                 orientation can be a value from 0 to 51
demoprocedures: -walker: The Rootmenu to controll the walker. Submenu points:
                 -mov Menu just to check the moving abilities
 	           -go:  Menu for different modes to walk 
	           -leg: Menu to move one leg in a specific way, 
                         i.e. one of the  possible
		         5 positions for the leg can get selected.
	           -svo: Menu to move one servo in degrees
		 -fbl: Menu to check the fumble-sensors
		 -usn: Menu to check the ultrasound



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

#include "eyebot.h"
#include <math.h>
#include <stdlib.h>

#define     FUM_LF              -400
#define     FUM_RF              -401
#define     FUM_LM              -402
#define     FUM_RM              -403
#define     FUM_LR              -404
#define     FUM_RR              -405


char z;
ServoHandle servohandles[12];
PSDHandle psdhandles[2];
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};
int PSDsemas[2]={PSD_FRONTLEFT,PSD_FRONTRIGHT};
int SDsemas[6] ={FUM_LF,FUM_RF,FUM_LM,FUM_RM,FUM_LR,FUM_RR};
                /* Semantics for servos, PSDs and sensing devices */

int x=10,y=10;     /* actual pos. in local area, resoultion is 256 */
int Orientation=0; /* the resolution of a circle is 52 */


/********** begining of layer one **********/
/*What to get of layer one */
int OSGetSDSignal()
/* This routine returns true if a sensordevice exists and is pressed*/
{ return (0);
}


/********** begining of layer two **********/

#define LEFT 0      /* Legselection values*/
#define RIGHT 1

#define FRONT 10
#define MIDDLE 11
#define REAR 12

#define VUP 5       /* Vertical legposition values */
#define UP 4        /* UP position */
#define CN 3        /* CeNtre */
#define DN 2        /* DowN */
#define VDN 1       /* Very DowN */

#define VBK 5       /* Horizontal legposition values */
#define BK 4   /* BacK    */
/* CN 3  CeNtre as in up/down */
#define FN 2
#define VFN 1  /* Very FroNt   */

typedef int Side;
typedef int Position;

/* ultrasonic is a prototype of a routine that returns the distance to an 
   obstacle. There are two routines existant, one for the left side and one 
   for the right side, because there are two ultrasonic-sensors
*/

int ultrasonicLeft(void)
{
  PSDStart(psdhandles[0], FALSE);        /* Start measuringprocess */
  while (!PSDCheck());                   /* value available */
  return(PSDGet(psdhandles[0]));         /* return measured value */
}
int ultrasonicRight(void)
{
  PSDStart(psdhandles[1], FALSE);        /* Start measuringprocess */
  while (!PSDCheck());                   /* value available */
  return(PSDGet(psdhandles[1]));         /* return measured value */
}


/* The routine liftleg shall lift a Leg on the defined side and Position
   to a specified angle
   movleg does the same like liftleg just in the other plane 
   If something went wrong an error is signaled through a one in the 
   return variable
*/

#define KorrFrLeUD 0        /* The bigger the more up */
#define KorrMiLeUD 0
#define KorrReLeUD 0
#define KorrFrRiUD 0        /* The bigger the more down */
#define KorrMiRiUD 0
#define KorrReRiUD 0

#define KorrFrLeFB  255/2      /* The bigger the more backwards */
#define KorrMiLeFB  255/2
#define KorrReLeFB  255/2
#define KorrFrRiFB 255/2      /* The bigger the more foreward */
#define KorrMiRiFB 255/2
#define KorrReRiFB 255/2


int liftleg(Side side, Position position, int hight)
/* if a 0 is returned all is OK */
/* otherwise a 1 is returnde */
{
  if ((hight>5)||(hight<1)) return(1);
  hight=hight*40;
  switch (side)                /* in hight value between 1 and 5 */
    {                          /* can get used: 1 is very lo */
    case LEFT:                 /* 5 is very high */
      switch (position)
	{
	case FRONT:            /* Here one Leg is selected, 160 is */
	  return(SERVOSet(servohandles[0],KorrFrLeUD-hight));
          /* the central angle, respectively */
	  break;               /* the correcture-factor */
	case MIDDLE:
	  return(SERVOSet(servohandles[4],KorrMiLeUD-hight));
	  break;
	case REAR: 
	  return(SERVOSet(servohandles[8],KorrReLeUD-hight));
	  break;
	}
    case RIGHT:
      switch (position)
	{
	case FRONT:
	  return(SERVOSet(servohandles[2],KorrFrRiUD+hight));
	  break;
	case MIDDLE:
	  return(SERVOSet(servohandles[6],KorrMiRiUD+hight));
	  break;
	case REAR: 
	  return(SERVOSet(servohandles[10],KorrReRiUD+hight));
	  break;	
	}
 }
 return(1);
}

int movleg(Side side, Position position, int distance)
/* if a 0 is returned all is OK */
/* otherwise a 1 is returnde */
{
  if ((distance>5)||(distance<1)) return(1);
  distance=distance*10;
  switch (side)                /* like in hight, distance can be */
    {                          /* a value between 1 and 5 */
    case LEFT:                 /* 1 is very rear, 5 is very front, */
      switch (position)        /* 3 is the centre */
	{
	case FRONT:
	  return(SERVOSet(servohandles[1],KorrFrLeFB+distance));
	  break;
	case MIDDLE:
	  return(SERVOSet(servohandles[5],KorrMiLeFB+distance));
	  break;
	case REAR: 
	  return(SERVOSet(servohandles[9],KorrReLeFB+distance));
	  break;
	}
    case RIGHT:
      switch (position)
	{
	case FRONT:
	  return(SERVOSet(servohandles[3],KorrFrRiFB-distance));
	  break;
	case MIDDLE:
	  return(SERVOSet(servohandles[7],KorrMiRiFB-distance));
	  break;
	case REAR: 
	  return(SERVOSet(servohandles[11],KorrReRiFB-distance));
	  break;	
	}
 }
 return(1);
}

int CheckleftUltra(void)
/* This routine returns a true value if no obstacle is in front of left PSD
   It should check the left PSD-sensor and if there is
*/
{
  return(ultrasonicLeft()>120);  /* no obstacle within 30 mm --> return true */
}

int CheckrightUltra(void)
/* This routine returns "true" if no obstacle is in front of the walker
   It should check the right PSD-sensor and if there is
*/
{
  return(ultrasonicRight()>120); /* no obstacle within 30 mm --> return true */
}

int CheckUltra(void)
/* This routine returns a true value if no obstacle is in front of the walker
   It should check the left and the right ultrasound-sensor and if there is
   no obstacle whithin a certain distance it should return a true value
*/ 
{
  return(CheckleftUltra()&&CheckrightUltra());
}



/********** begin of layer three **********/

/* Thesse are the datas for a standard beetle-onestep in layer 3 */
/* The different states of the walker */
int oneStep[8][6][2]= {
    /* q1 move a leg-trio forward */
              {{DN,BK},{CN,FN},
	       {CN,FN},{DN,BK},
	       {DN,BK},{CN,FN}},
    /* q2 change legtrio: on the flor in the air */
              {{DN,BK},{DN,FN},
	       {DN,FN},{DN,BK},
	       {DN,BK},{DN,FN}},
    /* q3 */
              {{CN,BK},{DN,FN},
	       {DN,FN},{CN,BK},
	       {CN,BK},{DN,FN}},
    /* q4 The of q0 inverse state / Step */
              {{CN,CN},{DN,CN},
	       {DN,CN},{CN,CN},
	       {CN,CN},{DN,CN}},
    /* q5 */
              {{CN,FN},{DN,BK},
	       {DN,BK},{CN,FN},
	       {CN,FN},{DN,BK}},
    /* q6 change leg-trio back */
              {{DN,FN},{DN,BK},
	       {DN,BK},{DN,FN},
	       {DN,FN},{DN,BK}},
    /* q7 */
              {{DN,FN},{CN,BK},
	       {CN,BK},{DN,FN},
	       {DN,FN},{CN,BK}},
    /* q0 */
              {{DN,CN},{CN,CN},
	       {CN,CN},{DN,CN},
	       {DN,CN},{CN,CN}}
    /* restart with q1 or not */
};

/* This are the datas for a standard turn-right in layer 3 */
/* The different states of the walker */
int turnRight[8][6][2]= {
    /* q1 move a legtrio forward */
              {{DN,BK},{CN,BK},
	       {CN,FN},{DN,FN},
	       {DN,BK},{CN,BK}},
    /* q2 change legtrio: on the flor in the air */
              {{DN,BK},{DN,BK},
	       {DN,FN},{DN,FN},
	       {DN,BK},{DN,BK}},
    /* q3 */
              {{CN,BK},{DN,BK},
	       {DN,FN},{CN,FN},
	       {CN,BK},{DN,BK}},
    /* q4 The of q0 inverse state / Step */
              {{CN,CN},{DN,CN},
	       {DN,CN},{CN,CN},
	       {CN,CN},{DN,CN}},
    /* q5 */
              {{CN,FN},{DN,FN},
	       {DN,BK},{CN,BK},
	       {CN,FN},{DN,FN}},
    /* q6 change legtrio back*/
              {{DN,FN},{DN,FN},
	       {DN,BK},{DN,BK},
	       {DN,FN},{DN,FN}},
    /* q7 */
              {{DN,FN},{CN,FN},
	       {CN,BK},{DN,BK},
	       {DN,FN},{CN,FN}},
    /* q0 */
              {{DN,CN},{CN,CN},
	       {CN,CN},{DN,CN},
	       {DN,CN},{CN,CN}}
    /* restart with q1 or not */
};

/* These are the datas for a standard turn left in layer 3 */
/* The different states of the walker */
int turnLeft[8][6][2]= {
    /* q1 move a legtrio forward */
              {{DN,FN},{CN,FN},
	       {CN,BK},{DN,BK},
	       {DN,FN},{CN,FN}},
    /* q2 change legtrio: on the flor in the air */
              {{DN,FN},{DN,FN},
	       {DN,BK},{DN,BK},
	       {DN,FN},{DN,FN}},
    /* q3 */
              {{CN,FN},{DN,FN},
	       {DN,BK},{CN,BK},
	       {CN,FN},{DN,FN}},
    /* q4 The of q0 inverse state / Step */
              {{CN,CN},{DN,CN},
	       {DN,CN},{CN,CN},
	       {CN,CN},{DN,CN}},
    /* q5 */
              {{CN,BK},{DN,BK},
	       {DN,FN},{CN,FN},
	       {CN,BK},{DN,BK}},
    /* q6 change legtrio back*/
              {{DN,BK},{DN,BK},
	       {DN,FN},{DN,FN},
	       {DN,BK},{DN,BK}},
    /* q7 */
              {{DN,BK},{CN,BK},
	       {CN,FN},{DN,FN},
	       {DN,BK},{CN,BK}},
    /* q0 */
              {{DN,CN},{CN,CN},
	       {CN,CN},{DN,CN},
	       {DN,CN},{CN,CN}}
    /* restart with q1 or not */
};


/* This are the datas for a beetle-backstep in layer 3 */
/* The different states of the walker */
int backStep[8][6][2]= {
              {{DN,FN},{CN,BK},   /* q1 move a legtrio forward */
	       {CN,BK},{DN,FN},
	       {DN,FN},{CN,BK}},
              {{DN,FN},{DN,BK},   /* q2 change legtrio: on floor in the air */
	       {DN,BK},{DN,FN},
	       {DN,FN},{DN,BK}},
              {{CN,FN},{DN,BK},   /* q3 */
	       {DN,BK},{CN,FN},
	       {CN,FN},{DN,BK}},
              {{CN,CN},{DN,CN},   /* q4 The of q0 inverse state / Step */
	       {DN,CN},{CN,CN},
	       {CN,CN},{DN,CN}},
              {{CN,BK},{DN,FN},   /* q5 */
	       {DN,FN},{CN,BK},
	       {CN,BK},{DN,FN}},
              {{DN,BK},{DN,FN},   /* q6 change legtrio back*/
	       {DN,FN},{DN,BK},
	       {DN,BK},{DN,FN}},
              {{DN,BK},{CN,FN},   /* q7 */
	       {CN,FN},{DN,BK},
	       {DN,BK},{CN,FN}},
              {{DN,CN},{CN,CN},   /* q0 */
	       {CN,CN},{DN,CN},
	       {DN,CN},{CN,CN}}   /* restart with q1 or not */
};


/* This part is important for the beginning to */
int staysolid[1][6][2]= {
              {{DN,CN},{DN,CN},   /* give the walker a solid position */
	       {DN,CN},{DN,CN},
	       {DN,FN},{DN,CN}}
};

/* WARNING: This can break the legs of the old walker*/
/* This are the datas for a high beetle-onestep in layer 3 */
/* The different states of the walker */
/* In this case the walker makes extra large, extra high steps */
int oneHiStep[8][6][2]= {
              {{VDN,VBK},{VUP,VFN},  /* q1 move a legtrio forward */
	       {VUP,VFN},{VDN,VBK},
	       {VDN,VBK},{VUP,VFN}},
              {{VDN,VBK},{VDN,VFN},  /* q2 change legtrio: on floor in air */
	       {VDN,VFN},{VDN,VBK},
	       {VDN,VBK},{VDN,VFN}},
              {{VUP,VBK},{VDN,VFN},  /* q3 */
	       {VDN,VFN},{VUP,VBK},
	       {VUP,VBK},{VDN,VFN}},
              {{VUP,CN},{VDN,CN},    /* q4 The of q0 inverse state / Step */
	       {VDN,CN},{VUP,CN},
	       {VUP,CN},{VDN,CN}},
              {{VUP,VFN},{VDN,VBK},  /* q5 */
	       {VDN,VBK},{VUP,VFN},
	       {VUP,VFN},{VDN,VBK}},
              {{VDN,VFN},{VDN,VBK},  /* q6 change legtrio back*/
	       {VDN,VBK},{VDN,VFN},
	       {VDN,VFN},{VDN,VBK}},
              {{VDN,VFN},{VUP,VBK},  /* q7 */
	       {VUP,VBK},{VDN,VFN},
	       {VDN,VFN},{VUP,VBK}},
              {{VDN,CN},{VUP,CN},    /* q0 */
	       {VUP,CN},{VDN,CN},
	       {VDN,CN},{VUP,CN}}    /* restart with q1 or not */
};

/* The different states of the walker */
int turnHiRight[8][6][2]= {
              {{VDN,BK},{DN,BK},     /* q1 move a legtrio forward */
	       {DN,FN},{VDN,FN},
	       {VDN,BK},{DN,BK}},
              {{VDN,BK},{VDN,BK},    /* q2 change legtrio: on floor in air */
	       {VDN,FN},{VDN,FN},
	       {VDN,BK},{VDN,BK}},
              {{DN,BK},{VDN,BK},     /* q3 */
	       {VDN,FN},{DN,FN},
	       {DN,BK},{VDN,BK}},
              {{DN,CN},{VDN,CN},     /* q4 The of q0 inverse state / Step */
	       {VDN,CN},{DN,CN},
	       {DN,CN},{VDN,CN}},
              {{DN,FN},{VDN,FN},     /* q5 */
	       {VDN,BK},{DN,BK},
	       {DN,FN},{VDN,FN}},
              {{VDN,FN},{VDN,FN},    /* q6 change legtrio back*/
	       {VDN,BK},{VDN,BK},
	       {VDN,FN},{VDN,FN}},
              {{VDN,FN},{DN,FN},     /* q7 */
	       {DN,BK},{VDN,BK},
	       {VDN,FN},{DN,FN}},
              {{VDN,CN},{DN,CN},     /* q0 */
	       {DN,CN},{VDN,CN},
	       {VDN,CN},{DN,CN}}     /* restart with q1 or not */
};

/* The different states of the walker */
int turnHiLeft[8][6][2]= {
              {{VDN,FN},{DN,FN},     /* q1 move a legtrio forward */
	       {DN,BK},{VDN,BK},
	       {VDN,FN},{DN,FN}},
              {{VDN,FN},{VDN,FN},    /* q2 change legtrio: on floor in air */
	       {VDN,BK},{VDN,BK},
	       {VDN,FN},{VDN,FN}},
              {{DN,FN},{VDN,FN},     /* q3 */
	       {VDN,BK},{DN,BK},
	       {DN,FN},{VDN,FN}},
              {{DN,CN},{VDN,CN},     /* q4 The of q0 inverse state / Step */
	       {VDN,CN},{DN,CN},
	       {DN,CN},{VDN,CN}},
              {{DN,BK},{VDN,BK},     /* q5 */
	       {VDN,FN},{DN,FN},
	       {DN,BK},{VDN,BK}},
              {{VDN,BK},{VDN,BK},    /* q6 change legtrio back*/
	       {VDN,FN},{VDN,FN},
	       {VDN,BK},{VDN,BK}},
              {{VDN,BK},{DN,BK},     /* q7 */
	       {DN,FN},{VDN,FN},
	       {VDN,BK},{DN,BK}},
              {{VDN,CN},{DN,CN},     /* q0 */
	       {DN,CN},{VDN,CN},
	       {VDN,CN},{DN,CN}}     /* restart with q1 or not */
};

/* The different states of the walker */
int backHiStep[8][6][2]= {
              {{VDN,FN},{DN,BK},    /* q1 move a legtrio forward */
	       {DN,BK},{VDN,FN},
	       {VDN,FN},{DN,BK}},
              {{VDN,FN},{VDN,BK},   /* q2 change legtrio: on floor in air */
	       {VDN,BK},{VDN,FN},
	       {VDN,FN},{VDN,BK}},
              {{DN,FN},{VDN,BK},    /* q3 */
	       {VDN,BK},{DN,FN},
	       {DN,FN},{VDN,BK}},
              {{DN,CN},{VDN,CN},    /* q4 The of q0 inverse state / Step */
	       {VDN,CN},{DN,CN},
	       {DN,CN},{VDN,CN}},
              {{DN,BK},{VDN,FN},    /* q5 */
	       {VDN,FN},{CN,BK},
	       {DN,BK},{VDN,FN}},
              {{VDN,BK},{VDN,FN},   /* q6 change legtrio back*/
	       {VDN,FN},{VDN,BK},
	       {VDN,BK},{VDN,FN}},
              {{VDN,BK},{DN,FN},    /* q7 */
	       {DN,FN},{VDN,BK},
	       {VDN,BK},{DN,FN}},
              {{VDN,CN},{DN,CN},    /* q0 */
	       {DN,CN},{VDN,CN},
	       {VDN,CN},{DN,CN}}    /* restart with q1 or not */
};

/* This part is important for the beginning to */
int stayHisolid[1][6][2]= {
              {{VDN,CN},{VDN,CN},   /* give the walker a solid position */
	       {VDN,CN},{VDN,CN},
	       {VDN,FN},{VDN,CN}}
};


/* n is the number of loops over the control array */
/* k is the number of states per control array */
int make_n_steps(int movdata[][6][2],int k,int n)
{
  int i,state;
  for(i=0;i<n;i++)
    {
      for(state=0;state<k;state++)
	{
      	  liftleg(LEFT, FRONT, movdata[state][0][0]);
	  liftleg(RIGHT,FRONT, movdata[state][1][0]);
	  liftleg(LEFT, MIDDLE,movdata[state][2][0]);
	  liftleg(RIGHT,MIDDLE,movdata[state][3][0]);
	  liftleg(LEFT, REAR,  movdata[state][4][0]);
	  liftleg(RIGHT,REAR,  movdata[state][5][0]);
      	  movleg(LEFT, FRONT, movdata[state][0][1]);
	  movleg(RIGHT,FRONT, movdata[state][1][1]);
	  movleg(LEFT, MIDDLE,movdata[state][2][1]);
	  movleg(RIGHT,MIDDLE,movdata[state][3][1]);
	  movleg(LEFT, REAR,  movdata[state][4][1]);
	  movleg(RIGHT,REAR,  movdata[state][5][1]);
	  OSWait(10);
	}
    }
  return(i);
}


/* n is the number of loops over the control array */
/* k is the number of states per control array */
int make_n_stepsC(int movdata[][6][2],int k,int n)
{
  int i,state;
  for(i=0;((i<n)&&(CheckUltra()));i++)
    {
      for(state=0;state<k;state++)
	{
      	  liftleg(LEFT, FRONT, movdata[state][0][0]);
	  liftleg(RIGHT,FRONT, movdata[state][1][0]);
	  liftleg(LEFT, MIDDLE,movdata[state][2][0]);
	  liftleg(RIGHT,MIDDLE,movdata[state][3][0]);
	  liftleg(LEFT, REAR,  movdata[state][4][0]);
	  liftleg(RIGHT,REAR,  movdata[state][5][0]);
      	  movleg(LEFT, FRONT, movdata[state][0][1]);
	  movleg(RIGHT,FRONT, movdata[state][1][1]);
	  movleg(LEFT, MIDDLE,movdata[state][2][1]);
	  movleg(RIGHT,MIDDLE,movdata[state][3][1]);
	  movleg(LEFT, REAR,  movdata[state][4][1]);
	  movleg(RIGHT,REAR,  movdata[state][5][1]);
	  OSWait(10);
	}
    }
  return(i);
}

/* n is the number of loops over the control array */
/* k is the number of states per control array */
void make_n_creeps(int movdata[][6][2],int k,int n,int speed) 
{
  int i,state;         /* this routine exists because of to agressive */
  for(i=1;i<=n;i++)    /* movement of the walker, it lost a leg during */
    {                  /* the first steps */
      for(state=0;state<k;state++)    /* speed is slow-val, greater speed is */
	{                             /* the slower the walker moves */
/*   Not ready yet
          liftlegw(LEFT, FRONT, movdata[state][0][0],speed);
	  liftlegw(RIGHT,FRONT, movdata[state][1][0],speed);
	  liftlegw(LEFT, MIDDLE,movdata[state][2][0],speed);
	  liftlegw(RIGHT,MIDDLE,movdata[state][3][0],speed);
	  liftlegw(LEFT, REAR,  movdata[state][4][0],speed);
	  liftlegw(RIGHT,REAR,  movdata[state][5][0],speed);
      	  movlegw(LEFT, FRONT, movdata[state][0][1],speed);
	  movlegw(RIGHT,FRONT, movdata[state][1][1],speed);
	  movlegw(LEFT, MIDDLE,movdata[state][2][1],speed);
	  movlegw(RIGHT,MIDDLE,movdata[state][3][1],speed);
	  movlegw(LEFT, REAR,  movdata[state][4][1],speed);
	  movlegw(RIGHT,REAR,  movdata[state][5][1],speed);
*/
	}
    }
}


/* n steps in the mode of a standard beetle */
/* a standardfunktion for the upper LAYER 3 interface */
int beetle_steps(int n)
{
  return(make_n_stepsC(oneStep,8,n));
}


/* n values turned right in the mode of a standard beetle */
/* a standardfunktion for the upper LAYER 3 interface */
int beetle_right(int n)
{
  return(make_n_steps(turnRight,8,n));
}


/* n values turned left in the mode of a standard beetle */
/* a standardfunktion for the upper LAYER 3 interface */
int beetle_left(int n)
{
  return(make_n_steps(turnLeft,8,n));
}


/* n backsteps in the mode of a standard beetle */
/* a standardfunktion for the upper LAYER 3 interface */
int beetle_back(int n)
{
  return(make_n_steps(backStep,8,n));
  /* Warning: In the back there exists no sensor */
}


/* just stay on your position with all legs in the same mode*/
/* a standardfunktion for the upper LAYER 3 interface */
int stillstand(void)
{
  return(make_n_steps(staysolid,1,1));
}


/* n steps in the mode of a standard beetle on plato-shoes*/
/* a standardfunktion for the upper LAYER 3 interface */
int beetle_hi_steps(int n)
{
  return(make_n_stepsC(oneHiStep,8,n));
}


/* n values turned right in the mode of a standard beetle on pla-*/
/* to-shoes a standardfunktion for the upper LAYER 3 interface */
int beetle_hi_right(int n)
{
  return(make_n_steps(turnHiRight,8,n));
}

/* n values turned left in the mode of a standard beetle on pla-*/
/* to-shoes a standardfunktion for the upper LAYER 3 interface */
int beetle_hi_left(int n)
{                           
  return(make_n_steps(turnHiLeft,8,n));
}


/* n backsteps in the mode of a standard beetle on pla-*/
/* to-shoes a standardfunktion for the upper LAYER 3 interface */
int beetle_hi_back(int n)
{
  return(make_n_steps(backHiStep,8,n));
  /* Warning: In the back there exists no sensor */
}


/* just stay on your position with all legs in hi mode*/
/* a standardfunktion for the upper LAYER 3 interface */
int stillhistand(void)
{
  return(make_n_steps(stayHisolid,1,1));
}



/********** begining of layer four **********/

int Quadrant1[13][2]=
 {{0,256},{-31,254},{-61,249},{-91,239},{-119,227},{-145,211},{-170,192},
  {-192,170},{-211,145},{-227,119},{-239,91},{-249,61},{-254,31}};
int Quadrant2[13][2]=
 {{-256,0},{-254,-31},{-249,-61},{-239,-91},{-227,-119},{-211,-145},
  {-192,-170},{-170,-192},{-145,-211},{-119,-227},{-91,-239},{-61,-249},
  {-31,-254}};
int Quadrant3[13][2]=
 {{0,-256},{31,-254},{61,-249},{91,-239},{119,-227},{145,-211},{170,-192},
  {192,-170},{211,-145},{227,-119},{239,-91},{249,-61},{254,-31}};
int Quadrant4[13][2]=
 {{256,0},{254,31},{249,61},{239,91},{227,119},{211,145},{192,170},{170,192},
  {145,211},{119,227},{91,239},{61,249},{31,254}};


/* This procedure returns the quadrant in what the vector points */
int selquad(int fromX,int fromY,int toX,int toY)
{
  if ((toX<=fromX)&&(toY>fromY)) return(1);
  if ((toX<fromX)&&(toY<=fromY)) return(2);
  if ((toX>=fromX)&&(toY<fromY)) return(3);
  if ((toX>fromX)&&(toY>=fromY)) return(4);
  return(0);
}

int sqr(int x)
{
  return(x*x);
}


/* This procedure returns the direction in what the walker has */
/* to go. The direction can be a number from 0 to 51 in the */
int seldir(int fromX,int fromY,int toX,int toY)
{
  int direction, Xnorm, Ynorm;       /* counter-clockwise. */
  Xnorm=((toX-fromX)*256)/sqrt(sqr(toX-fromX)+sqr(toY-fromY));
  Ynorm=((toY-fromY)*256)/sqrt(sqr(toX-fromX)+sqr(toY-fromY));
  direction=selquad(fromX,fromY,toX,toY);
  switch(direction)
    {
    case 1:
      for(direction=0;(((Xnorm<Quadrant1[direction][0])&&
                       (Ynorm<Quadrant1[direction][1]))&&(direction<12));
          direction++);
      return(direction);
    case 2:
      for(direction=0;(((Xnorm>Quadrant2[direction][0])&&
                       (Ynorm<Quadrant2[direction][1]))&&(direction<12));
          direction++);
      return(direction+13);
    case 3:
      for(direction=0;(((Xnorm>Quadrant3[direction][0])&&
                       (Ynorm>Quadrant3[direction][1]))&&(direction<12));
          direction++);
      return(direction+26);
    case 4:
      for(direction=0;(((Xnorm<Quadrant4[direction][0])&&
                       (Ynorm>Quadrant4[direction][1]))&&(direction<12));
          direction++);
      return(direction+39);
    default:
      return(52);
    }
}


/* info information */
void info(int toX, int toY,int toorient)
{
      LCDClear();       
      LCDMenu(" + "," - ","Nxt","END");
      LCDPrintf("Level 4: Info\n");
      LCDPrintf("x*256: %d\n",x);
      LCDPrintf("y*256: %d\n",y);
      LCDPrintf("toX*256: %d\n",toX);
      LCDPrintf("toY*256: %d\n",toY);
      LCDPrintf("Orientation: %d\n",Orientation);
      LCDPrintf("toorient: %d\n",toorient);
}

/* Go stright to the aim, if you can or */
/* The quants for one step are 256 */
void gotoxy(int toX,int toY)
{
  int toorient, stepstomake, stepsmade;
  x=x*256;y=y*256;toX=toX*256;toY=toY*256;
  while((abs(toX-x)>=256)||(abs(toY-y)>=256))
    {
      toorient=seldir(x,y,toX,toY);  /* turn the walker to the right */
      info(toX,toY,toorient);      /* STATUSINFORMATION */

      if (toorient>=Orientation)     /* direction */
	{
	  if ((toorient-Orientation)<=26) 
	    {
	      LCDPrintf("b_hi_left(%d)\n",toorient-Orientation); /* STATUS */
	      beetle_hi_left(toorient-Orientation);
	    }
	  else
	    {
	      LCDPrintf("b_hi_right(%d)\n",Orientation+52-toorient); /* STATUS */
	      beetle_hi_right(Orientation+52-toorient);
	    }
	}
      else
	{
	  if ((Orientation-toorient)<=26)
	    {
	      LCDPrintf("b_hi_right(%d)\n",Orientation-toorient);    /* STATUS */
	      beetle_hi_right(Orientation-toorient);
	    }
	  else
	    {
	      LCDPrintf("b_hi_left(%d)\n",toorient+52-Orientation);  /* STATUS */
	      beetle_hi_left(toorient+52-Orientation);
	    }
	}
      Orientation=toorient;        /* direction end */
      info(toX,toY,toorient);    /* STATUS */
      
      stepstomake=((sqrt(sqr(toX-x)+sqr(toY-y)))/256);
      stepsmade=beetle_hi_steps(stepstomake);   /* go to the aim */
      
      if (Orientation<13)     /* Correct chronological position to actual */
	{
	  x=x+stepsmade*Quadrant1[Orientation][0];
	  y=y+stepsmade*Quadrant1[Orientation][1];
	}
      else if ((Orientation>=13)&&(Orientation<26))
	{
	  x=x+stepsmade*Quadrant2[Orientation-13][0];
	  y=y+stepsmade*Quadrant2[Orientation-13][1];
	}
      else if ((Orientation>=26)&&(Orientation<39))
	{
	  x=x+stepsmade*Quadrant3[Orientation-26][0];
	  y=y+stepsmade*Quadrant3[Orientation-26][1];
	}
      else if ((Orientation>=39)&&(Orientation<52))
	{
	  x=x+stepsmade*Quadrant4[Orientation-39][0];
	  y=y+stepsmade*Quadrant4[Orientation-39][1];
	}                 /* gone to aim and computed new position */

      info(toX,toY,toorient);     /* STATUS */

      if (stepstomake!=stepsmade)   /* if the walker stopped at an obstacle */
	{                           /* try to override obstacle - or bypass */
        if (ultrasonicLeft()>ultrasonicRight())
           /* From here: Go around an obstacle, if it is not overridable */
	  { /* walk left around */
	    beetle_hi_left(13);
	    Orientation=((Orientation+13) % 52);
	  }
	else 
	  { /* walk right around */
	    beetle_hi_right(13);
	    Orientation=((Orientation+52-13) % 52);
	  }
	stepsmade=beetle_hi_steps(10);
	if (Orientation<13)   /* Correct chronological position to actual */
	  {
	    x=x+stepsmade*Quadrant1[Orientation][0];
	    y=y+stepsmade*Quadrant1[Orientation][1];
	  }
	else if ((Orientation>=13)&&(Orientation<26))
	  {
	    x=x+stepsmade*Quadrant2[Orientation-13][0];
	    y=y+stepsmade*Quadrant2[Orientation-13][1];
	  }
	else if ((Orientation>=26)&&(Orientation<39))
	  {
	    x=x+stepsmade*Quadrant3[Orientation-26][0];
	    y=y+stepsmade*Quadrant3[Orientation-26][1];
	  }
	else if ((Orientation>=39)&&(Orientation<52))
	  {
	    x=x+stepsmade*Quadrant4[Orientation-39][0];
	    y=y+stepsmade*Quadrant4[Orientation-39][1];
	  }      /* gone to aim an computed new position */
	}
	
    }
  x=x/256;
  y=y/256;
  /* Here global variables are saved: x, y, Orientation */
}

/********** end of layer four **********/



/*
The level4 demonstrates the Layer4 Funktions, i.e. moving of the walker to 
a specified position
*/
int lv4menu(int fromx, int fromy, int tox, int toy, int pos)
{ 
  LCDClear();
  LCDMenu(" + "," - ","Nxt","END");
  LCDPrintf("Level 4\n");
  LCDPrintf("\n");
  LCDPrintf("Actual X: %d\n",fromx); 
  LCDPrintf("Actual Y: %d\n",fromy);
  LCDPrintf("Dest   X: %d\n",tox);
  LCDPrintf("Dest   Y: %d\n",toy);
  LCDPrintf("Start walking\n");
  LCDSetChar(3+pos,15,'*');
  return(0);
} 


/* @@ int i,state; */
int lv4(void)
{
  int tox=x,toy=y,next=0;
  lv4menu(x,y,tox,toy,next);
  while (1==1)
    {
      z = KEYGet();
      switch (z)
	{
	case KEY1:
	  switch (next)
	    {
	    case 0:
	      tox++;
	      break;
	    case 1:
	      toy++;
	      break;
	    }
	  lv4menu(x,y,tox,toy,next);
	  break;
	case KEY2:
	  switch (next)
	    {
	    case 0:
	      tox--;
	      break;
	    case 1:
	      toy--;
	      break;
	    }
	  lv4menu(x,y,tox,toy,next);
	  break;
	case KEY3:
	  if (next==1) 
	    {
	      next=2;
	      lv4menu(x,y,tox,toy,next);
	      gotoxy(tox,toy);
	      next=0;
	    }
	  else next++;
	  lv4menu(x,y,tox,toy,next);
	  break;
	case KEY4:
	  return(0);
	  break;
	default:
	  return(1);
	  break;
	}
    }
}


/*
The gopart demonstrates the Layer3 Funktions, i.e. special moving functions 
of the walker are shown
*/
int gomenu(void)
{ 
  LCDClear();
  LCDMenu("BTL","TRN","LV4","END");
  LCDPrintf("\n");
  LCDPrintf("GOmenu\n");
  LCDPrintf("\n"); 
  LCDPrintf("-Walk like a\n");
  LCDPrintf("beetle,\n");
  LCDPrintf("-turn around or\n");
  LCDPrintf("-goto layer 4.\n");
  return(0);
} 


/* @@ int i,state; */
int go(void)
{
  gomenu();
  while (1==1)
    {
      z = KEYGet();
      switch (z)
	{
	case KEY1:
	  beetle_hi_steps(8);          /*beetle_steps(8);*/
	  gomenu();
	  break;
	case KEY2:
	  beetle_hi_right(8);
	  gomenu();
	  break;
	case KEY3:
	  lv4();
	  gomenu();
	  break;
	case KEY4:
	  return(0);
	  break;
	default:
	  return(1);
	  break;
	}
    }
}


/*
The legpart demonstrates the Layer2 Funktions, i.e. the Legs can get 
controlled in a state-oriented mode
*/

int legmenu(int Side, int Position, int Vert, int Horiz, int pos)
/* pos refers to the position on the screen  */
/* Vert is the vertikal movevalue of the leg */
/* Horiz is the horizontal movevalue of the leg */
/* Side is the side of the walker */
/* Position is the layer of legpairs expl. front*/
{
  LCDClear();                      
  LCDMenu(" + "," - ","Nxt","END");   
  LCDPrintf("\n");                     
  LCDPrintf("Legmenu\n");
  LCDPrintf("\n");
  LCDPrintf("Side: %s\n",(Side?"right":"left"));
  LCDPrintf("Position: %s\n",
         ((Position==FRONT)?"frn":((Position==MIDDLE)?"mid":"bak")));
  LCDPrintf("Vertikal: %d\n",Vert);
  LCDPrintf("Horizontal: %d\n",Horiz);
  LCDSetChar(2+pos,15,'*');
  return(0);
}


int leg(void)
{
  int pos;
  int Side;
  int Position;
  int Values[REAR-FRONT+1][RIGHT-LEFT+1][2]=
      {{{3,3},{3,3}},{{3,3},{3,3}},{{3,3},{3,3}}};
      /* Central Position for all hinges */
  pos=0;
  Side=LEFT;
  Position=FRONT;
  legmenu(Side,Position,Values[Position-FRONT][Side][0],
          Values[Position-FRONT][Side][1],pos);
  while (1==1)
    {
      z = KEYGet();
      switch (z)
	{
	case KEY1:
	  switch (pos)
	    {
	    case 0:
	      Side=1-Side;
	      break;
	    case 1:
	      if (Position == REAR) Position = FRONT; 
	      else Position++;
	      break;
	    case 2:
	      if (Values[Position-FRONT][Side][0]==5)
                 Values[Position-FRONT][Side][0]=1;
	      else Values[Position-FRONT][Side][0]++;
	      liftleg(Side,Position,Values[Position-FRONT][Side][0]);
	      break;
	    case 3:
	      if (Values[Position-FRONT][Side][1]==5)
                Values[Position-FRONT][Side][1]=1;
	      else Values[Position-FRONT][Side][1]++;
	      movleg(Side,Position,Values[Position-FRONT][Side][1]);
	      break;
	    default:
	      break;
	    }
	  legmenu(Side,Position,Values[Position-FRONT][Side][0],
                  Values[Position-FRONT][Side][1],pos);
	  break;
	case KEY2:
	  switch (pos)
	    {
	    case 0:
	      Side=1-Side;
	      break;
	    case 1:
	      if (Position == FRONT) Position = REAR; 
	      else Position--;
	      break;
	    case 2:
	      if (Values[Position-FRONT][Side][0]==1)
                Values[Position-FRONT][Side][0]=5;
	      else Values[Position-FRONT][Side][0]--;
	      liftleg(Side,Position,Values[Position-FRONT][Side][0]);
	      break;
	    case 3:
	      if (Values[Position-FRONT][Side][1]==1)
                Values[Position-FRONT][Side][1]=5;
	      else Values[Position-FRONT][Side][1]--;
	      movleg(Side,Position,Values[Position-FRONT][Side][1]);
	      break;
	    default:
	      break;
	    }
	  legmenu(Side,Position,Values[Position-FRONT][Side][0],
                  Values[Position-FRONT][Side][1],pos);
	  break;
	case KEY3:
	  if (pos==3) pos=0;
	  else pos++;
	  legmenu(Side,Position,Values[Position-FRONT][Side][0],
                  Values[Position-FRONT][Side][1],pos);
	  break;
	case KEY4:
	  return(0);
	  break;
	default:
	  break;
	}
    }
}


/*
The servopart demonstrates the Layer1 Funktions, i.e. each Servo can get 
selected and then the angle installed
*/
int servomenu(int Nr, int Angle, int pos)
{ 
  LCDClear();
  LCDMenu(" + "," - ","Nxt","END");
  LCDPrintf("\n");
  LCDPrintf("Servomenu\n");
  LCDPrintf("\n"); 
  LCDPrintf("Servo No: %d\n",Nr);
  LCDPrintf("\n");
  LCDPrintf("Angle: %d\n",Angle);
  LCDSetChar(3+2*pos,15,'*');
  return(0);
}


int servo(void)
{
  int pos=0;  /* Cursor position on the LCDisplay */
  int Angle[12];
  int Servo_Nr=0;
  for (pos=11;pos>=0;pos--){ Angle[pos]=30; }
  /*Testangles all to central Posision*/
  pos=0;
  servomenu(Servo_Nr,Angle[Servo_Nr],pos);
  while (1==1)
    {
      z = KEYGet();
      switch (z)
	{
	case KEY1:
	  if (!pos) 
	    {
	      if(Servo_Nr==11) Servo_Nr=0;else Servo_Nr++;   /* next servo */
	      SERVOSet(servohandles[Servo_Nr],Angle[Servo_Nr]);
	    }
	  else 
	    {
	      Angle[Servo_Nr]=Angle[Servo_Nr]+10;         /* increase angle */
	      if(Angle[Servo_Nr]>(210)) Angle[Servo_Nr]=210;	
	      SERVOSet(servohandles[Servo_Nr],Angle[Servo_Nr]);
	    }
	  servomenu(Servo_Nr,Angle[Servo_Nr],pos);
	  break;
	case KEY2:
	  if (!pos) 
	    {
	      if(!Servo_Nr) Servo_Nr=11;else Servo_Nr--;  /* last servo */
	      SERVOSet(servohandles[Servo_Nr],Angle[Servo_Nr]);
	    }
	  else
	    {
	      Angle[Servo_Nr]=Angle[Servo_Nr]-10;         /* decrease angle */
	      if (Angle[Servo_Nr]<0) Angle[Servo_Nr]=0;
	      SERVOSet(servohandles[Servo_Nr],Angle[Servo_Nr]);
	    }
	  servomenu(Servo_Nr,Angle[Servo_Nr],pos);
	  break;
	case KEY3:
	  if (pos==1) pos=0;
	  else pos++;
	  servomenu(Servo_Nr,Angle[Servo_Nr],pos);
	  break;
	case KEY4:
	  return(0);
	  break;
	default:
	  break;
	}
    }
}


/*
this is the main menu for the servo-part i.e. the part where something is 
moving
*/
int movemenu(void)
{ 
  LCDClear();
  LCDMenu("GO ","LEG","SVO","END");
  LCDPrintf("\n");
  LCDPrintf("Movemenu\n");
  LCDPrintf("\n"); 
  LCDPrintf("Please select\n");
  LCDPrintf("one of the fol-\n");
  LCDPrintf("lowing menue\n");
  LCDPrintf("points\n");
  return(0);
}


int move(void)
{
  movemenu();
  while (1==1)
    {
      z = KEYGet();
      switch (z)
	{
	case KEY1:
	  go();
	  movemenu();
	  break;
	case KEY2:
	  leg();
	  movemenu();
	  break;	
	case KEY3:
	  servo();
	  movemenu();
  	  break;
	case KEY4:
	  return(0);
	  break;
	default:
	  break;
	}
    }
}


/*
this ist the menu for the fumble, i.e. the fumble outputs can get checked here
*/
int fumblemenu(int number,int value)
{ 
  /*  LCDClear();*/
  LCDMenu(" + "," - ","R   ","END");
  LCDPrintf("\n");
  LCDPrintf("Fumblemenu\n");
  LCDPrintf("\n"); 
  LCDPrintf("Acutal fumlbe\n");
  LCDPrintf("is Number: %d\n",number);
  LCDPrintf("The value of it\n");
  LCDPrintf("is: %d\n",value);
  return(0);
}


int fumble(void)
{
  int number;
  number = 0;
  fumblemenu(number,0);
  while (1==1)
    {
      z = KEYGet();
      switch (z)
	{
	case KEY1:
	  if (number == 5) number=0;
	  else number++;
       	  fumblemenu(number,OSGetSDSignal(SDsemas[number]));
	  break;
	case KEY2:
	  if (number == 0) number=5;
	  else number--;
  	  fumblemenu(number,OSGetSDSignal(SDsemas[number]));
	  break;	
	case KEY3:
 	  fumblemenu(number,OSGetSDSignal(SDsemas[number]));
  	  break;
	case KEY4:
	  return(0);
	  break;
	default:
	  break;
	}
    }
}


/*
this ist the menu for the ultra-sound
*/
int ultrasoundmenu(int number,int value)
{ 
  LCDClear();
  LCDMenu(" + "," - ","R   ","END");
  LCDPrintf("\n");
  LCDPrintf("PSD-menu\n");
  LCDPrintf("\n"); 
  LCDPrintf("Acutal detector\n");
  LCDPrintf("is %s one\n",((!number)?"left":"right"));
  LCDPrintf("The value of it\n");
  LCDPrintf("is: %d\n",value);
  return(0);
}


int ultrasound(void)
{
  int number;
  number = 0;
  ultrasoundmenu(number,0);
  while (1==1)
    {
      z = KEYGet();
      switch (z)
      {
       case KEY1:
        if (number == 1) number=0;
        else number=1;
        ultrasoundmenu(number,((number==0)?ultrasonicLeft():ultrasonicRight()));
        break;
       case KEY2:
	if (number == 0) number=1;
	else number=0;
  	ultrasoundmenu(number,((number==0)?ultrasonicLeft():ultrasonicRight()));
	break;	
       case KEY3:
        ultrasoundmenu(number,((number==0)?ultrasonicLeft():ultrasonicRight()));
        break;
       case KEY4:
        return(0);
	break;
       default:
	break;
      }
    }
}


/*
This ist the main menu of this demonstration
*/
int walkermenu(void)
{ 
  LCDClear();
  LCDMenu("MOV","FBL","USN","END");
  LCDPrintf("\n");
  LCDPrintf("Walkermenu\n");
  LCDPrintf("\n"); 
  LCDPrintf("Please select\n");
  LCDPrintf("one of the fol-\n");
  LCDPrintf("lowing menue\n");
  LCDPrintf("points\n");
  return(0);
}



/************* MAIN ***************** */
/* main walker program                */
int main ()
{
  int i;

  /* initialization */
  LCDPrintf("Init Walker..\n");

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

  /* allocate handels for all psds */
  for(i=0;i<=1;i++) psdhandles[i]=PSDInit(PSDsemas[i]);

  /* middle position */
  LCDPrintf("Servos mid-pos..\n");
  for(i=0;i<=12;i++) SERVOSet(servohandles[i],255/2);
  LCDMenu("GO"," "," "," ");
  KEYWait(KEY1);

  walkermenu();
  while (1==1)
    {
      z = KEYGet();
      switch (z)
	{
	case KEY1:
	  move();
	  walkermenu();
	  break;
	case KEY2:
	  fumble();
	  walkermenu();
	  break;	
	case KEY3:
	  ultrasound();
	  walkermenu();
  	  break;
	case KEY4:
	  for(i=1;i<=12;i++) SERVORelease(servohandles[i]);
	  PSDRelease();
	  return 0;
	  break;
	default:
	  break;
	}
    }
}