/* Simple control program: robot wanders around avoiding obstacles */
/* Copyright 1997, Johuco Ltd., All Rights Reserved */
/* Written by Jon Connell, 12/97, Version 1.3 beta  */

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

#include <cord.h>


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

/* run-time shell (crt.s) automatically loops so "while" is superfluous */

void main ()
{
  int lf, s, c, rt;
  
  while (1)
  {
    /* get left and right occupancy from wide IR, center from narrow */	  
    s = ir_bits(0, 3);	  
    lf = s >> 1;
    rt = s & 0x01;
    c = ir_bits(1, 3);
   	  
    /* show lateral detections on red LEDs, center via beeper */
    both_leds(s);
    beeper(c, 0);
    
    /* pick motion */
    if (shakiness() > 150)
    {
      /* if bumped, stop a while */
      beep();
      stop();
      sleep_ms(250);
      quiet();
      sleep_ms(250);
    }
    else if (c)
    {
      /* back up and turn away from obstacle */  
      if (rt & lf)
        right();	      
      else 
        turn(lf - rt, 0);
      backward();
      sleep_ms(800);
    }
    else
    {
      /* go forward but veer away from obstacles */	        
      turn(rt - lf, 0);
      forward();
      sleep_ms(100);
    }
  }
}