Hi Freak10,
mit etwas Mathe geht das schon bis zu einem gewissen Grad an Genauigkeit UND Wahrscheinlichkeit.
Wie MeckPommER es schon gesagt hat: US-Sensoren erfassen nicht einen einzelnen Punkt, sonder dern Abstand zu einem Objekt, dass sich irgendwo auf einem Kreissegment von x Grad befindet.
Hier mal der Code, wie ich es auf meinem RoBoard unter Linux mit einem SRF10 gelöst habe:
[php:1:bb6f7b2aae]
--- geschrieben in C++ ---
void *ThreadSRF( void *ptr ) {
/* VAR-DEF */
int x, y;
float temp_distance, temp_angle, temp;
int PWM_period = 20000; // period in us
int angle_min = 40; // max right
int angle_max = 140; // max leftt
int angle=angle_min; // new position
int angle_new;
int diff = 5; // angle between measurements
int count = 10; // period count
bool direction=true;
char data[152][51] = {0};
/* INIT-Servo */
rcservo_SendPWMPulses( SRF_SERVO_CHANNEL, PWM_period, angle_min/0.1125+650, 100 );
usleep(200000);
/* MAIN */
while ( true )
{
SRF08_Start( SRF08ADDRESS, SRF08MODE );
usleep( 70000 );
if ( (direction==true && angle<angle_max) || angle<=angle_min ) {
angle_new = angle + diff;
direction = true;
}
else {
angle_new = angle - diff;
direction = false;
}
rcservo_SendPWMPulses( SRF_SERVO_CHANNEL, PWM_period, angle/0.1125+650, count );
temp = SRF08_ReadEcho( SRF08ADDRESS, 1, SRF08MODE );
// draw Map
for ( y=1; y<=50; y++) {
for ( x=1; x<=151; x++ ) {
temp_distance = sqrt( (-x*15+1132.5)*(-x*15+1132.5) + (1200-y*24)*(1200-y*24) );
temp_angle = atan2( (-x*15+1132.5), (1200-y*24) ) *180/M_PI + 90;
if ( (temp_distance > temp) && (temp_angle > angle-15) && (temp_angle < angle+15) )
data[x][y] = min( data[x][y]+1, 9 );
else if ( (temp_angle > angle-15) && (temp_angle < angle+15) )
data[x][y] = max( data[x][y]-1, 0 );
if (x>20 && x<130 && y>21 ) {
GotoXY(x-20,y-8);
switch (data[x][y]) {
case 1:
case 2:
printf("%s %s", COLOR[GREEN], COLOR[BLACK]);
break;
case 3:
case 4:
case 5:
printf("%s %s", COLOR[YELLOW], COLOR[BLACK]);
break;
case 6:
case 7:
case 8:
case 9:
printf("%s %s", COLOR[RED], COLOR[BLACK]);
break;
default:
printf("%s ", COLOR[BLACK]);
}
}
}
}
fflush( stdout );
angle = angle_new;
}
}
[/php:1:bb6f7b2aae]
Das ganze sieht dann später so auf dem Bildschirm aus (anklicken für große Version):
http://forums.trossenrobotics.com/gallery/files/3/1/3/9/ik3_thumb.jpg (http://forums.trossenrobotics.com/gallery/showimage.php?i=2165&c=3&userid=3139)
Gruß
Daniel