#define US1    S1     // Port Number
#define US2    S2     // Port Number
#define US3    S3     // Port Number
#define US4    S4     // Port Number
#define THRESHOLD 20
	 
int Dist1, Dist2, Dist3, Dist4;
string DIS1, DIS2, DIS3, DIS4;
	 

task MonitorUS()
{
	  while (true)
	  {
	    I2CWrite(US1, 0x41, 0x03);   // Set US1 Sensor in Event Capture Mode
	    Dist1 = SensorUS(US1);
	    Wait(50);
	    I2CWrite(US1, 0x41, 0x00);   // Set US1 Sensor to Off
	    DIS1 = NumToStr(Dist1);
	    TextOut(0, LCD_LINE3, "US Sensor1 " + DIS1 + "   ");

	    I2CWrite(US2, 0x41, 0x03);   // Set US2 Sensor in Event Capture Mode
	    Dist2 = SensorUS(US2);
	    Wait(50);
	    I2CWrite(US2, 0x41, 0x00);   // Set US2 Sensor to Off
	    DIS2 = NumToStr(Dist2);
	    TextOut(0, LCD_LINE4, "US Sensor2 " + DIS2 + "   ");

	    I2CWrite(US3, 0x41, 0x03);   // Set US3 Sensor in Event Capture Mode
	    Dist3 = SensorUS(US3);
	    Wait(50);
	    I2CWrite(US3, 0x41, 0x00);   // Set US3 Sensor to Off
	    DIS3 = NumToStr(Dist3);
	    TextOut(0, LCD_LINE5, "US Sensor3 " + DIS3 + "   ");

	    I2CWrite(US4, 0x41, 0x03);   // Set US3 Sensor in Event Capture Mode
	    Dist4 = SensorUS(US4);
	    Wait(50);
	    I2CWrite(US4, 0x41, 0x00);   // Set US3 Sensor to Off
	    DIS4 = NumToStr(Dist4);
	    TextOut(0, LCD_LINE6, "US Sensor4 " + DIS4 + "   ");
    }
}

  void Direction1()
  {
    OnFwd(OUT_C, 75);
    OnFwd(OUT_A, 75);
    Wait(750);
    Off(OUT_AC);
    OnRev(OUT_A, 75);
    OnFwd(OUT_C, 75);
    Wait(SEC_1);
    Off(OUT_AC);
  }

  void Direction2()
  {
    OnFwd(OUT_C, 100);
    OnRev(OUT_A, 100);
    Wait(SEC_1);
    Off(OUT_AC);
    OnFwd(OUT_A, 75);
    OnFwd(OUT_C, 75);
    Wait(750);
    Off(OUT_AC);
  }

  void Direction3()
  {
    OnRev(OUT_C, 75);
    OnRev(OUT_A, 75);
    Wait(750);
    Off(OUT_AC);
    OnRev(OUT_A, 75);
    OnFwd(OUT_C, 75);
    Wait(SEC_1);
    Off(OUT_AC);
  }

  void Direction4()
  {
    OnFwd(OUT_A, 100);
    OnRev(OUT_C, 100);
    Wait(SEC_1);
    Off(OUT_AC);
    OnRev(OUT_A, 100);
    OnRev(OUT_C, 75);
    Wait(750);
    Off(OUT_AC);
  }



  task main()
	{
	  ClearScreen();
	  SetSensorLowspeed(US1);
	  SetSensorLowspeed(US2);
	  SetSensorLowspeed(US3);
	  SetSensorLowspeed(US4);

	  I2CWrite(US1, 0x41, 0x00);   // Set US1 Sensor Off
	  I2CWrite(US2, 0x41, 0x00);   // Set US2 Sensor Off
	  I2CWrite(US3, 0x41, 0x00);   // Set US3 Sensor Off
	  I2CWrite(US4, 0x41, 0x00);   // Set US3 Sensor Off
	  
    start MonitorUS;
	  while (true)
	  {
	    OnFwd(OUT_A, 100);
      OnRev(OUT_C, 100);

	    if (Dist1 < 25 && Dist2 < 25)
     {
	     OnFwd(OUT_A, 75);
       OnRev(OUT_C, 75);
       Wait(SEC_2);
//       Off(OUT_AC);
     }

	    if (Dist2 < 25 && Dist3 < 25)
     {
	     OnRev(OUT_A, 75);
       OnFwd(OUT_C, 75);
       Wait(SEC_2);
//       Off(OUT_AC);
     }

	    if (Dist1 < 25 && Dist2 < 25 && Dist3 < 25)
     {
	     OnRev(OUT_A, 75);
       OnFwd(OUT_C, 75);
       Wait(SEC_2);
//       Off(OUT_AC);
     }

	    if (Dist1 < 35)
     {
       Off(OUT_AC);
       Direction1();
     }

	    if (Dist2 < 35)
     {
       Off(OUT_AC);
//       Wait(500);
       Direction2();
     }

	    if (Dist3 < 35)
     {
       Off(OUT_AC);
//       Wait(500);
       Direction3();
     }

	    if (Dist4 < 35)
     {
       Off(OUT_AC);
//       Wait(500);
       Direction4();
     }

	  } 
	}