Go Down

Topic: Pathfinding robot... (Read 4624 times) previous topic - next topic

simonfrfr

This is an idea that I came up with for my wavefrount code. It is all that I have right now and I am trying to get it to work right. Thanks.


Code: [Select]

// Includes
#include <Servo.h>
//#include "TimerThree.h"
// Constants
const int TurnAmount   =1275;
const int GridSize   =15;
const int StrightAmount=1900;
const int RobotWidth = 640;
int scan_one_cell=740;//lower number makes it see further
const float DegToRad = 0.0174532925;
const int numReadings =5;
// Sensor byte indices - offsets in packets 0, 5 and 6
#define SenBumpDrop 0            
#define SenWall 1
#define SenCliffL 2
#define SenCliffFL 3
#define SenCliffFR 4
#define SenCliffR 5

#define Sen6Size 52
#define SenChAvailable  39


// Global variables
int NoSteps=0;
int timer_cnt = 0;
int timer_on = 0;
int sensors[Sen6Size];
int distance = 0;
signed long int angle = 0;

// Scanning Variables
unsigned long int scan_angle=750;//position of scanner, in units of servo command
unsigned int tmp=255;
unsigned int object_found=0;

int old_state=1;//records current pointed direction of robot: 1 is up, 2 is right, 3 is down, and 4 is left (clockwise)
int new_state=1;//required direction of robot
//X is vertical, Y is horizontal
int Map[GridSize][GridSize];
Servo myservo3;
Servo myservo2;
Servo myservo;
// Wave Front Variables
int nothing=0;
int wall=255;
int goal=1;
int robot=254;

//starting robot/goal locations
int robot_x=8;
int robot_y=8;
int goal_x=2;
int goal_y=2;
int cell_size=330;//the size of the robot

//Map locations
int x=0;
int y=0;

//temp variables
int counter=0;

//when searching for a node with a lower value
int minimum_node=250;
int min_node_location=0;
int reset_min=250;//anything above this number is a special item, ie a wall or robot
// Global variables

int turn_angle = 0;
int turn_dir = 1;
int turning = 0;



void setup()
{
   pinMode(2, OUTPUT);
 pinMode(3, OUTPUT);
 digitalWrite(2, HIGH);
 digitalWrite(3, HIGH);
 Serial.begin(9600);
 myservo.attach(11);  // attaches the servo on pin 9 to the servo object
//Timer3.initialize();
myservo2.attach(10);
myservo3.attach(9);
 setupStepper();

 for (int H=0; H<GridSize;H++)
 {
   for (int W=0; W<GridSize;W++)
   {
     Map[H][W]=0;
   }
 }
 ////////////////////Enter your code below this line////////////////////
 //reset encoder function
 angle = 0;
 distance = 0;
 //store robot location in Map
 Map[robot_x][robot_y]=robot;
 //store robot location in Map
 Map[goal_x][goal_y]=goal;
 // Drive around until a button or unsafe condition is detected
}


void loop()
{
 while(   (!sensors[SenCliffL])
   && (!sensors[SenCliffFL])
   && (!sensors[SenCliffFR])
&& (!sensors[SenCliffR])
   && (!sensors[SenChAvailable]) && Map[robot_x][robot_y]!=goal)//program ends when robot reaches goal
 {
   find_walls();
   new_state=propagate_wavefront();
   //////////////wavefront code//////////////
   while(new_state!=old_state && new_state!=0)
   {
     //if not pointed in the right direction, rotate
     if (abs(old_state - new_state) == 2)//rotate 180 degrees
     {
       rotate_CCW(180,250);
       rotate_CW(90,150);
     }
     else if ((signed int)(old_state - new_state) == -1 || (signed int)(old_state - new_state) == 3)//rotate 90 degrees CW
       rotate_CW(90,150);
     else if ((signed int)(old_state - new_state) == 1 || (signed int)(old_state - new_state) == -3)//rotate 90 degrees CCW
       rotate_CCW(90,150);
     //make new state the old state
     old_state=new_state;
     //recheck with new location
     find_walls();
     //find new location to go to
     new_state=propagate_wavefront();
   }
   //there is no solution
   //Serial.println(new_state);
   if (new_state==0)
   {
     delay(2500);
     //reset the entire Map
     clear_Map();
   }
   //go to new location
   else//move if there is a solution, otherwise dont move forward
   {
     if (new_state==1)
       robot_x--;
     if (new_state==2)
       robot_y++;
     if (new_state==3)
       robot_x++;
     if (new_state==4)
       robot_y--;
     
       straight(cell_size,100);//distance, velocity
     //update new location of robot
     
   }
   //make new state the old state
   if(new_state!=0)
     old_state=new_state;
 }
 if (Map[robot_x][robot_y]=goal)
 {
   if (goal_x=10)
     goal_x=2;
   else
     goal_x=10;

   Map[robot_x][robot_y]=robot;
   //store robot location in Map
   Map[goal_x][goal_y]=goal;    
 }
}



void rotate_CCW(signed long int angle_tmp, signed long int velocity)
{
 turnRight();
//  Timer3.attachInterrupt(MoveTurn, 900);
 angle=0;
 while(NoSteps!=0)
 {
 }
}


//rotate a certain angle - 100 = 90 degrees!
void rotate_CW(signed long int angle_tmp, signed long int velocity)
{
 turnLeft();
// Timer3.attachInterrupt(MoveTurn, 900);
 angle=0;
 while(NoSteps!=0)
 {
 }

}


//drive straight function (use negative velocity to go in reverse)
void straight(signed long int distance_tmp, signed long int velocity)
{
 stright();
//  Timer3.attachInterrupt(MoveStright, 900);
 
 //  Serial.println("A"); // send the map to the serial port, for the processing
   for (int H=0; H<GridSize;H++)
   {
     for (int W=0; W<GridSize;W++)
     {
       delay(9);
       Serial.println(Map[H][W]);
     }
   }
   while(NoSteps!=0)
 {
 }
}


simonfrfr

Code: [Select]

const int LeftDir =  40;
const int LeftStep =  41;
const int RightDir =  42;
const int RightStep =  43;


void MoveStright()
{
 if (NoSteps>StrightAmount-100)
 {
   
//   Timer3.stop();
//   Timer3.attachInterrupt(MoveStrightSoft, 1500);
 }
 else
 {
   stepBoth();
   NoSteps++;
 }

}


void MoveStrightSoft()
{
 if (NoSteps>StrightAmount)
 {
  // Timer3.stop();
   NoSteps=0;
 }
 else
 {
 //  Timer3.stop();
 //  Timer3.attachInterrupt(MoveStrightSoft, NoSteps);
   stepBoth();
   NoSteps++;
 }
}

void MoveTurn()
{
 if (NoSteps>TurnAmount)
 {
//   Timer3.stop();
   NoSteps=0;
 }
 else
 {
   stepBoth();
   NoSteps++;
 }
}




void setupStepper()
{
/* pinMode(LeftDir, OUTPUT);
 pinMode(LeftStep, OUTPUT);
 pinMode(RightDir, OUTPUT);
 pinMode(RightStep, OUTPUT);
 digitalWrite(LeftDir, HIGH);  //fwd
 digitalWrite(RightDir, LOW);//fwd */
 
}


void stepBoth()
{    
 //digitalWrite(LeftStep, HIGH);
// digitalWrite(RightStep, HIGH);
myservo3.writeMicroseconds(1200);
myservo2.writeMicroseconds(1200);
Serial.println("step");
 delay(1);
myservo3.write(90);
myservo2.write(90);
//  digitalWrite(LeftStep, LOW);
// digitalWrite(RightStep, LOW);
}

void turnLeft()
{
myservo3.writeMicroseconds(1200);
myservo.writeMicroseconds(120);
 Serial.println("left");
 //digitalWrite(LeftDir, HIGH); //Bwd
// digitalWrite(RightDir, HIGH);//fwd
}


void turnRight()
{
  myservo3.writeMicroseconds(1200);
myservo.writeMicroseconds(120);
 Serial.println("right");
//  digitalWrite(LeftDir, LOW); //fwd
//  digitalWrite(RightDir, LOW);//Bwd
}

void stright()
{
  Serial.println("fwd++");
  myservo3.writeMicroseconds(1200);
myservo2.writeMicroseconds(1200);
 //digitalWrite(LeftDir, HIGH);  //fwd
// digitalWrite(RightDir, LOW);//fwd
}



Code: [Select]


//WAVEFRONT ALGORITHM

int propagate_wavefront()
{
 //clear old wavefront
 unpropagate();
 counter=0;//reset the counter for each run!
 //Serial.print("hi_5");
 while(counter<50)//allows for recycling until robot is found
 {
   x=0;
   y=0;
   while(x<GridSize && y<GridSize)//while the Map hasnt been fully scanned
   {
     //if this location is a wall or the goal, just ignore it
     if (Map[x][y] != wall && Map[x][y] != goal)
     {
       //a full trail to the robot has been located, finished!
       if (min_surrounding_node_value(x, y) < reset_min && Map[x][y]==robot)
       {
         //finshed! tell robot to start moving down path
         return min_node_location;
       }
       //record a value in to this node
       else if (minimum_node!=reset_min)//if this isnt here, 'nothing' will go in the location
           Map[x][y]=minimum_node+1;
     }

     //go to next node and/or row
     y++;
     if (y==GridSize && x!=GridSize)
     {
       x++;
       y=0;
     }
   }
   counter++;
 }
 return 0;
}

void unpropagate()//clears old path to determine new path
{
 //stay within boundary
 for(x=0; x<GridSize; x++)
   for(y=0; y<GridSize; y++)
     if (Map[x][y] != wall && Map[x][y] != goal) //if this location is something, just ignore it
       Map[x][y] = nothing;//clear that space

 //store robot location in Map
 Map[robot_x][robot_y]=robot;
 //store robot location in Map
 Map[goal_x][goal_y]=goal;
}

//if no solution is found, delete all walls from Map
void clear_Map(void)
{
 for(x=0;x<GridSize;x++)
   for(y=0;y<GridSize;y++)
     if (Map[x][y] != robot && Map[x][y] != goal)
       Map[x][y]=nothing;
}

//this function looks at a node and returns the lowest value around that node
//1 is up, 2 is right, 3 is down, and 4 is left (clockwise)
int min_surrounding_node_value(int x, int y)
{
 minimum_node=reset_min;//reset minimum

 //down
 if(x <(GridSize-1))//not out of boundary
   if  (Map[x+1][y] < minimum_node && Map[x+1][y] != nothing)//find the lowest number node, and exclude empty nodes (0's)
   {
     minimum_node = Map[x+1][y];
     min_node_location=3;
   }

 //up
 if(x > 0)
   if  (Map[x-1][y] < minimum_node && Map[x-1][y] != nothing)
   {
     minimum_node = Map[x-1][y];
     min_node_location=1;
   }

 //right
 if(y < (GridSize-1))
   if  (Map[x][y+1] < minimum_node && Map[x][y+1] != nothing)
   {
     minimum_node = Map[x][y+1];
     min_node_location=2;
   }

 //left
 if(y > 0)
   if  (Map[x][y-1] < minimum_node && Map[x][y-1] != nothing)
   {
     minimum_node = Map[x][y-1];
     min_node_location=4;
   }

 return minimum_node;
}
//UPDATE Map BY SCANNING FOR WALLS
void find_walls(void)
{
 //do not scan outside the border region
 if((old_state==1 && robot_x==0) || (old_state==2 && robot_y==9) || (old_state==3 && robot_x==9) || (old_state==4 && robot_y==0))
   return;

 else
 {
   scan_angle=0;

   //reset scanner
   myservo.write(scan_angle);
   delay(40);


   object_found=0;

   //do a full scan until something is sensed in the way
   while(scan_angle <90 && object_found==0)
   {
     float volts;
     float distance2 = 0;
     float Wdistance2 = 0;
     float distance3 = 0;
     float Wdistance3 = 0;
     
     for (int index = 0; index<numReadings;index++)
     {            // take x number of readings from the sensor and average them
       distance2 += (1000- analogRead(A1));
       distance3 += (1000- analogRead(A0));
       delay(10);
     }
     distance2 /=numReadings;
     distance3 /=numReadings;
     Wdistance3 = abs(distance3 *cos((scan_angle+90)*DegToRad));
     Wdistance2 = abs(distance2 *cos(scan_angle*DegToRad));
     /*
     Serial.print(Wdistance2);
      Serial.print(" ");
      Serial.print(RobotWidth);
      Serial.print(" ");
      Serial.print(distance2);
      Serial.print(" ");
      Serial.println(scan_one_cell);
      */

     if ((distance2 < scan_one_cell && Wdistance2 < RobotWidth )||(distance3 < scan_one_cell && Wdistance3 < RobotWidth ))//object found: add it to the Map
     {
       //account for angle of the robot wrt the Map, and stay in boundaries
       if (old_state==1 && robot_x>0)
       {
         Map[robot_x-1][robot_y]=wall;
       }
       else if (old_state==2 && robot_y<(GridSize-1))
       {
         Map[robot_x][robot_y+1]=wall;
       }
       else if (old_state==3 && robot_x<(GridSize-1))
       {
         Map[robot_x+1][robot_y]=wall;
       }
       else if (old_state==4 && robot_y>0)
       {
         Map[robot_x][robot_y-1]=wall;
       }
       object_found=1;
     }

     //servo scan code
     //-90 degrees at 300, lower goes CCW, 750 is centered, 1200 is far CW
     myservo.write(scan_angle);
     delay(12);
     scan_angle++;
   }
   myservo.write(20);
   if(object_found==0) //no object found, so clear it in the Map
   {
     //account for angle of the robot wrt the Map, and stay in boundaries
     if (old_state==1 && robot_x>0)
       Map[robot_x-1][robot_y]=nothing;
     else if (old_state==2 && robot_y<(GridSize-1))
       Map[robot_x][robot_y+1]=nothing;
     else if (old_state==3 && robot_x<(GridSize-1))
       Map[robot_x+1][robot_y]=nothing;
     else if (old_state==4 && robot_y>0)
       Map[robot_x][robot_y-1]=nothing;
   }
 }
 //makes sure robot/goal location isnt replaced with a wall
 //store robot location in Map
 Map[robot_x][robot_y]=robot;
 //store robot location in Map
 Map[goal_x][goal_y]=goal;
}



PaulS

Quote
It is all that I have right now and I am trying to get it to work right.

That code does something. You want it to do something. Whether those two things are the same thing only you and Madame Toussant can tell us, and she has weekends off. Her crystal ball has some fuzzy spots and her eyesight isn't all that good, either. So, it's probably best if you tell us.

Go Up