- void mazeSolve(void) {
- while (!status) {
- readLFSsensors();
- switch (mode) {
- case NO_LINE:
- Stop();
- Turnright(178);
- Forward(0);
- delay(100);
- if (EndCount == 0) {
- recIntersection('B');
- }
- Serial.println("U_TURN");
- break;
- case CONT_LINE:
- Stop();
- Forward(38);
- readLFSsensors();
- if (mode != CONT_LINE) {
- Stop();
- Turnleft(88); // or it is a "T" or "Cross"). In both cases, goes to LEFT
- if (EndCount == 0) {
- recIntersection('L');
- }
- Serial.println("T_LEFT_TURN");
- }
- else mazeEnd();
- Serial.println("mazeEnd");
- break;
- case RIGHT_TURN:
- Stop();
- Forward(5);
- readLFSsensors();
- if (mode == RIGHT_TURN) {
- Stop();
- Forward(33);
- readLFSsensors();
- if (mode == NO_LINE) {
- Stop();
- Turnright(88);
- if (EndCount == 0) {
- recIntersection('R');
- }
- Serial.println("RIGHT_TURN");
- } else {
- if (EndCount == 0) {
- recIntersection('S');
- }
- }
- Serial.println("Straight");
- }
- break;
- case LEFT_TURN:
- Stop();
- Forward(5);
- readLFSsensors();
- if (mode == LEFT_TURN) {
- Stop();
- Forward(33);
- Stop();
- Turnleft(88);
- if (EndCount == 0) {
- recIntersection('L');
- }
- Serial.println("LEFT_TURN");
- }
- break;
- case FOLLOWING_LINE:
- Straight();
- break;
- }
- }
- }
- //---------------------------------------------
- void recIntersection(char direction) {
- path[pathLength] = direction; // Store the intersection in the path variable.
- pathLength ++;
- simplifyPath(); // Simplify the learned path.
- }
- //----------------------------------------------
- void mazeEnd(void) {
- Stop();
- EndCount++;
- Serial.print(" EndCount=");
- Serial.println(EndCount);
- Stop();
- for (int i = 0; i < pathLength; i++)
- Serial.print(path[i]);
- Serial.print(" pathLenght ==> ");
- Serial.print(pathLength);
- Serial.println(" mazeEnd");
- status = 1;
- mode = STOPPED;
- if (EndCount == 1) { //Start位置へ戻る
- ledBlink(2);
- Serial.println("Start位置へ戻る");
- Stop();
- delay(1000);
- Turnright(178);
- Forward(0);
- delay(1000);
- Stop();
- status = 0;
- mazeSolve();
- Stop();
- delay(1000);
- Turnright(178);
- Forward(0);
- delay(1000);
- EndCount = 0;
- Stop();
- }
- }
- //---------------------------------------------------------------------
- // Path simplification. The strategy is that whenever we enEndCounter a
- // sequence xBx, we can simplify it by cutting out the dead end. For
- // example, LBL -> S, because a single S bypasses the dead end
- // represented by LBL.
- void simplifyPath() {
- // only simplify the path if the second-to-last turn was a 'B'
- if (pathLength < 3 || path[pathLength - 2] != 'B')
- return;
- int totalAngle = 0;
- int i;
- for (i = 1; i <= 3; i++)
- {
- switch (path[pathLength - i])
- {
- case 'R':
- totalAngle += 90;
- break;
- case 'L':
- totalAngle += 270;
- break;
- case 'B':
- 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;
- }
- //------------------------------------------------------------
- void mazeOptimization (void)
- {
- while (!status) {
- readLFSsensors();
- switch (mode)
- {
- case FOLLOWING_LINE:
- Straight();
- break;
- case CONT_LINE:
- if (pathIndex >= pathLength) mazeEnd ();
- else {
- mazeTurn (path[pathIndex]);
- pathIndex++;
- }
- break;
- case LEFT_TURN:
- if (pathIndex >= pathLength) mazeEnd ();
- else {
- mazeTurn (path[pathIndex]);
- pathIndex++;
- }
- break;
- case RIGHT_TURN:
- if (pathIndex >= pathLength) mazeEnd ();
- else {
- mazeTurn (path[pathIndex]);
- pathIndex++;
- }
- break;
- }
- }
- }
- //-----------------------------------------------------
- void mazeTurn (char dir) {
- switch (dir) {
- case 'L': // Turn Left
- Forward(38);
- Turnleft(88);
- break;
- case 'R': // Turn Right
- Forward(38);
- Turnright(88);
- break;
- case 'B': // Turn Back
- Turnright(178);
- break;
- case 'S': // Go Straight
- Forward(40);
- Straight();
- break;
- }
- }
- //-----------------------------------------------------
- void ledBlink(int times) {
- for (int i = 0; i < times; i++) {
- digitalWrite (ledPin, HIGH);
- delay (500);
- digitalWrite (ledPin, LOW);
- delay (500);
- }
- }
|