/* ====================================================================
 * sonar.c
 *
 * Test the mindsensors sonar ranging device.
 *
 *    Motors on A and C
 *    Sonar on input 2
 *
 * Copyright: Gary Harkin
 *            2005
 *
 * =====================================================================
 */


#include <config.h>
#include <conio.h>
#include <unistd.h>
#include <tm.h>

#include <dsensor.h>
#include <dmotor.h>

#define SONARSENS       SENSOR_2

#define ZERO_SPEED (0)
#define SLOW_SPEED (MAX_SPEED/3)
#define NORMAL_SPEED (2*MAX_SPEED/3)
#define FAST_SPEED   (MAX_SPEED)
#define TURN_SPEED   (MAX_SPEED)

#define TOO_CLOSE    5
#define CLEAR        15

static void display_range (int r)
{
   cls();
   lcd_number (r, unsign, e0);
}

static wakeup_t blocked (wakeup_t data) 
{
   int x;

   x = LIGHT(SONARSENS);
  
   cls();
   lcd_number (x, unsign, e0);

   return x < (unsigned short) data;
}

static wakeup_t object_gone (wakeup_t data) 
{
  return LIGHT(SONARSENS) > (unsigned short) data;
}

static void stop ()
{
   motor_a_speed (0);
   motor_c_speed (0);
}

static void backup (int speed, int t)
{
   motor_a_speed (speed);
   motor_c_speed (speed);

   motor_a_dir (rev);
   motor_c_dir (rev);

   msleep (t);
}


static void turn_right (int t)
{
   motor_a_speed (TURN_SPEED);
   motor_c_speed (TURN_SPEED);

   motor_a_dir (fwd);
   motor_c_dir (rev);

   msleep (t);
}

static void evade () 
{
   int dir=0;
  
   while (!object_gone (CLEAR))
   {
      stop ();
      backup (FAST_SPEED, 500);
      stop ();
      turn_right (250);
   }

}

static void stroll () 
{
   while (! shutdown_requested ())
   {
      motor_a_speed(NORMAL_SPEED);
      motor_c_speed(NORMAL_SPEED);
      motor_a_dir(fwd);
      motor_c_dir(fwd);

      wait_event (blocked, TOO_CLOSE);

      evade ();
   }
 
}

int main(int argc, char *argv[]) 
{
   int   x;

   /*
    * Set the sensor passive so that it can recalibrate.  Takes about 1 second.
    */
   ds_passive (&SONARSENS);
   sleep (1);

   /*
    * Now make it active and put it in percent mode for reading.
    */

   ds_active (&SONARSENS);
  
/* Testing code.  For hard surfaces, it calibrates to a raw value of about
   5 for a 6 inch separation.  It maxes out at about 80, but drops off
   in accuracy rapidly after about 6 feet. 

   while (1)
   {
      x = LIGHT(SONARSENS);
  
      cls();
      lcd_number (x, unsign, e0);
      sleep (1);
   }
 */

   stroll (); 
  
   return 0;
}