Durch einen Umgebungsscan die neue Zielrichtung finden
Hallo,
Ich habe endlich eine Funktion fertig gestellt, die das Umfeld des RP6 mit Hilfe eines SRF02 und dem Kompassmodul CMPS03 misst und in einem Array speichert. Um Zeiger zu vermeiden habe ich einfach ein Globales Array definiert.
Momentan wird die Distanz zur Umgebung in 36 Abschnitten gemessen.
Ich zögere es in 360 Schritten zu messen, da ich den ATmega 32 nicht überfordern will. Hat jemand Erfahrung damit 16bit Arrays mit 360 Gliedern zu verarbeiten? Schafft er das problemlos, oder ist das schon zu viel für ihn? Ein solches Array hat ja schon 720 bytes, wenn ich dann noch für die weitere verabreitung ähnliche Arrays brauche, dann kommt man schnell an die 2-3 kbytes.
Leider gibt es messfelher, wenn der RP6 im flachen Winkel die Distanz zu einigermaßen glatten Oberflächen messen will.
Also z.B. wenn der RP6 im 30° Winkel zur 20cm entfernten Wand steht misst er 400cm.
Was jetzt noch fehlt ist eine Funktion, die mir zuverlässig sagt in welcher Richtung am meißten platz ist, bzw. z.B. bei einem Flur mit knicken in welcher Richtung der Flur weiter geht.
Logischer weise sollten die Messfehler nicht in frage kommen. Also sollte man vlt. mehrere werte nebeneinander aufsummieren, sodass die Messfehler etwas geglättet werden. Ich bin auf der suche nach einer Relation der Signifikanz von Distanz und Breite.
d.H. Was ist besser vier Messwerte a 2m oder einer mit 4m? Der Einzelne könnte auch ein Messfehler sein, aber was, wenn es eine offene Tür ist, durch die der RP6 den Raum verlassen kann?
Momentan will ich die Summe von je drei nebeneinanderliegenden Messwerten bilden und die Richtung mit der größten Summe wird die neue Fahrtrichtung.
Ich Lösungen, die den Mittelwert der Messwerte mit inbetracht ziehen, wären vlt. auch nicht verkehrt.
Vielleicht hat ja jemand von euch noch kreative Ideen, wie man den Besten Weg finden kann...
hier der Code zum o.g. Text:
Code:
// Includes:
#include "RP6RobotBaseLib.h"
#include "RP6I2CmasterTWI.h"
#define CMPS03 0xC0
#define SRF02 0xE0
#define attachdistance 8
uint16_t area[36] = {0};
uint16_t distance_SRF02_blocking(void) //Distanzmessung
{
uint8_t srfbuffer[2]; //Speicher für die übertragenen Bytes
I2CTWI_transmit2Bytes(SRF02, 0, 81); // SRF02 bekommt im Register 0 den Befehl in cm zu messen
mSleep(65);
I2CTWI_transmitByte(SRF02, 2); // Ab dem zweiten Reigster Daten anfordern
I2CTWI_readBytes(SRF02, srfbuffer, 2); // und in dem Array speichern
return srfbuffer[0] * 256 + srfbuffer[1]-attachdistance;
}
uint16_t direction_CMPS03(void) //Richtungsbestimmung
{
uint8_t cmpsbuffer[2];
I2CTWI_transmitByte(CMPS03, 2); // Ab dem zweiten Reigster Daten anfordern
I2CTWI_readBytes(CMPS03, cmpsbuffer, 2); // und in dem Array speichern
return ((cmpsbuffer[0] * 256) + cmpsbuffer[1]);
}
void allroundscan(void)
{
uint16_t index = direction_CMPS03() / 100; // den Index and der aktuellen Richtung starten lassen
uint16_t counter = 0;
uint16_t direction = 0;
uint16_t distance = 0;
writeString_P("\n\tAreascan initiated\n*******************************************\n");
stop();
changeDirection(RIGHT);
moveAtSpeed(40,40);
while(counter < 36)
{
task_RP6System();
direction = direction_CMPS03() / 100;
// Drehrichtung bei übersprungenen Messungen korregieren (funktioniert nicht über den Nullsprung!!!) ausbaufähig
if (direction > index + 1 && index > 2)
changeDirection(LEFT);
if (direction < index -2 && index >= 2)
changeDirection(RIGHT);
// Messwert abspeichern
if (direction == index)
{
distance = distance_SRF02_blocking();
if (distance >= 1000)
area[index] = 0;
else
area[index] = distance;
writeString_P("Areascan Value [");
writeInteger(index*10,DEC);
writeString_P("]\t=");
writeInteger(area[index],DEC);
writeString_P("\n");
changeDirection(RIGHT);
index++;
counter++;
}
// Nullsprung
if (index >= 36)
index = 0;
}
stop();
writeString_P("\tAreascan completed\n*******************************************\n");
}
uint16_t direction_calculator(void)
{
uint16_t maximumsum = 0;
uint16_t newdirection = 0;
uint8_t i = 0;
for(i=0;i<36;i++)
{
if(i==0)
{
if(area[35]+area[i]+area[i+1] > maximumsum)
{
maximumsum = area[35]+area[i]+area[i+1];
newdirection = i;
}
}
else
{
if(area[i-1]+area[i]+area[i+1] > maximumsum)
{
maximumsum = area[i-1]+area[i]+area[i+1];
newdirection = i;
}
}
writeString_P("\nMaximum Summe ="); writeInteger(maximumsum,DEC); writeString_P("\t New Direction ="); writeInteger(newdirection*10,DEC);
}
return newdirection*100;
}
/*****************************************************************************/
// Main
int main(void)
{
initRobotBase();
powerON();
setLEDs(0b111111);
mSleep(1000);
setLEDs(0b001001);
I2CTWI_initMaster(100); // I2C speed set to 100kHz
// Main loop
while(true)
{
task_RP6System();
allroundscan();
}
return 0;
}
