|
Page 1 of 1
|
[ 7 posts ] |
|
BUG2 ALgorithm Implementation
| Author |
Message |
|
giannisAPI
Rookie
Joined: Sun Mar 02, 2008 9:41 am
Posts: 25
|
 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 |
|
|
|
Ford Prefect
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:
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 |
|
|
|
giannisAPI
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 |
|
|
|
giannisAPI
Rookie
Joined: Sun Mar 02, 2008 9:41 am
Posts: 25
|
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 |
|
|
|
Ford Prefect
Guru
Joined: Sat Mar 01, 2008 12:52 pm
Posts: 1030
|
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 |
|
|
|
giannisAPI
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 |
|
|
|
Ford Prefect
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)
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 |
|
|
|
|
Page 1 of 1
|
[ 7 posts ] |
|
Who is online |
Users browsing this forum: No registered users and 1 guest |
|
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
|
|
