8. Turnable beacons
The static approach to this problem shows a rather limited area in the room-center, where signal from all the beacons are received. About the same area will be reached by two, a larger surface by only one beacon. Together with dead-reckoning, a pritty good positioning will be available for robot-soccer for example in a limited field. Multi-robots is possible. Remember that positioning by two-beacon-data is still possible, since the unallowed circles-intersection point can be pruned by the help of dead-reckoning data. One-beacon-data is useless.
|
|
|
vertical situation (this can be neglected, since we use a ultrasonic reflector, which makes horizontal reception possible) |
|
horizontal situation (bi is computable from the value tan =(yr-yi)/(xr-xi) ) |
With the dynamic solution, the single robot will receive the beacon-signal anywhere in the room. Therefore the robot must send the information of its position back to the beacons. They will compute their best vertical and horizontal transmission angles. Thus, the beacons must be equiped with angle-sensors and a movable system.
As the robot communicates its position back to the beacons after about 400ms from the start of the process, it must send either the slightly transformed position involving its velocity and direction or simply send the corrected dead-reckoning data.
Here the beacon-design. There is only a horizontal motorized turning facility.
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
/* sender-beacon version 4.0 ; beacon should fire if it receives an IR-message from the robot, get P-message and adjust beacon orientation*/ #include <conio.h> #include <unistd.h> #include <string.h> #include <lnp.h> #include <dmotor.h> #include <dsensor.h> #define CORDIC_gain 1.646760258 pid_t adjust_thread; int xr=50; /*robot start position*/ int yr=50; int xi=0; /*coordinates of A <==============================================*/ int yi=0; double Phase (int k) /*in degrees*/
{
switch(k)
{
case 1: return 45.0000000;
case 2: return 26.5650512;
case 3: return 14.0362435;
case 4: return 7.12501635;
case 5: return 3.57633437;
case 6: return 1.78991061;
case 7: return 0.89517371;
case 8: return 0.44761417;
case 9: return 0.22381050;
case 10: return 0.11190568;
case 11: return 0.05595289;
case 12: return 0.02797645;
case 13: return 0.01398823;
case 14: return 0.00699411;
case 15: return 0.00349706;
case 16: return 0.00174853;
case 17: return 0.00087426;
case 18: return 0.00043713;
case 19: return 0.00021857;
case 20: return 0.00010928;
case 21: return 0.00005464;
case 22: return 0.00002732;
case 23: return 0.00001366;
case 24: return 0.00000683;
case 25: return 0.00000341;
case 26: return 0.00000177;
case 27: return 0.00000085;
case 28: return 0.00000042;
case 29: return 0.00000021;
case 30: return 0.00000010;
case 31: return 0.00000005;
case 32: return 0.00000002;
case 33: return 0.00000001;
default: return 888;
}
}
double atan(double x)
{ double a,b,Temp;
double kL=1;
double Theta=90;
int L,sign;
if(x>0) {b=1;} else {b=-1;}
Theta*=-b; /*starting value +90 or -90 depending sign of x*/
a=x*b; /*a will always be positive */
Temp=a; /*now rotate by +/- 90°; this could be done more quickly, but for better understanding*/ a=b*b; b=-Temp*b; for(L=1;L<34;L++)
{
if(b>0) { sign=-1; } else { sign=1; }
Theta+=sign*Phase(L);
Temp=a;
a-=b*kL*sign;
b+=Temp*kL*sign;
kL/=2;
}
if(x>0) { return Theta+90;} else {return Theta-90;}
} double abs(double x)
{
if (x>0) return x; else return -x;
}
#define ROT_PER_DEGREE -0.666666 /* 60 rotation pulses <=> 90 ° */ int adjust()
{
int temp,cas;
double dx,dy,deltax,deltay,ta,beta,rot;
ds_active (&SENSOR_3);
ds_rotation_on(&SENSOR_3);
ds_rotation_set(&SENSOR_3,0);
while(1)
{
cputs("xr");sleep(1);lcd_int(xr);sleep(1);
cputs("yr");sleep(1);lcd_int(yr);sleep(1);
cas=0;
dx=xr-xi;
dy=yr-yi;
if(abs(dx)>abs(dy))
{
cas=1;
deltay=dx; /*exchange to stay between 0 and 1*/
deltax=dy;
}
else
{
cas=0;
deltay=dy;
deltax=dx;
}
ta=abs(deltax/deltay);
if(dx>0)
{
if(dy>0) { if(cas==0) {beta=atan(ta);} else {beta=90-atan(ta);} } /*quadrant 1*/
else { if(cas==0) {beta=180-atan(ta);} else {beta=90+atan(ta);} } /*quadrant 2*/
}
else
{
if(dy>0) { if(cas==0) {beta=360-atan(ta);} else {beta=270+atan(ta);} } /*quadrant4 */
else { if(cas==0) {beta=180+atan(ta);} else {beta=270-atan(ta);} } /*quadrant 3*/
}
cputs("beta");sleep(1);lcd_int(beta);sleep(1);
rot=beta*ROT_PER_DEGREE;
if(rot<-120) rot+=240; /*perfect turn decision no more than 180° */
cputs("rot");sleep(1);lcd_int(rot);sleep(1);
while(ROTATION_3>rot+2)
{
motor_c_dir(fwd);
motor_c_speed(MAX_SPEED);
lcd_int(ROTATION_3);
}
motor_c_speed(0);
while(ROTATION_3<rot-2)
{
motor_c_dir(rev);
motor_c_speed(MAX_SPEED);
lcd_int(ROTATION_3);
}
motor_c_speed(0);
msleep(500); /*wait a bit*/
}
return 0;
}
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
int go=0;
int xrr,yrr;
void my_integrity_handler(const unsigned char *data,unsigned char len)
{
if (data[0]=='A') go=1; /* use A B or C accordingly <================================*/
if (data[0]=='P') /*P_message*/
{
xrr=(data[1]-128)*6; /*rebuild the coordinates */
yrr=(data[2]-128)*6;
go=2;
}
}
int main()
{
lnp_integrity_set_handler(my_integrity_handler);
adjust_thread=execi(&adjust,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE);
while(1)
{
while(go==0) msleep(10);
switch(go)
{
case 1: /*PING*/
{
msleep(50);
motor_a_speed(MAX_SPEED);
motor_a_dir(fwd);
msleep(35);
motor_a_speed(0);
motor_a_dir(off);
go=0;
break;
}
case 2:
{
xr=xrr; /*pass the coordinates*/
yr=yrr;
go=0;
break;
}
}
} return 0; } |
Note that the respective beacon-coordinates must be entered in the correct lines (red marked) AND the message "A", "B", or "C". The programs A_4.c, B_4.c and C_4.c should be used together with the rec_bot7.c from the positioning section.
HERE THE FIRST REAL-TIME PROGRAMS: A_5.c , B_5.c and C_5.c ! Use with rec_bot8.c !
/* sender-beacon version 5.0 ; beacon should fire if it receives an IR-message from the robot, get P-message and adjust beacon orientation NEW NEW NEW NEW no display*/ #include <unistd.h> #include <string.h> #include <lnp.h> #include <dmotor.h> #include <dsensor.h> #define CORDIC_gain 1.646760258 pid_t adjust_thread; int xr=50; /*robot start position*/ int yr=50; int xi=0; /*coordinates of A <==============================================*/ int yi=0; double Phase (int k) /*in degrees*/
{
switch(k)
{
case 1: return 45.0000000;
case 2: return 26.5650512;
case 3: return 14.0362435;
case 4: return 7.12501635;
case 5: return 3.57633437;
case 6: return 1.78991061;
case 7: return 0.89517371;
case 8: return 0.44761417;
case 9: return 0.22381050;
case 10: return 0.11190568;
case 11: return 0.05595289;
case 12: return 0.02797645;
case 13: return 0.01398823;
case 14: return 0.00699411;
case 15: return 0.00349706;
case 16: return 0.00174853;
case 17: return 0.00087426;
case 18: return 0.00043713;
case 19: return 0.00021857;
case 20: return 0.00010928;
case 21: return 0.00005464;
case 22: return 0.00002732;
case 23: return 0.00001366;
case 24: return 0.00000683;
case 25: return 0.00000341;
case 26: return 0.00000177;
case 27: return 0.00000085;
case 28: return 0.00000042;
case 29: return 0.00000021;
case 30: return 0.00000010;
case 31: return 0.00000005;
case 32: return 0.00000002;
case 33: return 0.00000001;
default: return 888;
}
}
double atan(double x)
{ double a,b,Temp;
double kL=1;
double Theta=90;
int L,sign;
if(x>0) {b=1;} else {b=-1;}
Theta*=-b; /*starting value +90 or -90 depending sign of x*/
a=x*b; /*a will always be positive */
Temp=a; /*now rotate by +/- 90°; this could be done more quickly, but for better understanding*/ a=b*b; b=-Temp*b; for(L=1;L<34;L++)
{
if(b>0) { sign=-1; } else { sign=1; }
Theta+=sign*Phase(L);
Temp=a;
a-=b*kL*sign;
b+=Temp*kL*sign;
kL/=2;
}
if(x>0) { return Theta+90;} else {return Theta-90;}
} double abs(double x)
{
if (x>0) return x; else return -x;
}
#define ROT_PER_DEGREE -0.666666 /* 60 rotation pulses <=> 90 ° */ int adjust()
{
int temp,cas;
double dx,dy,deltax,deltay,ta,beta,rot;
ds_active (&SENSOR_3);
ds_rotation_on(&SENSOR_3);
ds_rotation_set(&SENSOR_3,0);
while(1)
{
cas=0;
dx=xr-xi;
dy=yr-yi;
if(abs(dx)>abs(dy))
{
cas=1;
deltay=dx; /*exchange to stay between 0 and 1*/
deltax=dy;
}
else
{
cas=0;
deltay=dy;
deltax=dx;
}
ta=abs(deltax/deltay);
if(dx>0)
{
if(dy>0) { if(cas==0) {beta=atan(ta);} else {beta=90-atan(ta);} } /*quadrant 1*/
else { if(cas==0) {beta=180-atan(ta);} else {beta=90+atan(ta);} } /*quadrant 2*/
}
else
{
if(dy>0) { if(cas==0) {beta=360-atan(ta);} else {beta=270+atan(ta);} } /*quadrant4 */
else { if(cas==0) {beta=180+atan(ta);} else {beta=270-atan(ta);} } /*quadrant 3*/
}
rot=beta*ROT_PER_DEGREE;
if(rot<-120) rot+=240; /*perfect turn decision no more than 180° */
while(ROTATION_3>rot+2)
{
motor_c_dir(fwd);
motor_c_speed(MAX_SPEED);
}
motor_c_speed(0);
while(ROTATION_3<rot-2)
{
motor_c_dir(rev);
motor_c_speed(MAX_SPEED);
}
motor_c_speed(0);
msleep(10); /*wait a bit*/
}
return 0;
}
/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
int go=0;
int xrr,yrr;
void my_integrity_handler(const unsigned char *data,unsigned char len)
{
if (data[0]=='A') go=1; /* use A B or C accordingly <================================*/
if (data[0]=='P') /*P_message*/
{
xrr=(data[1]-128)*6; /*rebuild the coordinates */
yrr=(data[2]-128)*6;
go=2;
}
}
int main()
{
lnp_integrity_set_handler(my_integrity_handler);
adjust_thread=execi(&adjust,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE);
while(1)
{
while(go==0) msleep(10);
switch(go)
{
case 1: /*PING*/
{
msleep(50);
motor_a_speed(MAX_SPEED);
motor_a_dir(fwd);
msleep(35);
motor_a_speed(0);
motor_a_dir(off);
go=0;
break;
}
case 2:
{
xr=xrr; /*pass the coordinates*/
yr=yrr;
go=0;
break;
}
}
} return 0; } |
