// This program ramps an RC Servo Motor's speed
// up and down / forwards and backwards.

// Networking
oByte instruction = new oByte; // The instruction from the master (2)

oServo Arm = new oServo; // Our Arm servo    (4bytes)
oServo Hand = new oServo;  //Our Hand servo  (4)
oLCDSE LCD = New oLCDSE;  //LCD for debugging  (5)

oByte Left_motor = new oByte;
oByte Right_motor = new oByte;
oByte Last_wall = new oByte;

oByte Center_ir = new oByte;  // (2)
oByte Left_F_ir = new oByte;    // (2)
oByte Left_R_ir = new oByte;   // (2)
oByte Right_F_ir = new oByte;    // (2)
oByte Right_R_ir = new oByte;   // (2)
oByte flag = new oByte;     // (2)

//IRPD
oIRRange Multi_ir = New oIRRange;   // IR distance sensor   (5)

//oA2D Objects
oA2D Left_candle = new oA2D; // (3)
oA2D Right_candle = new oA2D; //(3)

// Start Switch
oDIO1 start_switch = new oDIO1;

// Net total object bytes = 40    

// Data defines
Final Left_f_max = 40;  //Min-max motor speeds for forward and reverse
Final Left_f_half = 32;
Final Left_f_min = 40;

Final Right_f_max = 16;
Final Right_f_half = 32;
Final Right_f_min = 40;

Final Left_r_max = -22;
Final Left_r_min = -4;
Final Right_r_max = -22;
Final Right_r_min = -4;


Final Center_ir_min = 43;
Final Center_ir_max = 35;

Final Left_F_ir_min = 31;
Final Left_F_ir_max = 33;
Final Left_R_ir_min = 24;
Final Left_R_ir_max = 30;

Final Right_F_ir_min = 31;
Final Right_F_ir_max = 33;
Final Right_R_ir_min = 24;
Final Right_R_ir_max = 30;

Final Stop_cmd = 130;
Final Forward_cmd = 160;
Final Scan_flame = 200;
Final Turn_left = 192;
Final Turn_right =224;

Final Rec_right = 7;
Final Rec_left = 8;

Final ack = 133;

//Useful values
// Click_dist = (2.75 * 3.145)/32 = .269" per click or 44.44 clicks per foot
// Circumference = Pi * D = (2.75 * 3.145) = 8.64"
// 90_degrees = Cir/4 = 2.15" = 8 clicks

//**********************************************************************************Begin Main
sub void Main(void)
{
  OOpic.Node = 1;  // Our network node

  // Init objects
  // Init IR's
  Multi_ir.IOLine = 2;  //IOLine 2
  Multi_ir.Center = 20; //Set to 20 for all IRPD
  Multi_ir.Operate=1;

  // Init LCD
  LCD.IOLine = 28;
  LCD.Clear;
  LCD.locate(0,0);
  LCD.String = "L      R        ";

  //Init Arm
  Arm.Center = 28;
  Arm.InvertOut = cvTrue;
  Arm.Refresh = 0; //70Hz refresh for max torque
  Arm.IOLine = 15;
  Arm.Operate = cvFalse;
  Arm = 40;

  //Init Hand
  Hand.Center = 28;
  Hand.InvertOut = cvTrue;
  Hand.Refresh = 1; //70Hz refresh for max torque
  Hand.IOLine = 14;
  Hand.Operate = cvTrue;
  Arm.Operate = cvTrue;
  Hand = 50; 
  OOPic.Delay=40;
  Hand.Operate = cvFalse;
  Arm.Operate = cvFalse;

  //Init any vars
  Left_motor = 0;
  Right_motor = 0;
  Last_wall = 0; //0=left 1=right

  //Init a2d's

  //Start switch
  start_switch.IOLine = 8;
  start_switch.Direction=cvInput;

  flag=0;

//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
  //Test_gripper;


  //Wait for start switch
  while(start_switch==0){
  Left_motor = 0;
  Right_motor = 0;
  Instruction=Stop_cmd;
  display_ir_dist;
     LCD.locate(0,0); //Move cursor to display Left_R_ir value;
  LCD.String = "L"+Str$(Left_F_ir);
  LCD.locate(0,14);  //Move cursor to display left_ir value;
  LCD.String = "C"+Str$(Center_ir);
  LCD.locate(0,7); //Move cursor to display left_ir value;
  LCD.String = "R"+Str$(Right_F_ir);}    

  //Ok, hall 1 to end then left*************************************
  LCD.locate(1,0); LCD.String = "1";
  do{
      Wall_follow(1);
  } while(center_ir > 42);
  while (instruction != ack){}
  instruction=Turn_left;

  //Ok, hall 2 to end then right************************************
  LCD.locate(1,0); LCD.String = "2";
  do{     
      Wall_follow(0);
  } while(center_ir > 38);
  while (instruction != ack){}
  instruction=Turn_right;

  //Ok, room 1 enter to wall the 180 right**************************
  LCD.locate(1,0); LCD.String = "3";
  do{
 	 Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_right;
  while (instruction != ack){}  
  instruction=Scan_flame;   //*******************************Scan room for flame
  while (instruction == Scan_flame){Display_ir_dist;}
  if(instruction==140) {Test_gripper;
   while (instruction != ack){}
   instruction=Turn_right;
   do{
 	 Wall_follow(1);
   } while(center_ir > 45);
   while (instruction != ack){}
   instruction=Turn_right;
   do{
 	 Wall_follow(1);
   } while(center_ir > 45);
   while (instruction != ack){}
   instruction=Turn_left;   // Now we are headed back out of the room
  }
  else while (instruction != ack){}

  //Ok, now exit room 1*********************************************
  do{ 
      Wall_follow(0);
  } while(center_ir > center_ir_min);
  //The robot leaves the first room
  while (instruction != ack){}
  instruction=Turn_left;

  //Ok, hall 2 to end of wall on right then rec_right****************
  do{
      Wall_follow(0);
  } while(left_f_ir > 50);
  while (instruction != ack){}
  instruction=Rec_right;

  //Ok, enter room 2, go to end, 180 to right************************
  do{
      Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_right;
  while (instruction != ack){}
  instruction=Turn_right;

  //Ok, leave room 2, to end then right******************************
  do{
      Wall_follow(0);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_right;
  //Robot leaving room two.

  //Ok, hall 2 to end  of left wall then rec_left********************
  LCD.locate(1,0); LCD.String = "5";
  do{
      Wall_follow(1);
  } while(left_f_ir < 60);
  while (instruction != ack){}
  instruction=Rec_left; //Turn dn room 3 hallway

  //Ok, hall 3 to end then right*************************************
  do{
      Wall_follow(1);
  } while(center_ir > center_ir_min); //end of room 3 hallway
  while (instruction != ack){}
  instruction=Turn_right;

  //Ok, enter room 3, turn 180 to right exit room 3******************
  LCD.locate(1,0); LCD.String = "6";
  do{
 	 Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_right;
  while (instruction != ack){}
  instruction=Turn_right;  //pointed out of room 3
  do{ 
      Wall_follow(0);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_left;
  //The robot left the third room

  //Ok, hall 3 to end then left*************************************
  LCD.locate(1,0); LCD.String = "7";
  do{
      Wall_follow(0);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=turn_left;

  //Ok, hall 3 to end then right************************************
  do{     
      if(left_f_ir > (left_f_ir_max + 10)) Wall_follow(0);
      else Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_right;

  //Ok, hall 4 to end then 180 to right*****************************
  do{     
      Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_right;
  while (instruction != ack){}
  instruction=Turn_right;

  //Ok, hall 4 to end of wall on left then rec_left*****************
  do{     
      Wall_follow(0);
  } while(left_f_ir > (left_f_ir_max + 10));
  do{     
      Wall_follow(0);
  } while(left_f_ir < (left_f_ir_max + 10));  
  while (instruction != ack){}
  instruction=stop_cmd;
  while (instruction != ack){}
  instruction=rec_left;  

  //Ok, room4 to end of turn 180 left*******************************
  do{
      Wall_follow(0);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=turn_left;
  while (instruction != ack){}
  instruction=turn_left;
  do{
      Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=turn_right;

  //Ok, hall 4 to end then turn right*******************************
  do{     
      Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=Turn_right;
  do{     
      Wall_follow(1);
  } while(center_ir > center_ir_min);
  while (instruction != ack){}
  instruction=stop_cmd;
}

sub void Wall_follow(Byte direction) //*********************************************Wall Follow
{
  while (instruction != ack){}
  instruction=stop_cmd;
  Display_ir_dist;
  switch (direction)
  {
    case 0 : //For right sensor
    {
      if(Right_F_ir < Right_F_ir_min) Veer_left;
      else if(right_f_ir > right_f_ir_max)// > 3)
      {if((right_f_ir - right_f_ir_max) < 22)veer_right;  
      else Forward;}
	else Forward;
     // LCD.locate(1,13); LCD.String = "WfR";
      break;
    }

    case 1 : // Case 1 is for the left sensor
    {
      if(Left_F_ir < Left_F_ir_min) Veer_right;
      //else if(Left_F_ir > Left_F_ir_max)Veer_left;
      else if((left_f_ir - left_f_ir_max) > 4)
      {if((left_f_ir - left_f_ir_max) < 12)veer_left;  
      else Forward;}
	else forward;
	//LCD.locate(1,13); LCD.String = "WfL";

      break;
    }
  }
}

sub void Forward(void)//****************************************************Forward
{ 
   Left_motor=(Left_f_max - 1); Right_motor=Right_f_max;  
  //LCD.locate(1,17); LCD.String = "FOR";
  //instruction=0;
  while (instruction != ack){}
  instruction=1; //forward
}

sub void Veer_left(void)
{ //LCD.locate(1,17); LCD.String = "VrL";

  Left_motor=3; Right_motor=6; 
  while (instruction != ack){}
  instruction=3;
}

sub void Veer_right(void)
{ //LCD.locate(1,17); LCD.String = "VrR";

  Left_motor=16; Right_motor=4; 
  while (instruction != ack){}
  instruction=3;
}
sub void Turn_to_left(void)
{ Left_motor=3; Right_motor=-3;  
  while (instruction != ack){}
  instruction=3;
}
//
sub void Turn_to_right(void)
{ Left_motor=-2; Right_motor=4;
  while (instruction != ack){}  
  instruction=3;
}


sub void Display_ir_dist(void)
{
  //Get center ir reading
  Multi_IR.IOLine = 5;  //Center IR module connected to A2D Line 2
  Center_ir = (Multi_IR + Multi_ir + Multi_ir)/3;  // Copy our value to the correct IR variable

  //Get left front ir reading
  Multi_IR.IOLine = 2;  //Le ft IR module connected to A2D Line 1
  Left_F_ir = (Multi_IR + Multi_ir + Multi_ir)/3;  // Copy our value to the correct IR variable

  //Get right front ir reading
  Multi_IR.IOLine = 6;  //Right IR module connected to A2D Line 3
  Right_F_ir = (Multi_IR + Multi_ir + Multi_ir)/3;  // Copy our value to the correct IR variable
//  LCD.locate(0,14); //Move cursor to display left_ir value;
//  LCD.String = "C"+Str$(Center_ir);


/*
  LCD.locate(0,0); //Move cursor to display Left_R_ir value;
  LCD.String = "L"+Str$(Left_F_ir);
  LCD.locate(0,14);  //Move cursor to display left_ir value;
  LCD.String = "C"+Str$(Center_ir);
  LCD.locate(0,7); //Move cursor to display left_ir value;
  LCD.String = "R"+Str$(Right_F_ir);
*/
}

sub void disp_vel(void){
  LCD.locate(1,0); //Move cursor to display Left_R_ir value;
  LCD.String = "L"+Str$(Left_motor) + " R"+Str$(Right_motor);
}

Sub void Test_gripper(void)
{OOPic.delay=50;
Hand.Operate = cvTrue;
Arm.Operate = cvTrue;
  do
  { Arm--;
OOPic.delay=5;
    switch(Arm)
    { case 22: hand=43; break;
      case 20: hand=40; break;
      case 17:  hand=15; break;
    }
  } while(Arm > 16);
  OOPic.delay=100;
  do
  { Arm++;
    OOPic.delay=10;
    switch(Arm)
    { case 38: hand=48;break;
	case 22: hand=8; break;
      case 20: hand=8; break;
      case 17:  hand=11; break;
    }
  } while(Arm < 41);
  Hand=50;
Hand.Operate = cvFalse;
Arm.Operate = cvFalse;

}