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

// Networking
oI2C slave = new oI2C ; // Our network object (8)

//Our Motor Control object list needed for this class
oServoSP1 Left_motor = New oServoSP1; //Left motor is a modified servo    (6)
oServoSP1 Right_motor = New oServoSP1; //Right motor is a modified servo  (6)

//Our Encoders 
oDIO1 Left_enc = new oDIO1; // Our left encoder input                     (1)
oDIO1 Right_enc = new oDIO1; // Our right encoder input                   (1)
oCounter L_Counter = new oCounter; //This object will increment our count for us (5)
oCounter R_counter = new oCounter; //This object will increment our count for us (5)

oByte Left_des = new oByte;
oByte Right_des = new oByte;
oByte cmd = new oByte;
oByte Motor_Slave = new oByte;

oByte Left_count = new oByte ; //Actual encoder click count   (2)
oByte Right_count = new oByte ; //Actual encoder click count  (2)

oByte Distance = new oByte;

oWord Left_dist = new oWord ; //Holds our accumulated distance            (3)
oWord Right_dist = new oWord ; //Holds our accumulated distance           (3)

oByte Left_vel = new oByte; //A scaled copy of the count variables       (2)
oByte Right_vel = new oByte; //A scaled copy of the count variables      (2)

oLCDSE LCD = New oLCDSE;  //LCD for debugging  (5)

oBit flame_found = new oBit;


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

// Using 73 of 78 object bytes

Final Fast = 20;   //This global will set our fastest speed
Final Turn_90 = 15; //
Final instruction = 127;//126+1
Final m_left_motor = 120;//119+1
Final m_right_motor = 118;//117+1
Final m_flag = 104;  //103+1
Final m_center_ir = 114; //113+1

Final my_delay=30;
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 = 2;  // Our network node
  slave.node = 1;  // tHE NODE WE WILL BE COMMUNICATING WITH
  slave.Mode = cv10bit;
  slave.Width = cv8bit;
  slave.noInc = 1;
  slave.location=instruction;

  //Initialize robot objects

  // Init motors
  Right_motor.IOLine = 14;
  Right_motor.Center = 54;  //Emperical testing has shown this to be an ideal value
  Right_motor.InvertOut = cvTrue;  //Run servo in reverse
  Right_motor.Operate = cvTrue;  //Turn it on

  Left_motor.IOLine = 15;
  Left_motor.Center = 55;  //Emperical testing has shown this to be an ideal value
  Left_motor.InvertOut = cvFalse; //Run servo normal
  Left_motor.Operate = cvTrue;  //Turn it on

  // Init encoders's
  Right_enc.IOLine = 12;
  Right_enc.Direction = cvInput; //Input mode
  Left_enc.IOLine = 13;
  Left_enc.Direction = cvInput; //Input mode

  //LEFT COUNTER OBJECT
  L_counter.clockin1.link (Left_enc.value);	//Link our trigger flag to the pin status of our encoder input
  L_counter.output.link (Left_count.value);	//Link our count auto increment to our count variable
  L_counter.direction = cvPositive;		//Increment
  L_counter.operate = cvTrue;		//Turn it on
  L_counter.mode = cvCount;  //Single encoder input, not quad
  L_counter.tick = 0;  //Increment our count, 1=reset count

  //RIGHT COUNTER OBJECT
  R_counter.clockin1.link (Right_enc.value);  //Link our trigger flag to the pin status of our encoder input
  R_counter.output.link (Right_count.value);	//Link our count auto increment to our count variable
  R_counter.direction = cvPositive; //Increment
  R_counter.operate = cvTrue;		//Turn it on
  R_counter.mode = cvCount;  //Single encoder input, not quad
  R_counter.tick = 0;  //Increment our count, 1=reset count

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


   //Init a2d's
  Left_candle.IOLine=1;
  Right_candle.IOLine=2;
  Right_candle.Operate=1;
  Left_candle.Operate=1;

  //init any vars
  Left_count = 0;
  Right_count = 0;
  Motor_Slave = 0;
  Flame_found = 0;

//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//******************************************************************************
//****************************************************************************** 

  while(cvTrue)
  { slave.Mode = cv10bit;
    slave.location=m_right_motor; Right_des=slave;
    slave.location=m_left_motor;Left_des=slave;
    slave.location=instruction; cmd=slave;
    LCD.locate(0,0); LCD.String = "L"+Str$(Left_candle) + " R"+Str$(Right_candle) + " C"+Str$(slave);

    //we have an instruction process it
    switch(cmd)
    { case 0: slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack; break; //do nothing
      case 1: Motor_Slave = 1;  Left_motor=Left_des; Right_motor=Right_des; slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack; break;
      case 3: Motor_Slave = 0;  Left_motor=Left_des; Right_motor=Right_des; slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack; break;
      case 7: 
	Left_count=0;      
	do{
	   right_motor=32;
	   left_motor=32;
	}while (left_count<22);
	left_motor=0;          ;
	right_motor=0;
	Turn_right; 
      slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack; 
      break;

	case 8:
	right_count=0;      
	do{
	   right_motor=32;
	   left_motor=32;
	}while (right_count<20);
	left_motor=0;          ;
	right_motor=0;
	Turn_left; slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack;
      break;

      case 130: Motor_Slave = 0; Left_motor=0; Right_motor=0; slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack; break;
      case 200: Scan_right; if(flame_found > 0) Extinguish; else {slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack;} break;// Scan will return a proper code
      case 201: Scan_left; if(flame_found > 0) Extinguish; else {slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack;} break;// Scan will return a proper code
      case 192: Turn_left; slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack; break;
      case 224: Turn_right; slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack;  break;
      default:  slave=0; slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=ack; break;
    }
  }// end while

}

sub void Vel_interval_code(void)
{  //This routine checks each tachometer reading
  Left_dist+=Left_count; // Accumulate our distance in clicks
  Right_dist+=Right_count; // Accumulate our distance in clicks
  Left_vel = Left_count; //Since we are accumulating count 3 times a second, this gives us clicks per second
  Right_vel = Right_count; //Since we are accumulating count 3 times a second, this gives us clicks per second
  Left_count = 0;  // Reset the count for the next reading
    Right_count = 0; // Reset the count for the next reading
 //This routine checks each tachometer reading and
  //attempts to slave the two wheel speed to each other
  if(Motor_slave==1){
   if(Left_motor > 1){
    if(Left_vel > Right_vel) {Right_motor++; Right_motor++; if(Right_motor > 50) Right_motor = 60;}
    else if ((Right_vel > Left_vel)) { Right_motor--; Right_motor--; if(Right_motor < 2) Right_motor = 0;}
   }
  }// end slave_motor
}

sub void Turn_left(void)
{ // Next we will turn off the motor operate functions
  Left_motor = 0;
  Right_motor = 0;
  Left_count = 0;
  Right_count = 0;
  Left_motor = -32;
  Right_motor = 32;
  do{} while (Left_count < Turn_90);
  Left_motor = 0;
  Right_motor = 0;
  OOPic.delay = 20;
}

sub void Turn_right(void)
{ // Next we will turn off the motor operate functions
  Right_motor = 0;
  Left_motor = 0;
  Left_count = 0;
  Right_count = 0;
  Right_motor = -32; //
  Left_motor = 32; //
  do{} while (Right_count < (Turn_90 + 2));
  Right_motor = 0;
  Left_motor = 0;
  OOPic.delay = 20;
}


sub void Scan_left(void)
{ // Next we will turn off the motor operate functions
  Left_motor = 0;
  Right_motor = 0;
  Left_count = 0;
  Right_count = 0;
  Left_motor = -4;
  Right_motor = 3;
  do{LCD.locate(0,0); LCD.String = "L"+Str$(Left_candle) + " L"+Str$(Left_count) + " C"+Str$(Right_candle);
    if (left_candle < 220){LCD.locate(1,0); LCD.String = "E"; flame_found=1; 
     do{
     Right_motor = 0;
     Left_motor = 0;
     Right_count = 0;
     Right_motor = -2; //
     Left_motor = -2; //
     } while (Right_count < Left_count);
      Left_count = 100;}
    } while (Left_count < Turn_90);
  Left_motor = 0;
  Right_motor = 0;
  OOPic.delay = 20;
}

sub void Scan_right(void)
{ // Next we will turn off the motor operate functions
  Right_motor = 0;
  Left_motor = 0;
  Left_count = 0;
  Right_count = 0;
  Right_motor = -2; //
  Left_motor = -2; //
  do{ LCD.locate(0,0); LCD.String = "L"+Str$(Left_candle) + " R"+Str$(right_count) + " C"+Str$(Right_candle);
    if (left_candle < 220){LCD.locate(1,0); LCD.String = "E"; flame_found=1;
     Right_motor = 0;
     Left_motor = 0;
     Left_count = 0; 
     Right_dist = right_count;
     do{
     Left_motor = -4;
     Right_motor = 3;
     LCD.locate(0,0); LCD.String = "L"+Str$(Left_count) + " R"+Str$(right_count) + " C"+Str$(Right_candle);
     } while (Left_count < Right_dist);
     Right_motor = 0;
     Left_motor = 0;

      right_count=100;}
   } while (Right_count < (Turn_90 + 2));
  Right_motor = 0;
  Left_motor = 0;
  OOPic.delay = 20;
}


sub void Extinguish(void)
{
  LCD.locate(1,3); LCD.String = "EXTINGUISH";
  slave.location=m_center_ir;
  while(slave > 33){
    if(Left_candle > right_candle) { if(slave > 60){left_motor = 2; right_motor=0;} else{left_motor = -1; right_motor=0;}}
    if(Right_candle > Left_candle) { if(slave > 60){right_motor = 4; left_motor=0;} else{right_motor=3; left_motor=0;}} 
    LCD.locate(0,0); LCD.String = "L"+Str$(Left_candle) + " R"+Str$(Right_candle) + " C"+Str$(slave);
  }
  left_motor=0; Right_motor=0;
  slave.Mode = cv23bit; slave.location=(instruction + 128)*256 + &H00; slave=140; 
  slave.location=m_center_ir;
 // while(1){LCD.locate(0,0); LCD.String = "L"+Str$(Left_candle) + " R"+Str$(Right_candle) + " C"+Str$(slave);}

}



sub void Display_dist(void)//*******************************************************Display_dist
{
  LCD.locate(0,8);
  LCD.String = Str$(Right_dist);
  LCD.locate(0,1); //Move cursor to display left_ir value;
  LCD.String = Str$(Left_dist);
}

sub void Display_vel(void)//*******************************************************Display_dist
{
  LCD.locate(1,8);
  LCD.String = "RM"+Str$(Right_vel);
  LCD.locate(1,0); //Move cursor to display left_ir value;
  LCD.String = "LM"+Str$(Left_vel);
}

sub void Display_speed(void)//*******************************************************Display_dist
{
  LCD.locate(1,8);
  LCD.String = "RM"+Str$(Right_motor);
  LCD.locate(1,0); //Move cursor to display left_ir value;
  LCD.String = "LM"+Str$(Left_motor);
}