Maze path search robot : mazeFunction.ino
Please copy and paste it into Arduino IDE.
  1. void mazeSolve(void) {
  2.   while (!status) {
  3.     readLFSsensors();
  4.     switch (mode) {
  5.       case NO_LINE:
  6.         Stop();
  7.         Turnright(178);
  8.         Forward(0);
  9.         delay(100);
  10.         if (EndCount == 0) {
  11.           recIntersection('B');
  12.         }
  13.         Serial.println("U_TURN");
  14.         break;
  16.       case CONT_LINE:
  17.         Stop();
  18.         Forward(38);
  19.         readLFSsensors();
  20.         if (mode != CONT_LINE) {
  21.           Stop();
  22.           Turnleft(88); // or it is a "T" or "Cross"). In both cases, goes to LEFT
  23.           if (EndCount == 0) {
  24.             recIntersection('L');
  25.           }
  26.           Serial.println("T_LEFT_TURN");
  27.         }
  28.         else mazeEnd();
  29.         Serial.println("mazeEnd");
  30.         break;
  32.       case RIGHT_TURN:
  33.         Stop();
  34.         Forward(5);
  35.         readLFSsensors();
  36.         if (mode == RIGHT_TURN) {
  37.           Stop();
  38.           Forward(33);
  39.           readLFSsensors();
  40.           if (mode == NO_LINE) {
  41.             Stop();
  42.             Turnright(88);
  43.             if (EndCount == 0) {
  44.               recIntersection('R');
  45.             }
  46.             Serial.println("RIGHT_TURN");
  47.           } else {
  48.             if (EndCount == 0) {
  49.               recIntersection('S');
  50.             }
  51.           }
  52.           Serial.println("Straight");
  53.         }
  54.         break;
  56.       case LEFT_TURN:
  57.         Stop();
  58.         Forward(5);
  59.         readLFSsensors();
  60.         if (mode == LEFT_TURN) {
  61.           Stop();
  62.           Forward(33);
  63.           Stop();
  64.           Turnleft(88);
  65.           if (EndCount == 0) {
  66.             recIntersection('L');
  67.           }
  68.           Serial.println("LEFT_TURN");
  69.         }
  70.         break;
  72.       case FOLLOWING_LINE:
  73.         Straight();
  74.         break;
  76.     }
  77.   }
  78. }
  80. //---------------------------------------------
  82. void recIntersection(char direction) {
  83.   path[pathLength] = direction; // Store the intersection in the path variable.
  84.   pathLength ++;
  85.   simplifyPath(); // Simplify the learned path.
  86. }
  88. //----------------------------------------------
  90. void mazeEnd(void) {
  91.   Stop();
  92.   EndCount++;
  93.   Serial.print(" EndCount=");
  94.   Serial.println(EndCount);
  95.   Stop();
  96.   for (int i = 0; i < pathLength; i++)
  97.     Serial.print(path[i]);
  98.   Serial.print(" pathLenght ==> ");
  99.   Serial.print(pathLength);
  100.   Serial.println(" mazeEnd");
  101.   status = 1;
  102.   mode = STOPPED;
  103.   if (EndCount == 1) { //Start位置へ戻る
  104.     ledBlink(2);
  105.     Serial.println("Start位置へ戻る");
  106.     Stop();
  107.     delay(1000);
  108.     Turnright(178);
  109.     Forward(0);
  110.     delay(1000);
  111.     Stop();
  112.     status = 0;
  113.     mazeSolve();
  114.     Stop();
  115.     delay(1000);
  116.     Turnright(178);
  117.     Forward(0);
  118.     delay(1000);
  119.     EndCount = 0;
  120.     Stop();
  121.   }
  122. }
  124. //---------------------------------------------------------------------
  126. // Path simplification. The strategy is that whenever we enEndCounter a
  127. // sequence xBx, we can simplify it by cutting out the dead end. For
  128. // example, LBL -> S, because a single S bypasses the dead end
  129. // represented by LBL.
  131. void simplifyPath() {
  132.   // only simplify the path if the second-to-last turn was a 'B'
  133.   if (pathLength < 3 || path[pathLength - 2] != 'B')
  134.     return;
  136.   int totalAngle = 0;
  137.   int i;
  138.   for (i = 1; i <= 3; i++)
  139.   {
  140.     switch (path[pathLength - i])
  141.     {
  142.       case 'R':
  143.         totalAngle += 90;
  144.         break;
  145.       case 'L':
  146.         totalAngle += 270;
  147.         break;
  148.       case 'B':
  149.         totalAngle += 180;
  150.         break;
  151.     }
  152.   }
  154.   // Get the angle as a number between 0 and 360 degrees.
  155.   totalAngle = totalAngle % 360;
  157.   // Replace all of those turns with a single one.
  158.   switch (totalAngle)
  159.   {
  160.     case 0:
  161.       path[pathLength - 3] = 'S';
  162.       break;
  163.     case 90:
  164.       path[pathLength - 3] = 'R';
  165.       break;
  166.     case 180:
  167.       path[pathLength - 3] = 'B';
  168.       break;
  169.     case 270:
  170.       path[pathLength - 3] = 'L';
  171.       break;
  172.   }
  174.   // The path is now two steps shorter.
  175.   pathLength -= 2;
  177. }
  179. //------------------------------------------------------------
  181. void mazeOptimization (void)
  182. {
  183.   while (!status) {
  184.     readLFSsensors();
  185.     switch (mode)
  186.     {
  187.       case FOLLOWING_LINE:
  188.         Straight();
  189.         break;
  190.       case CONT_LINE:
  191.         if (pathIndex >= pathLength) mazeEnd ();
  192.         else {
  193.           mazeTurn (path[pathIndex]);
  194.           pathIndex++;
  195.         }
  196.         break;
  197.       case LEFT_TURN:
  198.         if (pathIndex >= pathLength) mazeEnd ();
  199.         else {
  200.           mazeTurn (path[pathIndex]);
  201.           pathIndex++;
  202.         }
  203.         break;
  204.       case RIGHT_TURN:
  205.         if (pathIndex >= pathLength) mazeEnd ();
  206.         else {
  207.           mazeTurn (path[pathIndex]);
  208.           pathIndex++;
  209.         }
  210.         break;
  211.     }
  212.   }
  213. }
  215. //-----------------------------------------------------
  217. void mazeTurn (char dir) {
  218.   switch (dir) {
  219.     case 'L': // Turn Left
  220.       Forward(38);
  221.       Turnleft(88);
  222.       break;
  224.     case 'R': // Turn Right
  225.       Forward(38);
  226.       Turnright(88);
  227.       break;
  229.     case 'B': // Turn Back
  230.       Turnright(178);
  231.       break;
  233.     case 'S': // Go Straight
  234.       Forward(40);
  235.       Straight();
  236.       break;
  237.   }
  238. }
  240. //-----------------------------------------------------
  242. void ledBlink(int times) {
  243.   for (int i = 0; i < times; i++) {
  244.     digitalWrite (ledPin, HIGH);
  245.     delay (500);
  246.     digitalWrite (ledPin, LOW);
  247.     delay (500);
  248.   }
  249. }
by Paradise
元のページヘ戻る