/**
 * First OOPic robot using subsumption (priority) behaviors of
 * Wander, IRPD avoidance and bumper extraction.
 * Some clever dodges are used here.  The speed and direction 
 * for both servos is encoded into a single word and the mod and
 * div functions (/ and %) are used to extract them.  All the
 * behaviors are prioritized and act well in concert.  I'm going
 * to re-work this eventually to move entirely to event based
 * behaviors.  This is a simple example for use with hacked hobby
 * servos.
 * This version runs with compiler 3.0.4 and higher.
 * DLC 11/19/2000
 */
//
//-----------------------------------------------------------------
//  Create all of our objects
//-----------------------------------------------------------------

//servo motor values - Allows both encoded in one instruction
Final fwd = &H0A0A;
Final rev = &H3232;
Final tRight = &H220A;
Final tLeft = &H0A1F;
Final rRight = &H320A;
Final rLeft = &H0A32;
oWord drive = new oWord;	  			//Holds the servo values

//Random movement items
oNibble dir = New oNibble;
obyte dur = new oByte;
oRandomizer rattle = New oRandomizer;
oBit lTurn = new oBit;			//last turn made
oBit lRot = new oBit;			//last rotation made

oDio1 LEDL = New oDio1;    	  // LEDs that echo
oDio1 LEDR = New oDio1;	   	  // motor operations

oServo right = New oServo;   	  // Make a servo object.
oServo left = New oServo;   	  // Make a servo object.

//Wander VC setup
oGate wanderGate = new oGate(1);
oevent wanderEvent = new oEvent;

//IRPD VC setups
oDio4 irpd = New oDio4;
oNibble irVal = new oNibble;
oMath irpdMath = new oMath;
oEvent irpdEvent = new oEvent;
oGate irpdGate = new oGate(1);

//auto whisker VC setup
oDio1 BMPR = New oDio1;		  //bumper input
oEvent bmpLook = New oEvent;	  // event started by the bumper
oDebounce bmp = New oDebounce;  // debounce of bumper

//The variables for the program
oNibble priority = New oNibble; //Priority setups

//Generic vars uses here and there
oWord dlay = new oWord;
oByte temp2 = new oByte;

//-----------------------------------------------------------------
//  Main is the first routine called when the power is turned on
//-----------------------------------------------------------------
Sub void main(void)
{
  OOPic.Node=1;

  //In case we need status LEDs
  LEDL.IoLine = 30;
  LEDL.Direction = cvOutput;
  LEDR.IoLine  = 31;
  LEDR.Direction = cvOutput;

  //--Go set up the object properties--
  SetupServo;
  setupIRPD;
  setupBmp;
  setupWander;

  //--Set the servos to stop and then wait a while--
  right.Operate = cvFalse;
  left.Operate = cvFalse;
  OOPic.delay=200;
  drive = fwd;
  doDrive;
  right.Operate = cvTrue;
  left.Operate = cvTrue;   
  priority = 0;              // initial state

  //--do a program loop that never ends--
  do {
    //wander does some random wandering every second
    //irpd fires if a change in current IRPD values
    //bumper fires if it is hit and subsumes all automatically
    //doDrive;
  } while (1);
}

Sub void wanderEvent_CODE(void) {
  //Randomly wander the world
  if (priority > 1) {
    return;					//higher priority in action
  }
  else if (priority == 1) {
    dur--;
    if (dur == 0) {
      priority = 0;			//all done
    }
  }
  else {
    priority = 1;
    rattle.Output.Link(dir);
    rattle.Operate = cvTrue;
    rattle.Operate = cvFalse;
    //Alternate between left and right turns and rotates
    //Vast majority of the time go forward.
    if (dir < 2)
      if (lRot == 1) {
         drive = rRight;
         lRot = 0;
      }
      else {
         drive = rLeft;
         lRot = 1;
      }
    else if ((dir > 1) & (dir < 13))
      drive = fwd;
    else
      if (lTurn == 1) {
        drive = tRight;
        lTurn = 0;
      }
      else {
        drive = tLeft;
        lTurn = 1;
      }
      dur = 4;
    doDrive;
  }
}

Sub void bmpLook_CODE(void) {
  // This fires when the bumper is hit.

  if (priority > 3) {		//not if something higher is running
    return;
  }
  priority = 3;
  LEDL = 1;
  LEDR = 1;

  drive = rev;
  doDrive;	
  for (dlay=0;dlay<50;dlay++){}
  drive = rRight;
  doDrive;
  for (dlay=0;dlay<50;dlay++){}

  LEDL=0;
  LEDR=0;
  priority = 0;
}

Sub void irpdEvent_CODE(void) {
  //This routine reacts to IRPD obstruction detection
  if (priority > 2 ) {			//Higher priority function active
    return;
  }
  while (irVal != 0) {
    priority = 2;

    switch (irVal) {
    case 1:
      LEDL.set;
      LEDR.clear;
      drive = tright;
      break;

    case 2:
      LEDR.set;
      LEDL.clear;
      drive = tleft;
      break;

    case 3:
      LEDR.set;
      LEDL.set;
      drive = rRight;
      break;

    };				//This semicolon is a bug workaround
    doDrive;
  }
  LEDL.clear;
  LEDR.clear;
  priority = 0;			//all done now.
}

Sub void doDrive(void) {
  //This gives the motors their instructions
  right = drive/256;
  left  = drive % 256;
}


Sub void SetupServo(void) {
  //  A routine that sets up our servo properties

  //--Set up the first servo--
  right.Ioline = 12;      //Set the servo to use I/O Line 31.
  right.Center = 22;      //Set the servos center to 22. (see manual) 
  right.Operate = cvTrue; //Last thing to do, Turn the Servo on.
  //--Set up the second servo--
  left.Ioline = 11;      //Set the servo to use I/O Line 30.
  left.Center = 22;      //Set the servos center to 22. (see manual) 
  left.InvertOut = cvTrue; //Tell the servo to move in reverse.
  left.Operate = cvTrue;   //Last thing to do, Turn the Servo on.
}

Sub void setupIRPD(void) {
  //Set up IRPD math result in irVal
  //This is faster than generic math code
  irpd.IOGroup = 3;		//1=left,2=right,3=both
  irpd.Nibble = 0;
  irpd.Direction = cvInput;
  irpdMath.Input1.Link(irpd.Value);
  irpdMath.Output.Link(irVal);
  irpdMath.Mode=cvOr;
  irpdGate.Input1.Link(irVal.NonZero);
  irpdGate.Output.Link(irpdEvent.Operate);
  irpdMath.Operate = cvTrue;
  irpdGate.Operate = cvTrue;
}

Sub void setupBmp(void) {
  //This sets up the bumper interrupt 
  //Set up Bumper action subsumes all routines
  BMPR.IoLine = 28;
  BMPR.Direction = cvInput;
  bmp.InvertIn = cvTrue;
  bmp.Input.Link(BMPR.Value);
  bmp.Output.Link(bmpLook.Operate);
  bmp.Period = 2;
  bmp.Operate = cvTrue;
}

sub void setUpWander(void) {
    //Configure the wander event
    wanderGate.Input1.Link(OOPic.hz1);
    wanderGate.Output.Link(wanderEvent.Operate);
    wanderGate.Operate = cvTrue;
}