extern unsigned char ByteMotorPos[24];
extern bit TimerExpired;
void SetTimer(unsigned int milliseconds);
unsigned int ReadSensors(void);
int t = 1000;
//int choice;
int start=0;

enum MotorNumbers {
   LEFT_FRONT_SHOULDER = 0,
   LEFT_FRONT_UPPERLEG,
   LEFT_FRONT_LOWERLEG,
   LEFT_MIDDLE_SHOULDER,
   LEFT_MIDDLE_UPPERLEG,
   LEFT_MIDDLE_LOWERLEG,
   LEFT_BACK_SHOULDER,
   LEFT_BACK_UPPERLEG,
   LEFT_BACK_LOWERLEG,
   RIGHT_FRONT_SHOULDER,
   RIGHT_FRONT_UPPERLEG,
   RIGHT_FRONT_LOWERLEG,
   RIGHT_MIDDLE_SHOULDER,
   RIGHT_MIDDLE_UPPERLEG,
   RIGHT_MIDDLE_LOWERLEG,
   RIGHT_BACK_SHOULDER,
   RIGHT_BACK_UPPERLEG,
   RIGHT_BACK_LOWERLEG
};

#define SHOULDER_FORWARD    60
#define SHOULDER_ZERO       0
#define SHOULDER_BACKWARD  -90

#define UPPERLEG_UP   120         //100 //130 R mid high up
#define UPPERLEG_DOWN 45       //85

#define LOWERLEG_UP    -30      //-10
#define LOWERLEG_DOWN  -70   //-20

#define MOTOR_SETTINGS(i,zero,dir) MotorZero[i] = zero; MotorDir[i] = (dir>0) ? 1 : -1;
#define SET_MOTOR_POS(i,pos) ByteMotorPos[i] = ((unsigned char)pos)*MotorDir[i] + MotorZero[i];
#define CHECK_SENSOR(i,sensors) (((sensors)>>(16-(i)))&0x01)

unsigned char MotorZero[24] = {128, 128, 128, 128,128,128,128,128,128,128,128,128,128,128,128, 128,128,128,128,128,128,128,128,128};

char          MotorDir[24]  = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

unsigned char LeftStroke = 40, RightStroke = 38;      //35, 35

void
MotorsPosition()
{
MOTOR_SETTINGS(LEFT_FRONT_SHOULDER,130,1);   //110 centre,150
MOTOR_SETTINGS(LEFT_FRONT_UPPERLEG,140,1);
MOTOR_SETTINGS(LEFT_FRONT_LOWERLEG,93,1);
MOTOR_SETTINGS(LEFT_MIDDLE_SHOULDER,112,1);
MOTOR_SETTINGS(LEFT_MIDDLE_UPPERLEG,89,1);
MOTOR_SETTINGS(LEFT_MIDDLE_LOWERLEG,94,1);
MOTOR_SETTINGS(LEFT_BACK_SHOULDER,98,1);   //150
MOTOR_SETTINGS(LEFT_BACK_UPPERLEG,143,1);
MOTOR_SETTINGS(LEFT_BACK_LOWERLEG,97,1);
MOTOR_SETTINGS(RIGHT_FRONT_SHOULDER,120,-1);   //140 centre
MOTOR_SETTINGS(RIGHT_FRONT_UPPERLEG,124,1);
MOTOR_SETTINGS(RIGHT_FRONT_LOWERLEG,100,1);
MOTOR_SETTINGS(RIGHT_MIDDLE_SHOULDER,113,-1);
MOTOR_SETTINGS(RIGHT_MIDDLE_UPPERLEG,117,1);
MOTOR_SETTINGS(RIGHT_MIDDLE_LOWERLEG,155,1);
MOTOR_SETTINGS(RIGHT_BACK_SHOULDER,85,-1);
MOTOR_SETTINGS(RIGHT_BACK_UPPERLEG,59,1);
MOTOR_SETTINGS(RIGHT_BACK_LOWERLEG,105,1);
}

void
ConfigMotors()
{
MOTOR_SETTINGS(LEFT_FRONT_SHOULDER,130,1);  //110 centre,150
MOTOR_SETTINGS(LEFT_FRONT_UPPERLEG,95,1);
MOTOR_SETTINGS(LEFT_FRONT_LOWERLEG,185,1);
MOTOR_SETTINGS(LEFT_MIDDLE_SHOULDER,125,1);
MOTOR_SETTINGS(LEFT_MIDDLE_UPPERLEG,45,1);
MOTOR_SETTINGS(LEFT_MIDDLE_LOWERLEG,190,1);
MOTOR_SETTINGS(LEFT_BACK_SHOULDER,130,1);   //150
MOTOR_SETTINGS(LEFT_BACK_UPPERLEG,110,1);
MOTOR_SETTINGS(LEFT_BACK_LOWERLEG,190,1);
MOTOR_SETTINGS(RIGHT_FRONT_SHOULDER,125,-1);  //140 centre
MOTOR_SETTINGS(RIGHT_FRONT_UPPERLEG,90,1);
MOTOR_SETTINGS(RIGHT_FRONT_LOWERLEG,200,1);
MOTOR_SETTINGS(RIGHT_MIDDLE_SHOULDER,113,-1);
MOTOR_SETTINGS(RIGHT_MIDDLE_UPPERLEG,110,1);
MOTOR_SETTINGS(RIGHT_MIDDLE_LOWERLEG,190,1);
MOTOR_SETTINGS(RIGHT_BACK_SHOULDER,60,-1);
MOTOR_SETTINGS(RIGHT_BACK_UPPERLEG,30,1);
MOTOR_SETTINGS(RIGHT_BACK_LOWERLEG,185,1);
}

void
SetHips(char leftstroke, char rightstroke)
{
// shoulders in walking beam pattern
SET_MOTOR_POS(LEFT_BACK_SHOULDER,leftstroke);
SET_MOTOR_POS(LEFT_FRONT_SHOULDER,leftstroke);
SET_MOTOR_POS(LEFT_MIDDLE_SHOULDER,-leftstroke);
SET_MOTOR_POS(RIGHT_BACK_SHOULDER,-rightstroke);  SET_MOTOR_POS(RIGHT_FRONT_SHOULDER,-rightstroke);  SET_MOTOR_POS(RIGHT_MIDDLE_SHOULDER,rightstroke);
}

void
SetLegs(char upperleg1, char upperleg2,
       char lowerleg1, char lowerleg2)
{
// upper legs
SET_MOTOR_POS(LEFT_BACK_UPPERLEG,upperleg1);
SET_MOTOR_POS(LEFT_FRONT_UPPERLEG,upperleg1);
SET_MOTOR_POS(RIGHT_MIDDLE_UPPERLEG,upperleg1);
SET_MOTOR_POS(RIGHT_BACK_UPPERLEG,upperleg2);
SET_MOTOR_POS(RIGHT_FRONT_UPPERLEG,upperleg2);
SET_MOTOR_POS(LEFT_MIDDLE_UPPERLEG,upperleg2);

// lower legs
SET_MOTOR_POS(LEFT_BACK_LOWERLEG,lowerleg1);
SET_MOTOR_POS(LEFT_FRONT_LOWERLEG,lowerleg1);
SET_MOTOR_POS(RIGHT_MIDDLE_LOWERLEG,lowerleg1);
SET_MOTOR_POS(RIGHT_BACK_LOWERLEG,lowerleg2);
SET_MOTOR_POS(RIGHT_FRONT_LOWERLEG,lowerleg2);
SET_MOTOR_POS(LEFT_MIDDLE_LOWERLEG,lowerleg2);
}

void
Walk(void)
{
  static unsigned char state = 0;
  if (TimerExpired)
  {
   switch (state)
   {
    case 0: // group 2 forward, in air, stroke
     SetHips(LeftStroke, RightStroke);
     SetLegs(UPPERLEG_DOWN,UPPERLEG_UP,LOWERLEG_DOWN,LOWERLEG_UP);
     SetTimer(t);    //50 //100
     break;

    case 1: // group 2 down stroke, don't change forward backward positions
     SetLegs(UPPERLEG_UP,UPPERLEG_DOWN,LOWERLEG_UP,LOWERLEG_DOWN);
     SetTimer(t);    //100 //120
     break;

    case 2: // group 1 forward, in air, stroke
     SetHips(-LeftStroke, -RightStroke);
     SetLegs(UPPERLEG_UP,UPPERLEG_DOWN,LOWERLEG_UP,LOWERLEG_DOWN);
     SetTimer(t);    //100//120
     break;

    case 3: // group 1 down stroke
     SetLegs(UPPERLEG_DOWN,UPPERLEG_UP,LOWERLEG_DOWN,LOWERLEG_UP);
     SetTimer(t);    //100//120
     break;
    }
    state = (state+1)%4;
  }
}

void MoveToZeros() {
     SET_MOTOR_POS(LEFT_FRONT_SHOULDER,0);
     SET_MOTOR_POS(LEFT_FRONT_UPPERLEG,0);
     SET_MOTOR_POS(LEFT_FRONT_LOWERLEG,0);
     SET_MOTOR_POS(LEFT_MIDDLE_SHOULDER,0);
     SET_MOTOR_POS(LEFT_MIDDLE_UPPERLEG,0);
     SET_MOTOR_POS(LEFT_MIDDLE_LOWERLEG,0);
     SET_MOTOR_POS(LEFT_BACK_SHOULDER,0);
     SET_MOTOR_POS(LEFT_BACK_UPPERLEG,0);
     SET_MOTOR_POS(LEFT_BACK_LOWERLEG,0);
     SET_MOTOR_POS(RIGHT_FRONT_SHOULDER,0);
     SET_MOTOR_POS(RIGHT_FRONT_UPPERLEG,0);
     SET_MOTOR_POS(RIGHT_FRONT_LOWERLEG,0);
     SET_MOTOR_POS(RIGHT_MIDDLE_SHOULDER,0);
     SET_MOTOR_POS(RIGHT_MIDDLE_UPPERLEG,0);
     SET_MOTOR_POS(RIGHT_MIDDLE_LOWERLEG,0);
     SET_MOTOR_POS(RIGHT_BACK_SHOULDER,0);
     SET_MOTOR_POS(RIGHT_BACK_UPPERLEG,0);
     SET_MOTOR_POS(RIGHT_BACK_LOWERLEG,0);
}


void
main_racoon(void)
{
unsigned int sensors;
#define SENSOR(i) !(CHECK_SENSOR(i,sensors))


ConfigMotors();

    while(1)
    {
     //if(SENSOR(10))
     //        start=1;
     sensors = ReadSensors();

     if(SENSOR(10))
           start=1;
       if(start)
      {        
          Walk();
          //MoveToZeros();


     if (SENSOR(7)||SENSOR(8))

     { t= 70;
          }
     else { t=100;
          }
     if (SENSOR(1)||SENSOR(2)||SENSOR(3))
     {
      LeftStroke =70;
          RightStroke = 38;
     }

     else if (SENSOR(4)||SENSOR(5)||SENSOR(6))
     {
          LeftStroke = 40;
          RightStroke = 18;
     }

     else
     {
          LeftStroke = 40;
          RightStroke = 38;
     }

     if (SENSOR(3)&&SENSOR(4)&&SENSOR(7) && SENSOR(8))
     {    LeftStroke = 10;
        RightStroke = 38;
     }
     else
     {
        LeftStroke = 40;
        RightStroke = 30;
     }
}
}

}