/* Controller very similar to "wander.c" but implemented using "basal" */ 
/* Copyright 1998, Johuco Ltd., All Rights Reserved */
/* Written by Jon Connell, 1/98, Version 1.3 beta   */

/* This code shows the use of behaviors and policies but not activities. */
/* For a description of these constructs and a more complicated example  */
/* see "stalk.c".                                                        */

/* ====================================================================== */

#include <basal.h> 


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

/* function prototypes */

void wander_pol (void);

void stuck_bhv (void);
void steer_bhv (void);
void stuck2_bhv (void);
void left2_bhv (void);
void right2_bhv (void);
void cruise_bhv (void);
void back_bhv (void);
void crash_bhv (void);
void wide_bhv (void);
void warn_bhv (void);
void osc_bhv (void);


/* ====================================================================== */

/* activate behaviors every time sensors are updated */

void main ()
{
  init_basal();
  while (next_basal())
    wander_pol();
}


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

/* policy for how to react in various circumstances  */
/* takes about 0.4 ms out of 65.5 ms = 0.6% of cycle */    

void wander_pol ()
{
  /* pick turn direction */	
  stuck_bhv();	
  steer_bhv();
  stuck2_bhv();
  left2_bhv();
  right2_bhv();
  
  /* pick drive speed (crash is most important) */
  cruise_bhv();
  back_bhv();
  crash_bhv();	
  
  /* control LED eyes */
  wide_bhv();
  
  /* control beeper */
  warn_bhv();
  osc_bhv();
}


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

/* if no obvious way to turn, pick right */

void stuck_bhv ()
{

  calib[7] = 45;
   
  if ((wleft_val > 0) && (wright_val > 0))
    turn_speed = -100;
}


/* if obstacle on one side but not other, then steer away */

void steer_bhv ()
{
  if ((wleft_val > 0) && (wright_val == 0))
    turn_speed = -100;
  if ((wright_val > 0) && (wleft_val == 0))
    turn_speed = 100;	   
}


/* if there was no obvious way to turn, pick right */

void stuck2_bhv ()
{
  if ((wleft_val > 0) && (wright_val > 0))
    turn_speed = -100;
}


/* latch turn direction when backing up starts (yuck)     */
/* if obstacle was on left but not right, then steer away */

void left2_bhv ()
{
  static int left_mono;
  
  update_mono(left_mono, 8, 
	      ((nleft_val > 0) || (nright_val > 0)) &&
              ((wleft_val > 0) && (wright_val == 0)));
  if (left_mono)      
    turn_speed = -100;
}


/* latch turn direction when backing up starts (yuck)     */
/* if obstacle was on right but not left, then steer away */

void right2_bhv ()
{
  static int right_mono;
  
  update_mono(right_mono, 8, 
	      ((nleft_val > 0) || (nright_val > 0)) &&
              ((wright_val > 0) && (wleft_val == 0)));
  if (right_mono)      
    turn_speed = 100;
}


/* unless more important directive, go forward */

void cruise_bhv ()
{
  drive_speed = 100;
}


/* if something directly ahead then back up for a while       */
/* monostable extends control past time of obstacle detection */

void back_bhv ()
{
  static int back_mono;
  
  update_mono(back_mono, 8, (nleft_val > 0) || (nright_val > 0));
  if (back_mono)
    drive_speed = -100;
}


/* if bumped, shaken, or rolled then stop until things settle */
/* monostable extends control past time of actual impact      */

void crash_bhv ()
{
  static int crash_mono;
  
  update_mono(crash_mono, 16, shakiness_level > 150);
  if (crash_mono)
    drive_speed = 0;
}


/* copy IR readings to LED eyes (default is off) */

void wide_bhv ()
{
  int bits = 0;
  
  if (wleft_val > 0)
    bits |= 0x02;	  
  if (wright_val > 0)
    bits |= 0x01;	  
  led_cmd = bits;
}


/* if something ahead then beep */

void warn_bhv ()
{
  if ((nleft_val > 0) || (nright_val > 0))
    beeper_cmd = 1;
}


/* if bumped, shaken, or rolled then beep on and off evenly    */
/* crash monostable extends control past time of actual impact */
/* osc monostable decays and then resets itself, thresholded   */
/*   at halfway point to control beeper with 50% duty cycle    */

void osc_bhv ()
{
  static int crash_mono, osc_mono;
  
  update_mono(crash_mono, 16, shakiness_level > 200);
  if (crash_mono)
  {
    update_mono(osc_mono, 8, osc_mono == 0);
    if (osc_mono > 3)    	  
      beeper_cmd = 1;	    
    else
      beeper_cmd = 0;	    
  }
  else
    osc_mono = 0;	  
}