Navigation Continued....

Today, after testing out the code yesterday, I fully realized the challenge of having the robot turn consistently every time.

To tackle this, I decided two ultrasonic sensors can be used in the front to compare the two distances in front of them to make sure they're level. 

Programming this would probably involve that if one sensor value was bigger than the other, I should have one motor turn faster than the other to have it turn back into the correct direction. 

void stopstraighten()

{

  unsigned byte SonarValueL;

  SonarValueL = SensorValue[sonarL];

  unsigned byte SonarValueR;

  SonarValueR = SensorValue[sonarR];

  while(SonarValueR == SonarValueL);

  {

    motor[motorR] = 0;

    motor[motorL] = 0;

    wait1Msec(200);

  }

}

task main()

{

  while(1 == 1)

  {

    unsigned byte SonarValueL;

        unsigned byte SonarValueR;

    SonarValueL = SensorValue[sonarL];

    SonarValueR = SensorValue[sonarR];

    //straighten left

    while(abs(SensorValue[sonarR] - SensorValue[sonarL]) != 0)

    if (SensorValue[sonarR] < SensorValue[sonarL])

    {

      motor[motorR] = 40;

      motor[motorL] = -40;

          wait1Msec(30);

    motor[motorR] = 0;

    motor[motorL] = 0;

    wait1Msec(80);

    }

    //straighten right

    //SonarValueL = SensorValue[sonarL];

    //SonarValueR = SensorValue[sonarR];

    else //while(SonarValueR < SonarValueL)

    {

      motor[motorR] = -40;

      motor[motorL] = 40;

          wait1Msec(30);

    motor[motorR] = 0;

    motor[motorL] = 0;

    wait1Msec(80);

    }

  }

    motor[motorR] = 0;

    motor[motorL] = 0;

    wait10Msec(800000000);

}

However, I had difficulty with this design in the end. Since the ultrasonic sensor can only take readings in increments of an inch, there can still be great variability in alignment.