View unanswered posts | View active topics It is currently Fri Oct 30, 2020 6:46 pm  Page 1 of 1 [ 7 posts ]
 Print view Previous topic | Next topic
BUG2 ALgorithm Implementation
Author Message
Rookie

Joined: Sun Mar 02, 2008 9:41 am
Posts: 25 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. synchronconst float WheelDistance= 11.6 ; //the distance between the two wheelsconst float wheelDiameter = 5.6;//diameter of the wheelfloat TY = 120.0; //The Target Y coordinatefloat TX = 70.0; // The Target X xoordinatefloat CY = 0.0; // To hold the current Y coordinatefloat CX = 0.0;// To hold the current X coordinatefloat wheelCircumference; // To hold the wheel circumferencefloat axisCircumference; // To hold the axis circumferencefloat cmPerTick; // How many cm the robot travels per rotation tickfloat distanceReturned;float degreesToTurn;float ticksMoved; //to show at how many ticks the obstacle was encounteredint 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 Guru Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030 hi,
nice program -
although some things seems to me quite familiar 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)

Sun Apr 27, 2008 3:21 am Rookie

Joined: Sun Mar 02, 2008 9:41 am
Posts: 25 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 Rookie

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

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 Guru Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030     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)

Sun Apr 27, 2008 3:03 pm Rookie

Joined: Sun Mar 02, 2008 9:41 am
Posts: 25 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 Guru Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030 hi,
now I found a good understandable reference to the Bug2, and thus I finally understood your appoach http://www.cs.jhu.edu/~hager/Teaching/c ... ug-Alg.pdf

good work ! _________________
regards,
HaWe aka Ford
#define S sqrt(t+2*i*i)<2
#define F(a,b) for(a=0;a<b;++a)

Tue Apr 29, 2008 7:05 am
Display posts from previous:  Sort by Page 1 of 1 [ 7 posts ]

#### Who is online

Users browsing this forum: No registered users and 2 guests

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

Search for:
 Jump to:  Select a forum ------------------ ROBOTC Applications    ROBOTC for LEGO MINDSTORMS       Third-party sensors    ROBOTC for CORTEX & PIC    ROBOTC for VEX IQ    ROBOTC for Arduino    Robot Virtual Worlds    Multi-Robot Communications    Issues and Bugs Competitions & Partners    Mini Urban Challenge    CS2N Robot Virtual Worlds Competitions       VEX Skyrise Competition 2014-2015       VEX Toss Up 2013-2014       FTC Block Party! 2013-2014    Competitions using VEX - BEST, TSA, VEX, and RoboFest!    FTC Programming    RoboCup Junior and Other ROBOT Competitions Virtual Brick Robotics Discussions    General Discussions    Project Discussions Off-Topic ROBOTC Forum & ROBOTC.net Suggestions/Feedback    ROBOTC Forums Suggestions/Comments    ROBOTC.net Suggestions/Comments       NXT Programming: Tips for Beginning with ROBOTC       VEX Programming: Tips for Beginning with ROBOTC    2013 Robotics Summer Of Learning       VEX Toss Up Programming Challenge       FTC Ring It Up! Programming Challenge    International Forums       Spanish Forums          ROBOTC for MINDSTORMS          ROBOTC for VEX       French Forums          ROBOTC pour Mindstorms          ROBOTC pour IFI VEX       Japanese Forums （日本語のフォーラム）       German Forums    2015 Spring Carnival Event    PLTW (Project Lead The Way)    Robotics Merit Badge    2014 Robotics Academy Summer of Learning 