View unanswered posts | View active topics It is currently Sun Dec 16, 2018 1:21 pm






Reply to topic  [ 1 post ] 
NEW PROJECT: Path-Finding-Robot 
Author Message
Rookie
User avatar

Joined: Mon Apr 08, 2013 12:50 pm
Posts: 30
Post 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


Sat Jun 20, 2015 2:35 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 1 guest


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.