/*------------------------------------------------------------------------
| Filename: drive.c
|
| Author:       Nicholas Tay, UWA 1997
|
| Description:  Drives EyeBot vehicle
|               using PSD-sensor 
|               Attempts to drive down a corridor, avoiding the walls.
|		( Revised from drive_rc.c )
|
-------------------------------------------------------------------------- */

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

/* --------------------= VEHStop =---------------------- */

void VEHStop(VWHandle vw)
{
  VWSetSpeed(vw,0,0);
}

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


int main ()
{

  /* -----------= Variable declarations =-------------- */
  VWHandle vw;
  SpeedType s;
  PositionType start, stop, spos, back;
  int obstacle;
  int limit = 200;
  float diff, j1, j2;

  PSDHandle psd;
  /* BumpHandle bump_l, bump_r;*/
  IRHandle ir_lm, ir_rm, ir_rf, ir_lf;  

  
  /* - - - - - - -= 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;
  }

  
  /* - - - - - - -= BUMP Handler =- - - - - - - - - */

  /*
    if ((bump_l = BUMPInit(BUMP_LEFT)) == 0)  {
    LCDPutString("Bumper L Init Error!\n");
    OSWait(200);return 1;
    }
    if ((bump_r = BUMPInit(BUMP_RIGHT)) == 0) {
    LCDPutString("Bumper R Init Error!\n");
    OSWait(200);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);


  /* main action loop */ 

  /*  do { */

  do {
    obstacle = PSDGet(psd); /* get distance via IR */
    LCDPrintf("Obstacle at %dmm\n",obstacle);
    VWSetSpeed(vw,0.2,0);

    /* Stallcheck
      OSWait(5);
      VWGetSpeed(vw,&stop2);
      if (stop2.v == 0) {
      VEHStop(vw);
      obstacle = 0;
      VWSetSpeed(vw,-0.4,0);
      OSWait(20);
      VEHStop(vw);
      }
      */

    
    /* --------------< IR Handler >---------------- */
    
      if((IRRead(ir_lm) == 0) || (IRRead(ir_rm) == 0))  {
	VEHStop(vw);
	if(IRRead(ir_lm) == 0)  {
	  LCDPrintf("left too close\n");
	  VWSetSpeed(vw,-0.1,-0.63);
	} else {
	  LCDPrintf("right too close\n");
	  VWSetSpeed(vw,-0.1,0.63);
	}
	OSWait(65);
	VEHStop(vw);
      }
      if((IRRead(ir_lf) == 0) && (IRRead(ir_rf) == 0)) {
	LCDPrintf("Cant move forward\n");
	obstacle = 0;
	VEHStop(vw);
	VWSetSpeed(vw,-0.2,0);
	OSWait(50);
	VEHStop(vw);
	OSWait(3);
      }

    
  } while ((obstacle > limit) && (KEYRead() != KEY4));

  VEHStop(vw);
  VWGetPosition(vw,&stop);  
  
  /* ---------------< turn(180) >-------------- */
  
  if (stop.phi < 0)
    VWDriveTurn(vw,M_PI,1.3);
  else
    VWDriveTurn(vw,-M_PI,1.3);
  do {
  }while ((VWDriveDone(vw) == 0) && (KEYRead() != KEY4));
  
  
  /* -------------------> drive back <-------------------- */

  VEHStop(vw);
  VWGetPosition(vw,&spos);
  
  do {
    
    /* --------------= IR Handler =---------------- */
    
    VWSetSpeed(vw,0.2,0);
    if((IRRead(ir_lm) == 0) || (IRRead(ir_rm) == 0))  {
      VEHStop(vw);
      if(IRRead(ir_lm) == 0)  {
	LCDPrintf("left too close\n");
	VWSetSpeed(vw,-0.1,-0.63);
      } else {
	LCDPrintf("right too close\n");
	VWSetSpeed(vw,-0.1,0.63);
      }
      OSWait(65);
      VEHStop(vw);
      VWGetPosition(vw,&spos);
      }

    
    VWGetPosition(vw,&back);
    if ((j1 = back.x - start.x) < 0)
      j1 = -1*j1;
    if ((j2 = back.y - start.y) < 0)
      j2 = -1*j2;
    diff = j1+j2;
    LCDPrintf("DIFF %f metres\n",diff);

    /* -----< Safety exit loop >------ */
    
    if((IRRead(ir_lf) == 0) && (IRRead(ir_rm) == 0)) {
      VEHStop(vw);
      diff = 0.00;
    }
    
  } while ((diff > 0.25) && (KEYRead() != KEY4));
  /*  stop within 25cm accuracy */

  VEHStop(vw);
 
  LCDPrintf("TOTAL DIST = %.2fm\n",fabs(stop.x-start.x)+fabs(stop.y-start.y));

  /*
    BUMPRelease(bump_l);
    BUMPRelease(bump_r);
    */
  
  IRRelease(ir_rm+ir_rf+ir_lf+ir_lm);
  PSDRelease();
  VWRelease(vw); /* exit driver */

  return 0;
}