/**
 * 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.

//IRPD VC setups
oDio1 IRPDL = New oDio1;	  // left IRPD bit
oDio1 IRPDR = New oDio1;   	  // Right IRPD
oGate irGate = New oGate(1);    //connects oDio1's with oMath's
oBit irLeft = new oBit;		  //left IRPD
oBit irRight= new oBit;		  //right IRPD
oMath irFinal=new oMath;	  //final math result
oMath irMath=new oMath;	        //right IR x 2 value
oNibble irVal = New oNibble;    //result of IRPD looking
oNibble irInt = New oNibble;	  //intermediate value

//auto whisker VC setup
oDio1 BMPR = New oDio1;		  //bumper input
oGate bumpWire = New oGate(1);  // connection point for bumper
oEvent bmpLook = New oEvent;	  // event started by the bumper
oDebounce bmp = New oDebounce;  // debounce of bumper
oBit bumper = New oBit;

//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)
{
  // Setup I/O bits

  //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;

  //--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;					//Do some random wandering
    irpd;					//Look at current IRPD values
    doDrive;				//act on current drive value
    //bumper fires if it is hit and subsumes all automatically
  } while (1);
}

Sub void wander(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;
      }
    rattle.Output.Link(dur);
    rattle.Operate = cvTrue;
    rattle.Operate = cvFalse;		//Both are now random numbers
    if (dur > 10) {
      dur=10;				//we want SOME duration!
    }
  }
}

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 irpd(void) {
  //This routine reacts to IRPD obstruction detection
  if (priority > 2 ) {			//Higher priority function active
    return;
  }
  if (irVal > 0) {
    priority = 2;

    switch (irVal) {
    case 0:
      LEDL = cvOff;
      LEDR = cvOff;
      drive =fwd;
      break;

    case 1:
      LEDL = cvOn;
      LEDR = cvOff;
      drive = tright;
      break;

    case 2:
      LEDR = cvOn;
      LEDL = cvOff;
      drive = tleft;
      break;

    case 3:
      LEDR = cvOn;
      LEDL = cvOn;
      drive = rRight;
      break;

    };					//This semicolon is a bug workaround
  }
  else {
    if (priority == 2) {            //clear priority this pass
      priority = 0;
      LEDL = cvOff;
      LEDR = cvOff;
    }
  }  
}

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) {
  //This sets up the IRPD VC, its kind of ugly.

  //Set up IRPD math result in irVal
  //This is faster than generic math code
  IRPDL.Ioline = 24;
  IRPDL.Direction = cvInput;
  IRPDR.IoLine = 25;
  IRPDR.Direction = cvInput;
  irFinal.Input1.Link(IRPDL.Value);		//one's bit
  irMath.Input1.Link(IRPDR.Value);
  irMath.Input2.Link(IRPDR.Value);		//attempt x2 thing
  irMath.Mode = cvAdd;
  irMath.Operate = cvTrue;
  irMath.Output.Link(irInt);			//intermediate value
  irFinal.Input2.Link(irInt);
  irFinal.Mode = cvAdd;
  irFinal.Operate = cvTrue;
  irFinal.Output.Link(irVal);			//The final result
}

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