#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  sonarSensor,    sensorSONAR_inch)
#pragma config(Motor,  port1,           rightMotor,    tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port10,          leftMotor,     tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//VOID STATEMENTS =================================================================================

      //STRAIGHT
void moveStraight(int encoderCount)
{
      SensorValue[rightEncoder] = 0;
       SensorValue[leftEncoder] = 0;

    while(SensorValue[rightEncoder] < encoderCount)
  {
      if(SensorValue[leftEncoder] > SensorValue[rightEncoder])
      {
         motor[rightMotor] = 63;
         motor[leftMotor] = 47;
     }
     if(SensorValue[rightEncoder] > SensorValue[leftEncoder])
     {
        motor[rightMotor] = 55;
        motor[leftMotor] = 63;
     }
     if(SensorValue[leftEncoder] == SensorValue[rightEncoder])
     {
        motor[rightMotor] = 63;
        motor[leftMotor] = 48;
     }
  }
}

      //RIGHT

void moveRight(int encoderCount)
{
      SensorValue[rightEncoder] = 0;
       SensorValue[leftEncoder] = 0;

  while(SensorValue[leftEncoder] < encoderCount) //475!!!!!!
   {
    motor[rightMotor] = -63;
    motor[leftMotor] = 60;
   }

      motor[leftMotor] = 0;
     motor[rightMotor] = 0;
     wait1Msec(500);
}

task main()
{

   SensorValue[rightEncoder] = 0;      //Reset encoders to zero
  SensorValue[leftEncoder] = 0;

   while(1 == 1)                     //while true
   {
      if(SensorValue[sonarSensor] > 7)   // when the robot is greater than 7cm away from an object
      {
         moveStraight(1225);      //straight for the encoder count 1225 then turn right for the encoder count 450

         moveRight(450);
      }
      else                  // if it is closer than 7cm
      {
         motor[leftMotor] = 0;      //stop
         motor[rightMotor] = 0;
      }
   }
}