 # UWB Based unmanned indoor Human Following Robot using (PoZYX)

I am working on Follow me robot using Pozyx , basically the robot is a trolley with 2 drive and 2 free wheels like (2wd),

i was using 2 anchors mounted in front of robot and 1 tag which i keep with me, by calculating the distance from each anchor the direction is decided where to move like shown in code, but this is a very basic approach and results are not same as wanted,

``````if ((range.distance > 1500) || (range1.distance > 1500)) // distance is in mm
{
Stop();

}
else {

int difference = ((range.distance) - (range1.distance));

if ((difference < 250) && (difference > -250))
{
Forword();
}
if (difference > 250)
{
Left();
}
if (difference < -250)
{
Right();
}
``````

after this testing i read some research papers people are using trigonometric formulas to calculate the x,y quarantines like Pythagorean theorem, Then i implemented the formula, but i am only able to get the x,y coordinates only in front of anchors A1, and A2 not left and right (Picture attached),

i need help regarding how to implement these x,y coordinates on motor and calculating the angle that will use for turning the robot left and right,

``````void localization(float r1, float r0, float d, float x0, float y0, float x1,float y1){

float a= ((r0*r0)-(r1*r1)+(d*d))/(2*d);
float b=((r1*r1)-(r0*r0)+(d*d))/(2*d);
float h= sqrt((r0*r0)-(a*a));

float x2 = x0 + a * (x1 - x0) / d;
float y2 = y0 - a * (y1 - y0) / d;

float x3= x2+((h*(y1-y0))/d);
float y3= y2+((h*(x1-x0))/d);

Serial.print(x3);
Serial.print(",");
Serial.print(y3);
Serial.println(".");

}
``````