Pages: 1 2 [3]   Go Down
Author Topic: Pathfinding robot...  (Read 3655 times)
0 Members and 1 Guest are viewing this topic.
FL
Offline Offline
Newbie
*
Karma: 0
Posts: 34
Its not that I can't explain it... Its just you wouldn't understand.
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
// 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)
  {
  }
}

Logged

FL
Offline Offline
Newbie
*
Karma: 0
Posts: 34
Its not that I can't explain it... Its just you wouldn't understand.
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
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:

//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;
}


Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 601
Posts: 48543
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

Pages: 1 2 [3]   Go Up
Jump to: