wallfollowing code

Wallfollowing Code

#include <stdlib.h>
#include <stream.h>
#include <strings.h>
#include <math.h>

extern "C"
{
#include "../Nclient.h"
}

// state indices
int SONAR_BASE = 17;
int SONAR_MAX = 32;
int IR_BASE = 1;
int IR_MAX = 16;

// range limit
int IR_LIMIT = 14;

int TICK = 8;

int getShortestIndexIR(int, int);
int getShortestIndexIR();
int getShortestIndexSonar(int, int);
int getShortestIndexSonar();
void findWall(int);
bool followWall(int,unsigned int);
long bumpMask();
void iCorner(int);
int dist(int X, int Y);
int angTo(int X, int Y);
void bug1(int,int,int);
void bug2(int,int,int);

enum algol {WALL, BUG1, BUG2};

// integer distance computation
int dist(int X, int Y) {

int dx = (X-State[34]);
int dy = (Y-State[35]);

int d = sqrt(dx*dx+dy*dy);

return d;

}

// integer angle determination (*10 so fits units)
int angTo(int X, int Y) {

int dx = (X-State[34]);
int dy = (Y-State[35]);

double ang = atan2(dy,dx);
ang = (ang/(2*M_PI)*360);

int comp_ang = (ang*10.0);

cerr << "Angle computed to be " << comp_ang << endl;

return comp_ang;
}

// usage: bug port distance algol goalX goalY
int main(int argc, char** argv)
{

bool keepRunning = true;

int returnval;

algol alg = WALL;

int DESIRED_DISTANCE = 8;

int goalX = 0;
int goalY = 0;

if(argc > 1) { // they gave port or help
    if(!strcmp("-?",argv[1]) || !strcmp("-h",argv[1])) {
      cout << argv[0] << " port distance algol" << endl;
      exit(0);
    }
    SERV_TCP_PORT = atoi(argv[1]);
}
if(argc > 2) { // they gave distance
    DESIRED_DISTANCE = atoi(argv[2]);
}
if(argc > 3) { // they gave algolrithm
    if(argc > 5) { // they gave goal co-ords
      goalX = atoi(argv[4]);
      goalY = atoi(argv[5]);
    }
    if(!strcasecmp(argv[3],"bug1")) {
      alg = BUG1;
     }
     else if(!strcasecmp(argv[3],"bug2")) {
      alg = BUG2;
     }
}

// lets connect to the robot. if we don't, exit.
returnval = connect_robot(1);
if(returnval)
    {
      cout<<"We've made a connection"<<endl;
    }
else
    {
      cout<<"The robot doesn't want to talk to you. GO AWAY!"<<endl;
      exit(0);
    }

// zero the robot
zr();

// updates the state mask to all be 1, we'll look at everything.
for (int i = 0; i < 42; i++)
    {
      Smask[i] = 1;
    }
ct();
gs();

if(alg == WALL) {
    findWall(DESIRED_DISTANCE);
    // turn alongside wall so wall following code works better
    int index = getShortestIndexSonar();
    while(index != 13)
      {
      vm(0,350,350);
      gs();
      index = getShortestIndexSonar();
      }
    followWall(DESIRED_DISTANCE,-1);
}

else if(alg == BUG1) {
cout << "Going to (" << goalX << "," << goalY << ")\n";
bug1(goalX,goalY, DESIRED_DISTANCE);
}

else if(alg == BUG2) {
cout << "Going to (" << goalX << "," << goalY << ")\n";
bug2(goalX,goalY, DESIRED_DISTANCE);
}

// disconnect from the robot before exiting... this will keep him
// from being mad at us next time.
disconnect_robot(1);

return 0;
}

// This function turns the robot towards the goal and moves till it hits
// an obstacle. Then it follows the obstacle around until it finds the
// S-line again
void bug2(int goalX, int goalY, int DESIRED_DISTANCE) {

int s_ang; // slope of S-line

while(1) {
    //align with S-line
    int ang = angTo(goalX,goalY);
    s_ang = ang;
    cerr << "Angle to be set to " << ang << endl;
    int rot = ang - State[36];
    pr(0,rot,rot);
    while((State[36] - ang) > 20 || (ang - State[36]) > 20) {
      gs();
    }

    // drive till near an obstacle or goal
    gs();
    int front_dist = (State[1] + State[16] + State[2])/3;
    int goal_dist = dist(goalX,goalY);
    while(front_dist > DESIRED_DISTANCE && goal_dist > (10*DESIRED_DISTANCE)) {
      vm(200,0,0);
      gs();
      front_dist = (State[1] + State[16] + State[2])/3;
      goal_dist = dist(goalX,goalY);
    }
    if(goal_dist <= (10*DESIRED_DISTANCE)) break;

    // go around obstacle
    // turn alongside wall so wall following code works better
    int index = getShortestIndexSonar();
    while(index != 13)
      {
      vm(0,350,350);
      gs();
      index = getShortestIndexSonar();
      }

     // get away from initial point
    int iX = State[34];
    int iY = State[35];
    int oD = dist(goalX,goalY); // record distance of contact point
    while(dist(iX,iY) < (30*DESIRED_DISTANCE)) {
      followWall(DESIRED_DISTANCE,TICK);
      gs();
    }

     // follow till you hit sline (give or take five degrees)
    int c_ang = angTo(goalX,goalY);
    while(c_ang - ang > 50 || ang - c_ang > 50 || dist(goalX,goalY) > (oD+20)) {
      followWall(DESIRED_DISTANCE,TICK);
      gs();
      c_ang = angTo(goalX,goalY);
    }

}
}

// This function turns the robot towards the goal and moves till it hits
// an obstacle. Then it circumnavigates it, recording the closest point.
// Then it goes back to it and relaunches itself towards the start point
void bug1(int goalX, int goalY, int DESIRED_DISTANCE) {

int iX, iY; // initial X and Y positions to circumnavigate obstacle

while(1) {

    // align with S-line
    gs();
    int ang = angTo(goalX,goalY);
    cerr << "Angle to be set to " << ang << endl;
    int rot = ang - State[36];
    pr(0,rot,rot);
    while((State[36] - ang) > 20 || (ang - State[36]) > 20) {
      gs();
    }

    // drive till near an obstacle or goal
    gs();
    int front_dist = (State[1] + State[16] + State[2])/3;
    int goal_dist = dist(goalX,goalY);
    while(front_dist > DESIRED_DISTANCE && goal_dist > (10*DESIRED_DISTANCE)) {
      vm(200,0,0);
      gs();
      front_dist = (State[1] + State[16] + State[2])/3;
      goal_dist = dist(goalX,goalY);
    }
    if(goal_dist < (10*DESIRED_DISTANCE)) break;

    // go around obstacle
    iX = State[34];
    iY = State[35];
    int bestX = iX;
    int bestY = iY;
    int bestD = dist(goalX,goalY);

    // turn alongside wall so wall following code works better
    int index = getShortestIndexSonar();
    while(index != 13)
      {
      vm(0,350,350);
      gs();
      index = getShortestIndexSonar();
      }

      // get away from initial point
    while(dist(iX,iY) < (30*DESIRED_DISTANCE)) {
      followWall(DESIRED_DISTANCE,TICK);
      int D = dist(goalX,goalY);
      if(D < bestD) {
        bestD = D;
        bestX = State[34];
        bestY = State[35];
      }
      gs();
    }
      // now, get back to initial point
    while(dist(iX,iY) > (20*DESIRED_DISTANCE)) {
      followWall(DESIRED_DISTANCE,TICK);
      int D = dist(goalX,goalY);
      if(D < bestD) {
        bestD = D;
        bestX = State[34];
        bestY = State[35];
      }
      gs();
    }
    cout << "Back where we started!\n";
      // now go to the best point
    while(dist(bestX,bestY) > (20*DESIRED_DISTANCE)) {
      followWall(DESIRED_DISTANCE,TICK);
      gs();
    }
}

}

// this function gets run when the robot is far from walls. it will search
// out the closest wall and get DESIRED_DISTANCE away from it.
// the function uses PID control to get to the desired distance
void findWall(int DESIRED_DISTANCE)
{

long error = 0;
long errorLast;
long errorSum = 0;

double Kp = .7;
double Kd = .35;
double Ki = .15;

int tv = 0;
int rv = 0;

// this points the robot towards the nearest wall
int index = getShortestIndexSonar();
while(index != 1)
    {
      rv = 350;
      vm(tv,rv,rv);
      gs();
      index = getShortestIndexSonar();
    }

rv = 0;

gs();

error = State[getShortestIndexIR()] - DESIRED_DISTANCE;
errorLast = error;

// This does the PID control to get to desired distance
while(error != 0 || errorLast != 0)
    {
      errorLast = error;
      error = State[getShortestIndexIR()] - DESIRED_DISTANCE;
      errorSum = errorSum + error;

      // handling for integral windup
      if(errorSum > 250)
        {
          errorSum = 250;
        }
      if(errorSum < -250)
        {
          errorSum = -250;
        }

      // if you hit a wall, back up, else use the PID computed velocity
      if(State[33] & bumpMask() != 0)
        {
          tv = -5;
        }
      else
        {
          tv = Kp*error + Kd*(error - errorLast) + Ki*errorSum;
        }

      vm(tv,rv,rv);
      gs();
    }
}

// function assumes you start within IR range
// it attempts to follow a wall by tackling a few special cases
// the ticks limit is for the bug algorithms
// uses PD control to maintain distance from wall
bool followWall(int DESIRED_DISTANCE, unsigned int tick_limit)
{

static bool turn_corner = false;

unsigned int ticks = 0;
int tv = 0;
int rv = 0;

int error = 0;
int errorLast = 0;

double Kp = 4;
double Kd = 3;

while(ticks < tick_limit) {
gs();

    int dist_front = (State[1] + State[16] + State[2])/3;
    int dist_fr = State[getShortestIndexIR(14,16)];
    int dist_right = State[getShortestIndexIR(12,14)];
    int closest_sensor = getShortestIndexIR();
    int dist_near = State[closest_sensor];

errorLast = error;
error = DESIRED_DISTANCE - dist_right;

    // Nothing to the front, forge ahead and try to match distance
    if(dist_front > IR_LIMIT && dist_right <= DESIRED_DISTANCE + 1) {
      if(dist_near < DESIRED_DISTANCE/2) {
        tv = 50;
      }
      else {
        tv = 200;
      }
      rv = Kp*error + Kd*(error-errorLast);
    }

    // Something to the front, curve to the left
    else if(dist_front <= IR_LIMIT) {
      if(dist_near >= DESIRED_DISTANCE) { // wall approaching, gently curve
        tv = 50;
        rv = 100;
      }
      else if(dist_near > DESIRED_DISTANCE/2) { // closer than we should be, but not an emergency
        tv = 40;
        rv = 150;
      }
      else { // AGGGHHH!
        tv = 20;
        rv = 200;
      }
    }

    // Nothing at all curve to the right
    else {
      if(dist_near > IR_LIMIT) {
        tv = 100;
        rv = -350;
      }
      else { // something is still in range, be gentler
        tv = 80;
        rv = -200;
      }
    }

vm(tv,rv,rv);
++ticks;
}

return true;
}

// this returns which sonar has the closest obstacle in front of it.
// it returns a number from 1 to 16, instead of the index into State
// takes bounds
int getShortestIndexSonar() { return getShortestIndexSonar(SONAR_BASE,SONAR_MAX); }
int getShortestIndexSonar(int lowBound, int highBound)
{
gs();
long closestValue = State[lowBound];
int index = lowBound;
for (int i = lowBound; i <= highBound; i++)
    {
      if(State[i] < closestValue)
        {
          closestValue = State[i];
          index = i;
        }
    }
return (index - SONAR_BASE + 1);
}

// this returns which sonar has the closest obstacle in front of it.
// it returns a number from 1 to 16, instead of the index into State
// takes bounds
int getShortestIndexIR() { return getShortestIndexIR(IR_BASE,IR_MAX); }
int getShortestIndexIR(int lowBound, int highBound)
{
gs();
long closestValue = State[lowBound];
int index = lowBound;
for (int i = lowBound; i <= highBound; i++)
    {
      if(State[i] < closestValue)
        {
          closestValue = State[i];
          index = i;
        }
    }
return (index);
}

// this function returns a mask for the bump sensors. the mask can be
// ANDed with the State long for bump sensors to look at only the bump
// sensors in the front half of the robot.
long bumpMask()
{
long ret = 0;

if(State[36] < 180) ret = 0x000f803f;
else if(State[36] < 360) ret = 0x000f007f;
else if(State[36] < 540) ret = 0x000e00ff;
else if(State[36] < 720) ret = 0x000c01ff;
else if(State[36] < 900) ret = 0x000803ff;
else if(State[36] < 1080) ret = 0x000007ff;
else if(State[36] < 1260) ret = 0x00000ffe;
else if(State[36] < 1440) ret = 0x00001ffc;
else if(State[36] < 1620) ret = 0x00003ff8;
else if(State[36] < 1800) ret = 0x00007ff0;
else if(State[36] < 1980) ret = 0x0000ffe0;
else if(State[36] < 2160) ret = 0x0001ffc0;
else if(State[36] < 2340) ret = 0x0003ff80;
else if(State[36] < 2520) ret = 0x0007ff00;
else if(State[36] < 2700) ret = 0x000ffe00;
else if(State[36] < 2880) ret = 0x000ffc01;
else if(State[36] < 3060) ret = 0x000ff803;
else if(State[36] < 3240) ret = 0x000ff007;
else if(State[36] < 3420) ret = 0x000fe00f;
else ret = 0x000fc01f;

return ret;
}

// this handles inside corners. The robot will approach the wall and stop
// when it is 15 away. Then it will turn 90 degrees counterclockwise.
// there is a delay function, because the ws() command would not compile.
// so it gives a decent amount of time for the motors to move.
void iCorner(int DESIRED_DISTANCE)
{
while(State[1] != DESIRED_DISTANCE)
    {
      vm(50*(State[1] - DESIRED_DISTANCE)/(15-DESIRED_DISTANCE),0,0);
      gs();
    }
vm(0,0,0);
gs();

pr(0,900,900);
for(int i = 0 ; i < 25 ; i++)
    {
      gs();
    }

return;
}

Back