/** @author Peter Vanopulos
    @date 29/11/99
    @filename wall.c
    @memo
	The purpose of this file is to make the robot drive along a wall.
  Instead of sensing the wall is now very close and abruptly turning\
  the objective is to turn the robot smothly.

  Modified: Thomas Braunl, May 2002
*/

#include "eyebot.h"

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

#define v_lin 7.0
#define t_lin 0.3
#define v_rot 10.0
#define t_rot 0.1   
#define LIMIT 10		
#define SPEED 0.4	

int main ()
{ PSDHandle       psd_front, psd_left, psd_right;
  VWHandle        vw;

  int old_left, old_right, old_front;
  int new_left, new_right, new_front;

  vw=VWInit(VW_DRIVE,1); /* parameter for scale i.e. 100Hz*/			
  VWStartControl(vw, v_lin, t_lin , v_rot, t_rot);

  /* initialise the PSD's */
  psd_front = PSDInit(PSD_FRONT);
  psd_left = PSDInit(PSD_LEFT);
  psd_right = PSDInit(PSD_RIGHT);
  PSDStart(psd_front, TRUE);
  PSDStart(psd_left,TRUE);
  PSDStart(psd_right,TRUE);
  OSWait(10);

  LCDPrintf("Wall-Following\n\nPlace robot\nto wall at lhs\n");
  LCDMenu("","","","GO");

  /* intialise the old values */
  do
  { old_left = PSDGet(psd_left);
    LCDSetPrintf(5, 0, "Dist: %4d\n", old_left);
  } while (KEYRead() != KEY4);

  LCDMenu("","","","END");
  while(KEYRead() != KEY4)    /* loop until the end button is hit */
  { new_left  = PSDGet(psd_left);
    new_right = PSDGet(psd_right);
    new_front = PSDGet(psd_front);
    LCDPrintf("Dist L%4d F%4d", new_left, new_front);

    if(new_front > old_left)
    { /* still space, keep driving forward */
      if( abs(new_left-old_left) < LIMIT )
        VWDriveStraight(vw, 1.0, SPEED); 
      else /* follow the left wall */
      	if(new_left > old_left) /* turn left */
        { LCDPrintf("%d: move in\n", new_left);
          VWDriveCurve(vw, 0.15, M_PI/17, SPEED);
        }
        else 	/* turn right */	
        { LCDPrintf("%d: move out\n", new_left);
          VWDriveCurve(vw, 0.15, -M_PI/17, SPEED);
        }
    }

    else  /* otherwise make a right turn */
    {	LCDPutString("Turning Right\n");
      VWDriveCurve(vw, 0.2, -M_PI/2, 0.2);
      VWDriveWait(vw);
    }  

    /* update the PSD sensors */
    old_left  = new_left;
    old_right = new_right;
    old_front = new_front;

    OSWait(40); /* wait for x * 1/100 s */
  } /* end while loop */
    
  VWRelease(vw);
  PSDRelease();
  return 0;
}