View unanswered posts | View active topics It is currently Fri Apr 25, 2014 4:47 am






Reply to topic  [ 7 posts ] 
BUG2 ALgorithm Implementation 
Author Message
Rookie

Joined: Sun Mar 02, 2008 9:41 am
Posts: 25
Post BUG2 ALgorithm Implementation
HI guys,

As part of my project I had to implement the BUG2 algorithm in NXT.

The whole project was to implement 4 such algorithms and compare them.

BUG2 is an algorithm which achieves autonomous navigation in unknown environments by using only touch sensing. It uses no RTOS as this is a limitation set.

It is not legal to give the final version here. I present a version which is almost full working.

To get it working you need to set the following variables:

float TY = 120.0; //The Target Y coordinate
float TX = 70.0; // The Target X xoordinate
const float WheelDistance= 11.6 ; //the distance between the two wheels
const float wheelDiameter = 5.6;//diameter of the wheel


It is somehow an old version and a bit dirty but it works almost fine.

Hope you find it interesting.

Here is the code:

Code:
//*!!Sensor,    S1,          touchSensor, sensorTouch,      ,                    !!*//
//*!!                                                                            !!*//
//*!!Start automatically generated configuration code.                           !!*//
const tSensors touchSensor          = (tSensors) S1;   //sensorTouch        //*!!!!*//


//******************************************************************************//
//            Motors & Sensors                                                                 //
//                                                                              //
//  [I/O Port]     [Name]          [Type]               [Description]           //
//  Port 1         touchsensor     Touch                Front mounted           //
//  Port A         none            Motor                Right Motor             //
//  Port B         none            Motor                Left Motor              //
//                                                                              //
//******************************************************************************//

#define navMoveTurn      -100      // Mot. synchron
const float WheelDistance= 11.6 ; //the distance between the two wheels
const float wheelDiameter = 5.6;//diameter of the wheel
float TY = 120.0; //The Target Y coordinate
float TX = 70.0; // The Target X xoordinate
float CY = 0.0; // To hold the current Y coordinate
float CX = 0.0;// To hold the current X coordinate
float wheelCircumference; // To hold the wheel circumference
float axisCircumference; // To hold the axis circumference
float cmPerTick; // How many cm the robot travels per rotation tick
float distanceReturned;
float degreesToTurn;
float ticksMoved; //to show at how many ticks the obstacle was encountered
int length; //to store the length of the straight line (Start, Target)
/*these variables store the leave and hit point of the obstacle encountered*/
float obstacleXStart;
float obstacleYStart;
float obstacleXEnd;
float obstacleYEnd;

/*received a distance in centimetres
**and returns the dstance in rotating ticks*/
float calculateRotating( float i){
  float distance;
   distance = i / cmPerTick;
   return distance;

}
/*computes the straight distances
** from the robots current position to T*/
float computeST(float i, float x) {
   float length = (i * i) + (x * x);
   length = sqrt(length);

   return length;
}
/*function to switch of the motors*/
void haltMotors(){
   motor[motorA] = 0;
  motor[motorB] = 0;
  nMotorEncoder[motorA] = 0;
  nMotorEncoder[motorB] = 0;

}
/*****************************************************************************************************/
/*function to calculate the circumference of the robot wheels*/
void calWheelCircumference(){
   wheelCircumference = wheelDiameter * (float)PI;
}
/*function to calculate the circumference of the circle of the whole robot*/
void calAxisCircumference(){
   axisCircumference  = WheelDistance * (float)PI;
}
/*function to calculate the distance covered per tick rotation*/
void calCMPerTick(){
   cmPerTick  = wheelCircumference / 360;
}
 /** A function to turn a specified number of degrees
   * @param angle Angle to rotate in degrees.
   * A positive value rotates left, a negative value right.
   */
void rotate(float angle) {
   float axisCMPerDegree    = axisCircumference / 360;
   float distanceToTravel    = axisCMPerDegree * angle;
   float ticks                    = distanceToTravel / cmPerTick;
  haltMotors();
   if (angle > 0){
      nSyncedMotors = synchAB;
        nSyncedTurnRatio=-100;
        if (angle > 85.0) {
          motor[motorA] = 20;
       }
       else if (angle < 85.0) {
           motor[motorA] = 13;
       }
       while (nMotorEncoder[motorA] < ticks) {}
   }
  if (angle < 0) {
      nxtDisplayTextLine(2,"ticks %f",ticks);
       //  wait1Msec(5000);
      nSyncedMotors = synchBA;
      nSyncedTurnRatio = -100;
      if (angle * -1 > 85.0) {
          motor[motorB] = 20;
       }
       else if (angle * -1 < 85.0) {
           motor[motorB] = 13;
       }
      while(nMotorEncoder[motorB] < ticks  * -1) {nxtDisplayTextLine(2,"angle %f",angle);}
   }
   nSyncedMotors = synchNone;
   nSyncedTurnRatio  = 0;
   nxtDisplayTextLine(1," ");
    haltMotors();
 }
void moveBackwards(float distance) {
    haltMotors();
    nSyncedMotors = synchAB;
    nSyncedTurnRatio = +100;
   motor[motorA] = -20;
   while(nMotorEncoder[motorA] > distance * -1){}
   nSyncedMotors = synchNone;
    haltMotors();
 }
/*function to move straight a number of rotatings*/
int moveStraight(float distance) {
    int i;
    haltMotors();
    nSyncedMotors = synchAB;
    nSyncedTurnRatio = +100;
   motor[motorA] = 20;
   while(nMotorEncoder[motorA] < distance && SensorValue(touchSensor) == 0) {
     nxtDisplayTextLine(1,"DR %f", distanceReturned);

    }

    /*if an obstacle is touched before we have reached the target*/
   if (SensorValue(touchSensor) == 1 && nMotorEncoder[motorA] < distance) {
        motor[motorA] = 0;
       motor[motorB] = 0;
         ticksMoved = nMotorEncoder[motorA];
         i = 1;
    }
    /*if the target is reached and no obstacles have been encountered*/
    else if ( nMotorEncoder[motorA] ==  distance || nMotorEncoder[motorA] >  distance ) {
         motor[motorA] = 0;
       motor[motorB] = 0;
         ticksMoved = nMotorEncoder[motorA];
        i = 0;
    }
   nSyncedMotors = synchNone;
   haltMotors();
   return i;
}
/*Target Reachability Test*/
int exchaustObstacle() {
  int i;
  rotate(degreesToTurn * -1);
  i = 1;
  degreesToTurn = 0;
  moveBackwards(20 / cmPerTick);
  CY += 20.0;
  while (i == 1) {
       rotate(90);
       i = moveStraight(10 / cmPerTick);
       if (i == 1) {

       }
       if (i == 0) {
             CX -= 10.0;
             rotate(-90.0);
             i = moveStraight(22 / cmPerTick);
             if (i == 1) {
                    moveBackwards(ticksMoved);
             }
             if (i == 0) {
                    CY -= 22.0;
             }
       }
   }//first while i == 1
   if (TY > obstacleYStart && TY < CY) {
          if (CX == TX) {

          }
          else if (CX > TX) {

          }
          else if (CX < TX) {// it the target is right
              nxtDisplayTextLine(1,"in 2 if");
                wait1Msec(5000);wait1Msec(5000);
                i = 1;
                while (i == 1 && CY > obstacleYStart) {
                      rotate(-90.0);

                       i = moveStraight((TX - CX) / cmPerTick);
                      if (i == 1) {

                            moveBackwards(ticksMoved);
                            rotate(90.0);
                            i = moveStraight(10 / cmPerTick);
                            if (i == 0) {
                            CY -= 10;
                            i = 1;
                            }
                      }
                      if (i == 0){
                                          CX += TX - CX;
                                          rotate(-90.0);

                      }
                      if (obstacleYStart > CY ) {
                             eraseDisplay();
                             PlaySound(soundBeepBeep);PlaySound(soundBeepBeep);
                             powerOff();
                      }
                 }
            }

          }
  wait1Msec(2000);wait1Msec(2000);wait1Msec(2000);
  if (CX == TX) {
        i = 0;
  }
  else if (CX != TX) {
        i =  1;
  }

  return i;
}

/*this function computes the new straight line (Start, Target)*/
int  newSTLine() {
   nxtDisplayTextLine(1,"on newSTLine");
   wait1Msec(2000);
   float tempY;
   int i;
   float tempX;
  if (CX != TX && CY != TY) {
       nxtDisplayTextLine(1,"nothing equal");
       wait1Msec(2000);
       if (TY < CY) {
             nxtDisplayTextLine(1,"TY < CY");
             rotate(180.0);
              tempY =  CY - TY;
       }
       else if (TY > CY) {  nxtDisplayTextLine(1,"target is further up");
            tempY = TY - CY;
       }
       if (TX > CX ) {nxtDisplayTextLine(1,"T left");
            tempX = TX - CX;
       }

       else if (TX < CX ) {nxtDisplayTextLine(1,"target right");
            tempX = CX - TX;
       }
     eraseDisplay();

     length = computeST(tempY, tempX);
     degreesToTurn = ((float)tempX / (float)length);
     degreesToTurn = asin(degreesToTurn)* 180.0 / PI;

     if ((TX > CX && CY > TY) || (TX < CX && CY < TY)) {
          nxtDisplayTextLine(1,"turn opposite");
          degreesToTurn = degreesToTurn * -1;
        }

        eraseDisplay();
        nxtDisplayTextLine(1,"Rotating");
        nxtDisplayTextLine(2," %f", degreesToTurn);
      wait1Msec(5000);//wait1Msec(5000);
            rotate(degreesToTurn);

        distanceReturned = calculateRotating((float)length);
        i = moveStraight(distanceReturned);

      if (i == 0) {
         haltMotors();
       }
     if (i == 1) {
        haltMotors();

        if (TX > CX ) {
              CX += ((ticksMoved * cmPerTick) * sinDegrees( degreesToTurn));
          }
          else if (TX < CX) {
             CX -= ((ticksMoved * cmPerTick) * sinDegrees( degreesToTurn));
          }
          if (TY > CY) {
            CY += ((ticksMoved * cmPerTick) * cosDegrees( degreesToTurn));
          }
          else if (TY < CY) {
             CY -= ((ticksMoved * cmPerTick) * cosDegrees( degreesToTurn));
          }
          if ( TY < CY && TX < CX) {
                  eraseDisplay();
                  nxtDisplayTextLine(4,"TarY %f", TY);
                 i = exchaustObstacle();
                 if (i == 0) {
                       if (CY > TY){
                             rotate(180.0);
                             i = moveStraight((CY - TY) / cmPerTick);
                       }
                       else if ( CY < TY) {
                              i = moveStraight((TY - CY) / cmPerTick);
                        }
                        if (i == 0) {
                             haltMotors();
                        }
                 }

        }
        else {
               rotate(degreesToTurn * -1);
        }
       }
   }
  return i;
}

/*a function to decentralize the robot from
**the coordinates of the target if these are equal*/
void decentralize() {
    if (CX == TX) {
         rotate(90);
         int i = moveStraight(10 /cmPerTick);
         if (i == 1) {
               moveBackwards(ticksMoved + (10.0 / cmPerTick));
               rotate(-90.0);
               CX -= 10.0;
         }
         else if (i == 0) {
               CX += 10.0;
               rotate(-90.0);
          }
      }
      if (TY == CY) {
            int k = moveStraight(10 / cmPerTick);
                  if (k == 1) {
                        moveBackwards(ticksMoved + (10.0 / cmPerTick));
                        CY -= 10.0;
                  }
                  else if (k == 0) {
                        CY += 10.0;
                  }
      }

}
void followBoundary(){
   float degreesTurned;
   float turning;
   float opposite;
   float adjacent;
   float hypotenuse;
   int i = 1;



   obstacleXStart = CX;
   obstacleYStart = CY;

   turning = 10.0;


  moveBackwards(20.0 / cmPerTick);//move back 20 cm
  CY = CY - 20.0; // position changed, update the Y coordinate
  adjacent = 20.0;
  /*new code*/
   while (i == 1) {
            eraseDisplay();
           rotate(turning);//turn to check obstacle
          degreesTurned += turning;//update variables
          nxtDisplayTextLine(1,"degTurn " );
            nxtDisplayTextLine(2,"%f",degreesTurned );

          nxtDisplayTextLine(4,"deg %f",degreesTurned );
          hypotenuse = adjacent /cosDegrees(degreesTurned);
            opposite = sinDegrees(degreesTurned) * hypotenuse;
            i = moveStraight(hypotenuse / cmPerTick);
           nxtDisplayTextLine(2,"i == %d",i );

          if (i == 1) {nxtDisplayTextLine(1,"i == 1");
                 moveBackwards(ticksMoved);
                 if (degreesTurned > 79.0) {nxtDisplayTextLine(1,"dg > 79");
                         rotate (90.0 - degreesTurned);
                         degreesTurned += 90.0 - degreesTurned;
                         int q = moveStraight ((opposite + 10.0) / cmPerTick );
                         if (q == 1) {nxtDisplayTextLine(1,"q == 1");
                                rotate(((degreesTurned * -1) * 2));
                                degreesTurned += ((degreesTurned * -1) * 2);
                                int w = moveStraight(ticksMoved + ( 20.0 / cmPerTick));
                                if (w == 1) {nxtDisplayTextLine(1,"w == 1");
                                      rotate (90.0);
                                      degreesTurned += 90.0;
                                      CX = CX - (ticksMoved * cmPerTick);

                                }
                                if (w == 0) {nxtDisplayTextLine(1,"w == 0");
                                             rotate (90.0);
                                      degreesTurned += 90.0;
                                      CX = CX - (ticksMoved * cmPerTick);

                               }
                        }
                       else if (q == 0) {nxtDisplayTextLine(1,"q == 01");
                               CX += opposite + 10.0;
                               rotate(-90.0);
                               degreesTurned += -90.0;
                               int s = moveStraight(20.0 / cmPerTick);
                               if (s == 0) {nxtDisplayTextLine(1,"s == 0");

                                       obstacleXEnd = CX;
                                     obstacleYEnd = CY;
                                      CY += 20.0;
                                      eraseDisplay();
                               }
                               if ( s == 1) {nxtDisplayTextLine(1,"s == 1");
                                     moveBackwards(ticksMoved);

                               }
                      }
                  }
         }
         else if (i == 0) {nxtDisplayTextLine(1,"else i == 0");
                    CX += opposite;
                    CY += 20.0 ;
                    rotate(degreesTurned * -1);
                    obstacleXEnd = CX;
                 obstacleYEnd = CY;
         }
   }
   /*on this point the robot should always by facing the +Y direction (UP)*/
   if (CX > TX) {nxtDisplayTextLine(1,"CX > TX");
          int x = 0;
            while (x == 0){
                    int a = moveStraight( 10 / cmPerTick);
                    if (a == 0) {nxtDisplayTextLine(1,"a == 0");
                             CY += 10.0;
                          rotate(-90);
                          int l  =  moveStraight((opposite / 2) / cmPerTick);
                          if ( l == 1) {nxtDisplayTextLine(1,"l == 1");
                                          moveBackwards (ticksMoved);
                                rotate(90.0);
                                obstacleXEnd = CX - (ticksMoved * cmPerTick);
                          }
                          else if ( l == 0) {nxtDisplayTextLine(1,"l == 0");
                                CX -= opposite / 2;
                                x = 1;
                                rotate(90.0);
                                i = 0;
                          }
                   }
                   else if (a == 1) {nxtDisplayTextLine(1,"a == 1");
                             CY += ticksMoved * cmPerTick;
                    }
           }
      obstacleYEnd = CY; //set the end of the obstacle regarding the Y coordinate
  }

degreesTurned = 0;
turning = 0;
opposite = 0;
adjacent = 0;
hypotenuse = 0;

if (TX == CX || TY == CY) {
         decentralize();
         nxtDisplayTextLine(1,"all ==");
      }
i = 0;
}
/*function to check that the neccesary variables
** are initialized*/
void initialize() {
   if ( WheelDistance == 0.0 || wheelDiameter == 0.0) {
        nxtDisplayTextLine(1,"Error on Predef Variables");
        powerOff();
      }
   calWheelCircumference();
   calAxisCircumference();
   calCMPerTick();
}
/*main control function*/
task main()
{
   initialize();
   int i;/*to store the result of the function moveStraight()*/
    nMotorEncoder[motorA] = 0;
   nMotorEncoder[motorB] = 0;

   /*if the Target is in the opposite direction*/
   if ( TY <  0) {
     nxtDisplayTextLine(1,"Opposite direction");
      rotate(180); /*turn to opposite direction.*/
      TY = TY * -1; //convert negative to positive number;
   }
   /*if the target is not straight ahead*/
  if ( TX != 0) {
           nxtDisplayTextLine(1,"TX != 0");
           length = computeST(TY, TX);

           if (TX > 0 && TY != 0) {
                   nxtDisplayTextLine(1,"TX > 0 && TY != 0");

                    /*calculate number of degrees to turn in order to be heading straight to the Target.*/
                   degreesToTurn = ((float)TX / (float)length);
                  degreesToTurn = asin(degreesToTurn)* 180.0 / PI;
                    rotate(degreesToTurn);
           }

           else if(TX < 0 && TY != 0) {
                   nxtDisplayTextLine(1,"TX < 0 && TY != 0");
                   degreesToTurn = ((float)TX / (float)length);
                  degreesToTurn = asin(degreesToTurn)* 180.0 / PI;
                   rotate(degreesToTurn);
           }
           else if (TX > 0 && TY == 0.0) {
                   nxtDisplayTextLine(1,"TX > 0 && TY == 0.0");
                   rotate(90);
                   length = (int)TX;
           }
          else if (TX < 0 && TY == 0.0) {
                  nxtDisplayTextLine(1,"TargetX < 0 && TY == 0.0");
                   rotate(-90);
                   length = (int)TX;
           }
           distanceReturned = calculateRotating(length);
  }
  /*if the target is straight ahead*/
  if (TX == 0) {
         nxtDisplayTextLine(1,"TX == 0");
          length = (int)TY;
          distanceReturned = calculateRotating((float)length);
  }
  eraseDisplay(); /*clear the screen*/
   i = moveStraight(distanceReturned);
   if (i == 0) {
          nxtDisplayTextLine(1,"i == 0");
         haltMotors();
         nxtDisplayTextLine(1,"Target Reached");
   }
   if (i == 1) {
          nxtDisplayTextLine(1,"i == 1");
          wait1Msec(1000);
          rotate(degreesToTurn * -1); //face the obstacle

          CX += ((ticksMoved * cmPerTick) * sinDegrees( degreesToTurn));

         CY += ((ticksMoved * cmPerTick) * cosDegrees( degreesToTurn));

      wait1Msec(5000);//wait1Msec(5000);
         int p =  1;
         /*while the target is not reached snd reachability test return 0*/
         while(p == 1) {
                 followBoundary();
                p = newSTLine();
         }
         powerOff();
         haltMotors();
   }
}


Sat Apr 26, 2008 1:51 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
hi,
nice program 8) -
although some things seems to me quite familiar :wink:
your program seems to work as kind of a single task formulation whereas my navigator
viewtopic.php?t=455
is based more on a multi tasking architecture.
To be more flexilble at different challenges I defined the most needed routines in a header file:
Code:
void StopMotors ();               // muss nach jedem Einzelmanoever aufgerufen werden!
                                  // stoppt + speichert augenblickliche Positionsdaten

void move_cm(float s);            // geradeaus Anzahl cm vor o. zurueck
void forward();                   // vorwaerts bis StopMotors() aufgerufen wird
void reverse();                   // rueckwaerts bis StopMotors() aufgerufen wird

void turn_degr(float Winkel);     // drehen um einen best. Winkel (+) links, (-) rechts herum
void turn_left();                 // links herum bis StopMotors() aufgerufen wird
void turn_right();                // rechts herum bis StopMotors() aufgerufen wird
void turn_toXY(float x, float y);    // dreht aus der aktuellen zur angegeb. Zielposition

void GotoXY(float x, float y);       // geht von der aktuellen zur angegeb. Zielposition
void GotoField(short xF, short yF);  // geht ins Zentrum des betr. Feldes
float GetDistToXY(float x, float y); // berechnet die Weglaenge zur angegeb. Zielposition
But of course, all of our odometric functions are working the same way.

Further on it would be very interesting to me how you implemented a compass heading correction of your courses (if you intended to do that). Meanwhile I tried a lot, but it's not trivial to do while the robot is moving: the position calculation error with simultaneously compass correction causes bigger miscalculations than without. So I do this currently only when the robot has stopped in between different movings.

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Sun Apr 27, 2008 3:21 am
Profile
Rookie

Joined: Sun Mar 02, 2008 9:41 am
Posts: 25
Post 
what do you mean by "compass heading correction"?

Do you mean how i am keeping track of the direction?


Sun Apr 27, 2008 2:52 pm
Profile
Rookie

Joined: Sun Mar 02, 2008 9:41 am
Posts: 25
Post 
hi again,

I have seen your program....

It seems a good implementation, unfortunately I have no time to look it more....

I use no multitasking as this is a limitations set for the work.


Sun Apr 27, 2008 2:55 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
giannisAPI wrote:
what do you mean by "compass heading correction"?
Do you mean how i am keeping track of the direction?

odometric navigation (counting the motor encounter ticks and thus calculting the movement) often is not valid enough (e.g. if wheels are slipping or when pushing with 1 wheel at an obstacle. so this miscalculation maybe can be reduced when you are reading the compass heading (if you got one).

(sorry for my English)

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Sun Apr 27, 2008 3:03 pm
Profile
Rookie

Joined: Sun Mar 02, 2008 9:41 am
Posts: 25
Post 
Hi,

I have made my own touch sensor which kind of surrounds the whole robot so there is no way that it will be pushing an obstacle with one motor.

Regarding the compass thing, I assume that the environment is not slippery...

The main idea of the project is to check if such algorithms, under so basic hardware could always come up with the right answer (reach the target or conclude on a finite amount of time that the target is not reachable).

A sort answer is that these algorithms they will always come up with an answer as long as the environment satisfies the limitations set (such as not slippery).

A question raised is who decides about these limitations and if these limitations are likely to be true in a real environment....


Sun Apr 27, 2008 8:09 pm
Profile
Guru
User avatar

Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
Post 
hi,
now I found a good understandable reference to the Bug2, and thus I finally understood your appoach :wink:
http://www.cs.jhu.edu/~hager/Teaching/c ... ug-Alg.pdf

good work ! 8)

_________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)
float x,y,r,i,s,j,t,n;task main(){F(y,64){F(x,99){r=i=t=0;s=x/33-2;j=y/32-1;F(n,50&S){t=r*r-i*i;i=2*r*i+j;r=t+s;}if(S){PutPixel(x,y);}}}while(1)}


Tue Apr 29, 2008 7:05 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 7 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


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.