/* Controller to find dark place then spring out and ram objects  */
/* >> PRESS BUTTON TO START - resets if timeout or button pressed */
/* Copyright 1998, Johuco Ltd., All Rights Reserved */
/* Written by Jon Connell, 2/98, Version 1.3 beta   */

/* This codes uses "basal" and "virtual" as well as activities. */
/* For a simpler control program see "wander2.c"                */

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

It is often useful to have a more structured control system than just
free-form C code. What we have found convenient is a three level
hierarchy of behaviors, policies, and activities. These granularities
correspond well to various types of code reusability and they
modularize the functionality in a way that makes it easy to debug a
controller one piece at a time.

The basic level of control is a behavior. Typically a behavior only
controls one actuator modality (i.e. NOT both steering and
translation speed). This makes it easier to re-use them in other
controllers.  For a specific sensory pattern a behavior recommends a
particular actuator control value. This part of the code is the
"transfer function". If a behaviors does not know what to do it
should remain silent and generate no control value. This gating is
accomplished by a binary "applicability predicate". Sometimes it is
useful to have some time constants controlling how long a behavior is
active. These are implemented through self-decaying state variables
called "monostables". A monostable returns to zero after its
triggering condition has been false for a given amount of time.

Above behaviors are policies. These are collections of behaviors
which in combination give the robot some particular skill. Policies
principally fuse the commands from different behaviors. Some of this
command arbitration occurs automatically via the individual
applicability predicates of the behaviors.  Still, it is not unusual
for different behaviors to generate conflicting commands. To resolve
this we impose a fixed priority "suppression network" in which one
behavior always wins for each actuator. This is typically implemented
just by the sequence of behavior calls -- they are called in order
from least important to most important. Each behavior affects some
global control variables and the most important behavior for each
modality thus has the "last word" concerning what values are set.

Finally there are activities. These are sequences of policies which
the robot shifts between based on certain discrete events. They are
implemented like finite state automata, typically with a few bits of
persistent state. They can also be recursively embedded so a high
level activity can be composed of lower level activities. A
particularly useful interaction is to have the high level activity
automatically terminate the lower level one when some goal condition
has been achieved or when a timeout occurs. In general, the actual
details of the policies and transitition rules used by an activity
are hidden from its caller, however its current state is visible.
This is useful for determining whether progress is being made. It
also allows us to use an activity to determine the condition of the
environment -- the final state it arrives at codes the condition.

Many behaviors take raw sensory values as their inputs and produce
hardware specific control values as outputs. Yet in certain cases it
makes sense to connect to higher-level virtual sensors and actuators
instead. These can be constructed using policies which work with
specific global variables instead of base-level I/O commands. For
instance, for a legged robot it is very useful to have a virtual
actuator like "travel-direction". This way strategic guidance
behaviors do not have to understand all the interactions between legs
or worry about dynamic balance. Similarly, it is often advantageous
to have a generic virtual sensor like "target-location". Many
different policies can set this variable based on the current
circumstances and goals, yet the same back-end policies for pursuing
the target can be used irregardless of its derivation.

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

/* include basal.h first else nesting of file names gets too long */

#include <basal.h> 
#include <virtual.h> 


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

/* function prototypes */

int button_act (int start);
int stalk_act (int start);
int pause_ram_act (int start);

void blink_pol (void);
void ir_led_pol (void);
void squeal_pol (void);
void find_dark_pol (void);
void avoid_pol (void);
void aim_light_pol (void);
void beep_pause_pol (void);
void smash_pol (void);

void wink_eyes_bhv (void);
void nled_bhv (void);
void wled_bhv (void);
void ndiff_bhv (void);
void wdiff_bhv (void);

void steady_noise_bhv (void);
void beeping_bhv (void);

void cruise_bhv (void);
void slow_bhv (void);
void blocked_bhv (void);
void shy_bhv (void);
void floor_it_bhv (void);

void turn_in_place_bhv (void);
void light_circle_bhv (void);
void unwedge_bhv (void);
void n_veer_bhv (void);
void w_veer_bhv (void);
void n_center_bhv (void);
void w_center_bhv (void);


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

/* dispatches to step in activity every time sensor values are updated  */
/* virtual sensors and actuators are policies on new global variables   */
/*   that are used to provide a higher-level interface to the robot     */
/* try substituting single activities, policies, or behaviors in place  */
/*   of pause_ram_act to see what they do in isolation or for debugging */

void main ()
{
  int state = 0;

  init_basal();
  init_virtual();
  while (next_basal())
  {
    /* compute virtual sensors, do activity, run virtual actuators */
    next_virtual();
    v_sensor_pol();
    state = pause_ram_act(state); 
    v_actuator_pol();
  }
}


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

/* Note: activities listed from simple to more complex, not by call level */

/* this is used strictly as an advanced sensor -- no associated policy */
/* reaches state 3 only when button is pressed and then released       */

int button_act (int start)
{
  int stable = 0, state = start;

  while (!stable)
   switch (state)
   {
     case 1:
       /* wait for button to be pressed */
       /* "shift_state" sets new value and invokes next loop iteration */
       if (button_val != 0)
         shift_state(2);
       mark_stable(); 
     
     case 2:
       /* wait for button to be released */
       if (button_val == 0)
         shift_state(3);
       mark_stable();
       
     case 3:
       /* final absorbing state */
       mark_stable();
	
     default:
       /* catch-all in case of initialization or error */
       shift_state(1);	      
   }
  return state;   
}


/* The core stalking activity itself has 5 states:         */
/*   1. squeal a while then start loop                     */
/*   2. first roam around heading toward dark places       */
/*   3. when under a dark overhang turn to face light      */
/*   4. when aligned pause while beeping for a bit         */
/*   5. zoom out and head toward obstacles until collision */

int stalk_act (int start)
{
  static int mono;
  int stable = 0, state = start;

  while (!stable)
    switch (state)
    {
      case 1:
        /* squeal in anticipation until timeout */
        /* "enter_reset" sets var to zero if this state just entered */
        enter_reset(mono);	      
        update_mono(mono, 32, mono == 0);
	if (!mono)
          shift_state(2);
        squeal_pol();
	mark_stable();
	
      case 2:
        /* roam looking for dark place until top dim enough */	      
        if (top_level < 50) 
          shift_state(3);		
        find_dark_pol();
        mark_stable();

      case 3:
        /* turn in place until brightest direction found     */	      
        /* if discovered then go back and seek another place */	      
        /* (only integer math is supported by the compiler)  */
        if (front_level > 150)	      
	  shift_state(4);
        if (top_level > 100)  
          shift_state(2);     
        aim_light_pol();
	mark_stable();
	
      case 4:
        /* sit and beep until timeout occurs (set timer at entry) */	      
        enter_reset(mono);	  
        update_mono(mono, 40, mono == 0);
	if (!mono)
          shift_state(5);   		
        beep_pause_pol();
	mark_stable();
	
      case 5:
        /* attempt to hit things until collision happens */	      
        if (shakiness_level > 50) 
          shift_state(1);		
        smash_pol();
        mark_stable();
	
      default:
	/* catch-all in case of initialization or error */
        shift_state(1);	      
    }

  /* could add a partial policy here; it will have higher priority */
  /* (e.g. for collision avoidance) since it always runs after the */
  /* policy selected by the state machine                          */
  return state;    
}


/* main activity "calls" two subactivities              */
/* checks for state transitions until correct one found */
/*   then runs associated policy and exits loop         */

int pause_ram_act (int start)
{
  static int substate, substate2, patience; 
  int stable = 0, state = start;

  /* always show IRs on LEDs irregardless of state */
  ir_led_pol();
  while (!stable)
    switch (state)	  
    {
      case 1:
	/* stay still and wait for button to be pressed then released   */
        enter_reset(substate);	      
	if (substate == 3)  
          shift_state(2); 
	substate = button_act(substate);
	blink_pol();
	mark_stable();
	
      case 2:
        /* simultaneously watch for button and run main activity  */     
	/* give up if button hit or whole sequence takes too long */
	enter_reset(substate);
	enter_reset(substate2);
        update_mono(patience, 900, substate2 <= 1); 
	if (!patience) 
          shift_state(1);		
	if (substate > 1)
          shift_state(3);		    
	substate = button_act(substate);
	substate2 = stalk_act(substate2); 
        mark_stable();

      case 3:
	/* stop moving and wait for button to be released */
	if (substate == 3)
          shift_state(1);
        substate = button_act(substate);
        mark_stable();
	
      default:
        /* catch-all in case of initialization or error */
        shift_state(1);	      
    }
  return state;    
}


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

/* blink both eyes on and off alternately */

void blink_pol ()
{
  wink_eyes_bhv();	
}


/* copy directional proximity dtections to LED eyes             */
/* prefer showing difference (if any), wide overrides narrow    */
/* this is useful and might be a candidate for a virtual sensor */

void ir_led_pol ()
{
  nled_bhv();
  wled_bhv();
  ndiff_bhv();	
  wdiff_bhv();  	
}


/* sit still and activate beeper (use turn and drive defaults) */
/* could in-line code this single simple behavior if desired   */

void squeal_pol ()
{
  steady_noise_bhv();
}


/* generally circle until a darker spot found then go straight  */
/* this default motion policy is modified by mixing in the      */
/*   standard collision avoidance partial policy                */

void find_dark_pol ()
{
  cruise_bhv();
  light_circle_bhv();
 
  avoid_pol();
}


/* collision avoidance can be mixed with various default motions  */
/* might want to add more complicated behaviors like in wander2.c */
/* try commenting out all behaviors then adding them back in      */
/*   from least important to most important to see what they do   */

void avoid_pol ()
{
  slow_bhv();
  blocked_bhv(); 
  shy_bhv();

  unwedge_bhv();
  n_veer_bhv();
  w_veer_bhv();
}


/* turn in place, activity will terminate this step automatically  */
/* could add some collision avoidance but moves are small and slow */

void aim_light_pol ()
{
  turn_in_place_bhv();	
}


/* beep on and off to let user know robot is about to attack */

void beep_pause_pol ()
{
  beeping_bhv();
}


/* generally run forward swerving toward any IR detections  */

void smash_pol ()
{
  floor_it_bhv();
  
  n_center_bhv();
  w_center_bhv();
}


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

/* blink LEDs on and off rapidly */

void wink_eyes_bhv ()
{
  static int phase;
  
  update_mono(phase, 4, phase == 0);
  if (phase > 2)
    led_cmd = 2;
  else
    led_cmd = 1;	  
}


/* show if either narrow sensor is seeing anything (except noise) */

void nled_bhv ()
{
  if ((nright_max > 10) || (nleft_max > 10))
    led_cmd = 3;
}


/* show if either wide sensor is seeing anything (except noise) */

void wled_bhv ()
{
  if ((wright_max > 10) || (wleft_max > 10))
    led_cmd = 3;
}


/* if one narrow sensor noticeable closer than other show asymmetry */

void ndiff_bhv ()
{
  if (nright_max > nleft_max + 50)
    led_cmd = 1;	  
  else if (nleft_max > nright_max + 50)
    led_cmd = 2;	  
}


/* if one wide sensor noticeable closer than other show asymmetry */

void wdiff_bhv ()
{
  if (wright_max > wleft_max + 50)
    led_cmd = 1;	  
  else if (wleft_max > wright_max + 50)
    led_cmd = 2;	  
}


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

/* just leave beeper on */

void steady_noise_bhv ()
{
  beeper_cmd = 1;	
}


/* use monostable to build an oscillator controlling tone generator */

void beeping_bhv ()
{
  static int osc;
  
  update_mono(osc, 8, osc == 0);
  if (osc > 4)
    beeper_cmd = 1;	  
}


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

/* go forward continuously */

void cruise_bhv ()
{
  drive_cmd = 200;
}


/* slow down if center IRs see anything */

void slow_bhv ()
{
  drive_cmd = 200 - (max(nright_max, nleft_max) << 1);	
}


/* if both wide IR see something, back up */

void blocked_bhv ()
{
  static int block;
  
  update_mono(block, 8, (wleft_max > 40) && (wright_max > 40));
  if (block)
    drive_cmd = -200;
}


/* if any IR very close, back up for a while */

void shy_bhv ()
{
  static int near;
  
  update_mono(near, 8, (wleft_max > 85) || (wright_max > 85));
  if (near)
    drive_cmd = -200;
}


/* turn off motor servo (set > 100) to obtain highest speed */

void floor_it_bhv ()
{
  drive_cmd = 200;
}


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

/* as long as drive_cmd = 0, virtual will move robot back and forth */

void turn_in_place_bhv ()
{
  turn_cmd = -100;	
}


/* turn left unless front getting darker */

void light_circle_bhv ()
{
  if (front_level >= front_avg)
    turn_cmd = 100;	  
}


/* if both wide IR see something, pick a turn direction */

void unwedge_bhv ()
{
  static int block;
  
  update_mono(block, 8, (wleft_max > 40) && (wright_max > 40));
  if (block)
    turn_cmd = 100;
}


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

void n_veer_bhv ()
{
  if (nleft_max > nright_max + 50)
    turn_cmd = -100;
  else if (nright_max > nleft_max + 50)
    turn_cmd = 100;	   
}


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

void w_veer_bhv ()
{
  if (wleft_max > wright_max + 50)
    turn_cmd = -100;
  else if (wright_max > wleft_max + 50)
    turn_cmd = 100;	   
}


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

void n_center_bhv ()
{
  if (nleft_max > nright_max + 50)
    turn_cmd = 100;
  else if (nright_max > nleft_max + 50)
    turn_cmd = -100;	   
}


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

void w_center_bhv ()
{
  if (wleft_max > wright_max + 50)
    turn_cmd = 100;
  else if (wright_max > wleft_max + 50)
    turn_cmd = -100;	   
}