/* program to drive eyebot around the 4th floor lab table.
   written by - Joon Ng (8/3/98)
   The Univ. of Western Australia, E&E Eng., CIIPS
*/

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

IRHandle ir_lm, ir_rm, ir_rf, ir_lf;  

/* stop vehicle from moving */
void VEHStop(VWHandle vw)
{
  VWSetSpeed(vw,0,0);
}

/* turn x degrees on the spot */
void turnx(VWHandle vw,int ang)
{
  double angle;

  angle = (ang*M_PI)/180;
  
  VWDriveTurn(vw,angle,1);
  while ((VWDriveDone(vw)==0) && (KEYRead()!=KEY4));
}


/* drive round the table anti-clockwise once */
void driveround(VWHandle vw,PSDHandle psd)
{
  int obstacle, x;

  x = 0;
  do {
    /* get distance to obstacle using PSD sensor */
    obstacle = PSDGet(psd);
    /* if obstacle within 20cm, slow down */
    if (obstacle < 200)
      VWSetSpeed(vw,0.1,0);
    else
      VWSetSpeed(vw,0.2,0);

    /* check the IR sensors */

    if((IRRead(ir_lm) == 0) && (IRRead(ir_rm) == 0)) {
      /* stuck in corner, reverse 50cm and turn left 45 degrees */
      VEHStop(vw);
      VWDriveStraight(vw,-0.5,0.1);
      turnx(vw,45);
    }
    if (IRRead(ir_lm) == 0 ) {
      /* front left corner near obstacle, turn 5 degrees away */
      VEHStop(vw);
      while (IRRead(ir_lm) == 0) {
        turnx(vw,-5);
        }
    }
    if (IRRead(ir_rm) == 0 ) {
      /* front right corner near obstacle, turn 5 degrees away */
      VEHStop(vw);
      while (IRRead(ir_rm) == 0) {
        turnx(vw,5);
        }
    }
    if((IRRead(ir_lf) == 0) || (IRRead(ir_rf) == 0))  {
      /* obstacle within 2cm in front, turn left 90 degrees */
      VEHStop(vw);
      turnx(vw,90);
      /* increment turn count */
      x = x + 1;
    }
    
    /* keep driving until 4 left turns made or KEY4 pressed */
  } while ((x < 4) && (KEYRead() != KEY4)); 

  return;
}



int main (void)
{

  /* -----------= Variable declarations =-------------- */
  VWHandle vw;
  SpeedType s;
  PositionType start, stop;
  PSDHandle psd;
  
  
  /* - - - - - - -= PSD Handler =- - - - - - - - */

  LCDMenu("","","","STOP");

  psd = PSDInit(PSD_FRONT);
  if(psd == 0)
  {
    LCDPutString("PSDInit Error!\n");
    OSWait(200); return 1;
  }
  if(PSDStart(psd, TRUE))
  {
    LCDPutString("PSD busy\n");
    return 1;
  }

  
  /* - - - - - - -= IR Handler =- - - - - - - - - - */

  
    if ((ir_lm = IRInit(IR_LM)) == 0)  {
      LCDPutString("IR Init Error!\n");
      OSWait(200); return 1;
    }
    if ((ir_rm = IRInit(IR_RM)) == 0)  {
      LCDPutString("IR Init Error!\n");
      OSWait(200); return 1;
    }
    if ((ir_rf = IRInit(IR_RF)) == 0)  {
      LCDPutString("IR Init Error!\n");
      OSWait(200); return 1;
    }
    if ((ir_lf = IRInit(IR_LF)) == 0)  {
      LCDPutString("IR Init Error!\n");
      OSWait(200); return 1;
    }

    
  /* - - - - - - - - - - - - - - - - - - - - - */

  vw = VWInit(VW_DRIVE,1); /* init v-omega interface */
  if(vw == 0)
  {
    LCDPutString("VWInit Error!\n");
    OSWait(200); return 1;
  }
  s.v = 0.0;  s.w = 0.0;

  VWGetPosition(vw,&start);
  VWStartControl(vw,7,0.3,7,0.1);

  /* wait until psd sensor return reading more than 700 */
  /*  while (PSDGet(psd) < 700); */

  driveround(vw,psd);
  
  VEHStop(vw);
  VWGetPosition(vw,&stop);  
  
  LCDPrintf("%.2f  %.2f\n",fabs(stop.x-start.x),fabs(stop.y-start.y));

  IRRelease(ir_rm+ir_rf+ir_lf+ir_lm);
  PSDRelease();
  VWRelease(vw); /* exit driver */
  
  return 0;
}