 	diversionCheckingStartTime = millis();
 }
 
+//LBR = B
+//LBS = R
+//RBL = B
+//SBL = R
+//SBS = B
+//LBL = S
+void simplifyMaze()
+{
+  pathLength++;
+  
+  if(pathLength < 3 || path[pathLength-2] != back)
+    return;
+
+  int totalAngle = 0;
+  int i;
+  for(i=1;i<=3;i++)
+  {
+    switch(path[pathLength-i])
+    {
+      case right:
+        totalAngle += 90;
+  break;
+      case left:
+  totalAngle += 270;
+  break;
+      case backward:
+  totalAngle += 180;
+  break;
+    }
+  }
+
+  // Get the angle as a number between 0 and 360 degrees.
+  totalAngle = totalAngle % 360;
+
+  // Replace all of those turns with a single one.
+  switch(totalAngle)
+  {
+    case 0:
+  path[pathLength - 3] = 'S';
+  break;
+    case 90:
+  path[pathLength - 3] = 'R';
+  break;
+    case 180:
+  path[pathLength - 3] = 'B';
+  break;
+    case 270:
+  path[pathLength - 3] = 'L';
+  break;
+  }
+
+  // The path is now two steps shorter.
+  pathLength -= 2;
+  
+} 
+
 #pragma endregion
 
 void printSensorValues()


		moveBothMotors(maxMotorSpeed, forward, maxMotorSpeed, backward);
		checkForNewLineOnSide(right);
		break;

	case left:

const float derivateConst = 1.0f;

const int maxMotorSpeed = 150;

Direction path[300];
unsigned int ;

int drivePastDelay = 300; // tune value in mseconds motors will run past intersection to align wheels for turn NOT TESTED

