/* This code implements the walking algorithm described in the Actuation       section of this paper.  It uses a single array to represent the 12 servos.  The values assigned to each servos represent a pulse width, this number is directly related to the position of the servo in degrees. */


#define DELAY 5500
#define IRLATCH *(unsigned char*)(0x7000)

#include <hc11.h>
#include <mil.h>
#include <analog.h>
#include <vectors.h>
#include <serial.h>

/* currpos holds the current position of the servos */
char cp[12];

void servoit(int sn, int pw);
void doservos(void);
void waitabit(int waittime);
doit(int waittime);
void zeroservos(void);

void backwards(void);
void turn( int m );
void setup( void );


int main(void)
{
 int i, j, lval, rval;
 init_serial();
 zeroservos(); 
 init_analog();
 IRLATCH = 0xFF;

 /* first set the new starting positions */
 setup();
 j = 0;
 while(1)
 {

  lval = analog(6);
  rval = analog(7);

  if( lval > 100 && rval > 100 )
     {
       setup();
       backwards();
       j++;
       if( j == 2 )
        { turn( 8 ); }
     }

  else if( lval > 100 )
     {
       j = 0;
       turn( 7 );
     }

  else if( rval > 100 )
     {
       j = 0;
       turn( 15 );
     }

  else 
  {
  /* begin right side movement */
  /* first shift the weight on to the left side */

  cp[1] = 58;  /* zero posn for shoulder box */
  doit(DELAY);

  cp[4] = 55; /* lift up to move to zero posn */
  cp[5] = 64;  /* zero posn for shoulder box */
  doit(DELAY);
  cp[4] = 80;  /* return to start posn */
  doit(DELAY);

  cp[7] = 120;  /* zero posn - 20 */
  doit(DELAY);

  /* back right leg movement */
  cp[10] = 45; /* bring leg up (was 40 in test7) */
  doit(DELAY);
  cp[8] = 0;  /* swing shoulder box forward */
  doit(DELAY);

  cp[10] = 100; /* set back right leg more down for movement */
  doit(DELAY);

  cp[8] = 55;  /* swing shoulder box back */
  doit(DELAY);
  
  /* start movement on the left side */
  /* first shift weight to the right side */

  /* cp[8] is already a zero posn */

  cp[11] = 65; /* lift up to move to zero posn */
  cp[7] = 58;  /* zero position for shoulder box */
  doit(DELAY);
  cp[11] = 20; /* return to start posn */
  doit(DELAY);

  /* front left perpendicular for tripod */
  cp[5] = 0;  /* 90 degrees position */
  doit(DELAY);

  /* back left movement (small movements are good) */
  cp[0] = 75;  /* pick leg up */
  cp[1] = 75;  /* swing left rear shoulder forward */
  doit(DELAY);

  cp[0] = 25; /* more down for movement left rear */
  doit(DELAY);
  cp[1] = 70; /* swing shoulder back for movement */
  doit(DELAY);
  }
 }
}

void backwards()
{
int m = 4;

/* begin backwards movement */

 while(m>0)
 {
  setup();
  /* begin right side movement */
  /* first shift the weight on to the left side (bring legs down)*/
  cp[5] = 64;  /* zero posn for shoulder box */
  doit(DELAY);

  cp[0] = 75;  /* lift up to move shoulder box */
  cp[1] = 58;  /* zero posn for shoulder box */
  doit(DELAY);
  cp[0] = 40;  /* return to start posn */
  doit(DELAY);

  /* back leg used in tripod position/ front used to propell */
  cp[8] = 10;  /* zero posn + 10 */
  doit(DELAY);

  /* front right leg movement */
  cp[11] = 75; /* bring leg up */
  doit(DELAY);
  cp[7] = 90;  /*(was 45 for 8) swing shoulder box forward */
  doit(DELAY);

  cp[11] = 20; /* set back right leg more down for movement */
  doit(DELAY);
  cp[7] = 80;  /*(was 55 for 8) swing shoulder box back */
  doit(DELAY);
  
  /* start movement on the left side */
  /* first shift weight to the right side */
  cp[7] = 58;  /* zero position for shoulder box */
  doit(DELAY);

  cp[10] = 45; /* up for shoulder movement */
  cp[8] = 64;  /* zero position for shoulder box */
  doit(DELAY);
  cp[10] = 90; /* back down to start posn */
  doit(DELAY);

  cp[1] = 100;  /* zero position - 20 */
  doit(DELAY);

  /* front left movement (small movements are good) */

  cp[4] = 40;  /*(was 80 for 0) pick leg up */
  doit(DELAY);
  cp[5] = 40;  /* swing left rear shoulder forward */
  doit(DELAY);

  cp[4] = 90; /* more down for movement left rear */
  doit(DELAY);
  cp[5] = 60; /* swing shoulder back for movement */
  doit(DELAY);

  m = m - 1;
 }
}  

void turn( int  h )
{
  backwards();

 while( h > 0 )
  {
     setup();
   cp[1] = 58;
   doit(DELAY);

   /* before turning left side balance the right side with tripod */
   cp[11] = 65;  /* up for shoulder movement */
   cp[7] = 64;
   doit(DELAY);
   cp[11] = 45;  /* down to start */
   doit(DELAY);

   cp[8] = 64;
   doit(DELAY);

   cp[4] = 55; /* up for shoulder movement */
   cp[5] = 0;
   doit(DELAY);
   cp[4] = 75;  /* down to start */
   doit(DELAY);

   cp[1] = 120;
   cp[5] = 64;
   doit(DELAY);

   /* now move right side to turn left */
   cp[7] = 120;
   doit(DELAY);
   cp[10] = 45;
   cp[8] = 25;
   doit(DELAY);
   cp[10] = 75;
   doit(DELAY);

  h = h-1;
  }

}

/* CCW is postitive */
/* 64 is centered, 1 is ~45degrees all the way right,
   127 is all the way left   */
void servoit(int sn, int pw)
{
 put_char(0);
 put_char(sn);
 put_char(pw);
}

void doservos(void)
{
 int i;
 for (i=0;i<12;i++){servoit(i,cp[i]);}
}

void waitabit(int waittime)
{
 int i;
 for (i=1;i<waittime;i++){}
 return;
}
doit(int waittime)
{
 doservos();
 waitabit(waittime);
}
void zeroservos(void)
{
 int i;
 for(i=0;i<=12;i++) {cp[i]=64;}
 doit(2*DELAY);
}
void setup( void )
{
  cp[7] = 100;
  cp[8] = 25;
  cp[5] = 25;
  cp[1] = 100;
  cp[11] = 45;
  cp[10] = 90;
  cp[0] = 40;
  cp[4] = 70;
  cp[9] = 50;
  cp[6] = 58;
  cp[2] = 58;
  cp[3] = 55;
  doit(DELAY);
}