Maze Navigation

Introduction

For maze navigation, we have already made the wavefront algorithm, now we just have to apply that code to the robot.

From our initial code, it saved the directions the robot had to go into a simple c struct. It looks like this:

enum DIR {UP,RIGHT,DOWN,LEFT};

struct path {

  int turns;

  DIR *directions;

  byte steps;

};

The beauty of it is that our directions are saved as enumerated. We also saved them in a certain order, such that we can tell if the robot needs to turn left or right at a given intersection.

Given four directions on the grid: UP, RIGHT, DOWN, and LEFT, we can determine if we need to turn if the direction changed. Since they are enumerated, we used something like if (currentDirection+1)%4=finalPath.direction[i]), then we knew we had to turn clockwise.

If we had if (currentDirection+3)%4=finalPath.direction[i]), then we knew we had to turn counterclockwise, else, the robot should just have to go straight. The robot should never have to make a 180.

From this we were able to get the robot detecting the intersection, and kind of turning.

Here is our first attempt at gong through the maze. The robot should have immediately turned right, gone to the edge of the map, then turned to the left and gone to the edge of the map. It didn't turn out that way

VID_20170409_184317.mp4

From this initial run, we saw some problems that needed to be fixed.

Notice how the robot is stopping as soon as it crosses the line. Instead of stopping the robot so its line sensors have just crossed the line, the robot needs to move forward a certain number of ticks such that the robots wheels are now on the line. This way, when the robot rotates, it rotates on top of the intersection, rather than just behind it.

The robot also needs more work on turning. It missed its first turn completely, probably because of the way it is set up to detect lines while it is turning. This will probably be fixed by the same issue above, moving the robot forward until its wheels are on the new line to turn to.

Final Code

void doMaze(int x, int y) {

    // these initial lines just ensure the robot finds the line on the other side of the paper

    long imm = millis();

    leftMotorSpeed=140;

    rightMotorSpeed=-140;

    Forward(0);

    while (millis()-imm<1000 && digitalRead(PaperSensor)==HIGH && digitalRead(LineSensor)==HIGH && analogRead(PaperSensor2)<30);

    Stop(0);

    delay(1000);

    imm=millis();

    followCurvyLine(1); //ah yes the imfamous reset// this might help :)

    while(millis()-imm<2000) followCurvyLine(0);

    Stop(0);

    delay(1000);

    leftMotorSpeed=-130;

    rightMotorSpeed=130;

    Forward(0);

    while (digitalRead(PaperSensor)==HIGH && digitalRead(LineSensor)==HIGH && !lost);

    if (lost) return;

    Stop(0);

    delay(1000);

    if (digitalRead(PaperSensor)==LOW) {

      leftMotorSpeed=130;

      rightMotorSpeed=-130;

      Forward(0);

      while (digitalRead(LineSensor)==HIGH && !lost);

      if (lost) return;

      Stop(0);

      delay(1000);

    while (digitalRead(PaperSensor)==HIGH && !lost) followCurvyLine(0);

    if (lost) return;

    while (digitalRead(PaperSensor)==LOW && !lost); // cross the line

    if (lost) return;

    leftTick=0;

    while (leftTick<7) followBasicLine(0);;

    Stop(0);

    delay(300);

    lost=false;

    //this calls the wavefront function found here.

    wavefront(1,5,x,y);                                         //do initial calculations

    //this function sets up the finalPath struct defined above.

    //dispalys the result

    currentMenu = wavefrontResult;

    drawCurrentMenu();

    detachInterrupt(digitalPinToInterrupt(button));             //button will not register as a lost click

    while (digitalRead(button)==LOW);                           //wait for button press

    DIR currentDirection = UP;

    long since = millis();

    for (int i=0;i<finalPath.steps;i++) {

      //code for a clockwise turn.

      if (finalPath.directions[i]!=currentDirection && (currentDirection+1)%4==finalPath.directions[i]) {

        Serial.println("Turn clockwise"); //turn clockwise

        rightMotorSpeed=-rightMotorDefaultSpeed/1.2;

        leftMotorSpeed=leftMotorDefaultSpeed/1.2;

        Forward(0);

        since=millis();

        while(digitalRead(LineSensor)==LOW || millis()-since<50);

        since=millis();

        while(digitalRead(LineSensor)==HIGH || millis()-since<50); //prevents accidenal triggering

        since=millis();

        while(digitalRead(LineSensor)==LOW || millis()-since<50);

        Stop(0);

        delay(250);

      //code for a coutner-clockwise turn

      else if (finalPath.directions[i]!=currentDirection && (currentDirection+3)%4==finalPath.directions[i]) {

        Serial.println("Turn c-clockwise"); //turn ccw

        rightMotorSpeed=rightMotorDefaultSpeed/1.1;

        leftMotorSpeed=-leftMotorDefaultSpeed/1.4;

        Forward(0);

        since=millis();

        while(digitalRead(LineSensor)==HIGH  || millis()-since<50);

        since=millis();

        while(digitalRead(LineSensor)==LOW || millis()-since<50);

        since=millis();

        while(digitalRead(LineSensor)==HIGH || millis()-since<50);

        Stop(0);

        delay(250);

      //robot can display lost message here, but it should never happen.

      else if (finalPath.directions[i]!=currentDirection) {

        Serial.println("robot is confused");

        lost=true;

        return;

      //then go straight until next intersection.

      Serial.println("Straight");

      currentDirection=finalPath.directions[i];

      followBasicLine(1);

      while (digitalRead(PaperSensor)==HIGH) followBasicLine(0);

      while (digitalRead(PaperSensor)==LOW); // cross the line

      leftTick=0;

      while (leftTick<7)  followBasicLine(0);;

      Stop(0);

      delay(300); //wait for delay

    //blink to show completion

    for (byte i=0;i<5;i++) {

      analogWrite(ledPin,255);

      delay(500);

      analogWrite(ledPin,0);

      delay(550);

    tmpx=MAP_WIDTH; //resets for next time

    seekLight(); //when the robot is done the maze, he will go to the light.

    //this is the same function as found on lab 6

received_422190214829702.mp4

Google Sites

Report abuse