ROBOTC.net forums
http://www.robotc.net/forums/

NEW PROJECT: Path-Finding-Robot
http://www.robotc.net/forums/viewtopic.php?f=55&t=11546
Page 1 of 1

Author:  Sigtrygg [ Sat Jun 20, 2015 2:35 pm ]
Post subject:  NEW PROJECT: Path-Finding-Robot

Hello community!

I found the algorithm of path finding under forum from RobotC:
viewtopic.php?f=15&t=5616&hilit=map
Azhari put it in. The code was written by Tim Friez.
I modified it as I needed it.
It is very easy to modify the matrix/grid and change the starting point of robot and the goal.
I coded it for my LEGO NXT REM-Bot, but it works without changings in RVW.

Watch the video on Youtube:


Here ist the code:
Code:
#pragma config(Sensor, S1,       touchSensor,       sensorTouch)
#pragma config(Sensor, S3,       HTGYRO,          sensorAnalogInactive)
#pragma config(Sensor, S4,       sonarSensor,       sensorSONAR)
#pragma config(Motor,    motorA,               sonarMotor,       tmotorNXT, PIDControl, encoder)
#pragma config(Motor,    motorB,               rightMotor,       tmotorNXT, PIDControl, encoder)
#pragma config(Motor,    motorC,               leftMotor,       tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard

//Map-Navigation in RVW, Robot: Buggy-Bot with gyro, by Sigtrygg 2015

/* Credits :
Special thanks to to Xander Soldaat for gyro drivers and example codes!
I found the algorithm of path finding under forum from RobotC:
http://www.robotc.net/forums/viewtopic.php?f=15&t=5616&hilit=map
Azhari put it in, but I don't know who made it.
I modified it as I needed it.
*/

//used map: Path_Finder2.rvl
//used robot: Buggy-Bot

//includes
#include "drivers/hitechnic-gyro.h"

//GLOBAL VARIABLES grid world dimensions
const int x_size = 11;
const int y_size = 3;

//GLOBAL ARRAY representation of grid world using a 2-Dimensional array
//0  = open space
//1  = barrier
//2  = goal
//99 = robot
int map[x_size][y_size] =
{   {2,1,0},
   {0,0,0},
   {0,1,1},
   {0,0,0},
   {1,0,1},
   {0,0,0},
   {0,1,99},
   {0,0,0},
   {1,0,1},
   {0,0,0},
   {0,0,0}};

//******* FUNCTION turn left or right for specific angle while using a GYRO-sensor ***
void turn(int degrees, char direction , int velocity);

//FUNCTION move forward for a variable number of grid blocks
void moveForward(int blocks)
{
   //convert number of blocks to encoder counts
   //wheel circumference = 18,85;   //(cm) Buggy-Bot
   //one block = 30,48.cm = 12''
   int countsToTravel = (30.48/18.85)*(360)*blocks;
   nSyncedMotors= synchBC;  // sync motors B and C
    nSyncedTurnRatio = 100;
   //encoder target for countsToTravel
   nMotorEncoder[rightMotor] = 0;
   nMotorEncoderTarget[rightMotor] = countsToTravel;
   motor[rightMotor] = 60;
   while(nMotorRunState[rightMotor] != runStateIdle)
   {
      // do not continue
   }
   motor[rightMotor] = 0;
   wait1Msec(150);
}

//******* FUNCTION turn left or right for specific angle while using a GYRO-sensor ***
void turn(int degrees, char direction, int velocity)
{
   //correction is necessary because of drifting !!
   int correction=0;

   if (degrees==90)
      correction=4; //four degrees correction

   float rotSpeed = 0;
   float heading = 0;
   nSyncedMotors = synchNone;

   // Reset the timer.
   clearTimer(T1);

   while (abs(heading) < (degrees-correction))
   {
      // Wait until 20ms has passed
      while (time1[T1] < 20)
         wait1Msec(1);

      // Reset the timer
      clearTimer(T1);

      // Read the current rotation speed
      rotSpeed = HTGYROreadRot(HTGYRO);

      // Calculate the new heading by adding the amount of degrees
      // we've turned in the last 20ms
      // If our current rate of rotation is 100 degrees/second,
      // then we will have turned 100 * (20/1000) = 2 degrees since
      // the last time we measured.
      heading += rotSpeed * 0.02;

      // Display our current heading on the screen
      //nxtDisplayCenteredBigTextLine(3, "%2.0f", heading);

      if (direction == 'r')
      {
         motor[rightMotor] = -velocity;
         motor[leftMotor] = velocity;
      }
      else if (direction == 'l')
      {
         motor[rightMotor] = velocity;
         motor[leftMotor] = -velocity;
      }
   }

   //stop
   motor[rightMotor] = 0;
   motor[leftMotor] = 0;
   wait1Msec(150);
}


//FUNCTION print wavefront map to NXT screen
void PrintWavefrontMap()
{
   int x_pos=10, y_pos=60;
   string map_string="";

   for(int x=0; x < x_size; x++)

   {
      for(int y=0; y < y_size; y++)
      {
         if(map[x][y] == 99)
         {
            nxtDisplayStringAt (x_pos,y_pos,"R");


         }
         else if(map[x][y] == 2)
         {
            nxtDisplayStringAt (x_pos,y_pos, "G");


         }
         else if(map[x][y] == 1)
         {
            nxtDisplayStringAt (x_pos,y_pos, "X");


         }
         else if(map[x][y] == '*')
         {
            nxtDisplayStringAt (x_pos,y_pos, "*");

         }
         else
         {

            sprintf(map_string,"%d",map[x][y]);
            nxtDisplayStringAt (x_pos,y_pos, map_string);
            wait1Msec(5);

         }
         x_pos=x_pos+10;
      }
      x_pos=10;
      y_pos=y_pos-8;
   }
}


//FUNCTION wavefront algorithm to find most efficient path to goal
void WavefrontSearch()
{
   int goal_x, goal_y;
   bool foundWave = true;
   int currentWave = 2; //Looking for goal first

   while(foundWave == true)
   {
      foundWave = false;
      for(int y=0; y < y_size; y++) //0-5
      {
         for(int x=0; x < x_size; x++) //0-10
         {
            if(map[x][y] == currentWave) //bei x=7 und Y=2
            {
               foundWave = true;
               goal_x = x; //7
               goal_y = y; //2

               if(goal_x > 0) //This code checks the array bounds heading WEST
                  if(map[goal_x-1][goal_y] == 0)  //This code checks the WEST direction
                  map[goal_x-1][goal_y] = currentWave + 1;

               if(goal_x < (x_size - 1)) //This code checks the array bounds heading EAST
                  if(map[goal_x+1][goal_y] == 0)//This code checks the EAST direction
                  map[goal_x+1][goal_y] = currentWave + 1;

               if(goal_y > 0)//This code checks the array bounds heading SOUTH
                  if(map[goal_x][goal_y-1] == 0) //This code checks the SOUTH direction
                  map[goal_x][goal_y-1] = currentWave + 1;

               if(goal_y < (y_size - 1))//This code checks the array bounds heading NORTH
                  if(map[goal_x][goal_y+1] == 0) //This code checks the NORTH direction
                  map[goal_x][goal_y+1] = currentWave + 1;
            }
         }
      }
      currentWave++;
      PrintWavefrontMap();
      wait1Msec(50);
   }
}

//FUNCTION follow most efficient path to goal
//and update screen map as robot moves
void NavigateToGoal()
{
   //Store our Robots Current Position
   int robot_x, robot_y;

   //First - Find Goal and Target Locations
   for(int x=0; x < x_size; x++)
   {
      for(int y=0; y < y_size; y++)
      {
         if(map[x][y] == 99)
         {
            robot_x = x;
            robot_y = y;
         }
      }
   }

   //Found Goal and Target, start deciding our next path
   int current_x = robot_x;
   int current_y = robot_y;
   int current_facing = 3;
   int next_Direction = 3;
   int current_low = 99;

   while(current_low > 2)
   {
      current_low = 99; //Every time, reset to highest number (robot)
      next_Direction = current_facing;
      int Next_X = 0;
      int Next_Y = 0;

      //Check Array Bounds West
      if(current_x > 0)
         if(map[current_x-1][current_y] < current_low && map[current_x-1][current_y] != 1) //Is current space occupied?
      {
         current_low = map[current_x-1][current_y];  //Set next number
         next_Direction = 3; //Set Next Direction as West
         Next_X = current_x-1;
         Next_Y = current_y;
      }

      //Check Array Bounds East
      if(current_x < (x_size -1))
         if(map[current_x+1][current_y] < current_low && map[current_x+1][current_y] != 1) //Is current space occupied?
      {
         current_low = map[current_x+1][current_y];  //Set next number
         next_Direction = 1; //Set Next Direction as East
         Next_X = current_x+1;
         Next_Y = current_y;
      }

      //Check Array Bounds South
      if(current_y > 0)
         if(map[current_x][current_y-1] < current_low && map[current_x][current_y-1] != 1)
      {
         current_low = map[current_x][current_y-1];  //Set next number
         next_Direction = 2; //Set Next Direction as South
         Next_X = current_x;
         Next_Y = current_y-1;
      }

      //Check Array Bounds North
      if(current_y < (y_size - 1))
         if(map[current_x][current_y+1] < current_low && map[current_x][current_y+1] != 1) //Is current space occupied?
      {
         current_low = map[current_x][current_y+1];  //Set next number
         next_Direction = 0; //Set Next Direction as North
         Next_X = current_x;
         Next_Y = current_y+1;
      }

      //Okay - We know the number we're heading for, the direction and the coordinates.
      current_x = Next_X;
      current_y = Next_Y;
      map[current_x][current_y] = '*';

      //Track the robot's heading
      while(current_facing != next_Direction)
      {
         if (current_facing > next_Direction)
         {
            turn(90, 'l', 20);
            current_facing--;
         }
         else if(current_facing < next_Direction)
         {
            turn(90, 'r', 20);
            current_facing++;
         }
      }
      moveForward(1);
         eraseDisplay();
      PrintWavefrontMap();
      wait1Msec(50);
   }
}

task main()
{
   HTGYROstartCal(HTGYRO);
   wait1Msec (1000);
   WavefrontSearch();    //Build map of route with wavefront algorithm
   NavigateToGoal(); //Follow most efficient path to goal
   wait1Msec(5000);  //Leave time to view the LCD screen
}


Hope, you have fun to try it.

Bye,

Sigtrygg

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/