From 56d9bdd39ed36c36e9a61411b86c76d5228b2133 Mon Sep 17 00:00:00 2001 From: sicarius Date: Sun, 11 Feb 2007 18:32:03 +0000 Subject: Added lot's of code-files used during work --- source/ct-Bot/map.c | 659 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 659 insertions(+) create mode 100644 source/ct-Bot/map.c (limited to 'source/ct-Bot/map.c') diff --git a/source/ct-Bot/map.c b/source/ct-Bot/map.c new file mode 100644 index 0000000..753941d --- /dev/null +++ b/source/ct-Bot/map.c @@ -0,0 +1,659 @@ +/* + * c't-Bot - Robotersimulator fuer den c't-Bot + * + * This program is free software; you can redistribute it + * and/or modify it under the terms of the GNU General + * Public License as published by the Free Software + * Foundation; either version 2 of the License, or (at your + * option) any later version. + * This program is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * You should have received a copy of the GNU General Public + * License along with this program; if not, write to the Free + * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, + * MA 02111-1307, USA. + * + */ + +/*! @file map.c + * @brief Karte + * @author Benjamin Benz (bbe@heise.de) + * @date 19.09.06 +*/ +#include +#include "ct-Bot.h" +#include "bot-local.h" +#include "sensor_correction.h" +#include "map.h" +#include "mmc.h" +#include "mini-fat.h" +#include "mmc-vm.h" + +#ifdef MAP_AVAILABLE +#include +#include + +/* + * Eine Karte ist wie folgt organisiert: + * Es gibt Sektionen zu je MAP_SECTION_POINTS * MAP_SECTION_POINTS. + * Diese Sektionen enthalten direkt die Pixel-Daten + * Auf dem PC haelt (noch) ein uebergeordnetes Array haelt die einzelnen Sections + * So laesst sich leicht eine Karte aufbauen, + * ohne dass viel Platz fuer unbenutzte Felder draufgeht + * + * Auf dem MCU passt eine Sektion in einen Flashblock, bzw immer 2 Sektionen in einen Block + * Es stehen immer 2 Sections also 1 Flash-Block im SRAM und werden bei Bedarf gewechselt + * + * Felder sind vom Typ int8 und haben einen Wertebereich von -128 bis 127 + * 0 bedeutet: wir wissen nichts über das Feld + * negative Werte bedeuten: Hinderniss + * postivie Werte bedeuten: freier Weg + * je größer der Betrag ist, desto sicherer die Aussage über das Feld + * Der Wert -128 ist Löchern vorbehalten und wird dann auch nicht durch die Abstandssensoren verändert + * (Achtung, die Locherkennung ist derzeit noch nicht fertig impelementiert) + * + * Felder werden wie folgt aktualisiert: + * Wird ein Punkt als frei betrachtet, erhoehen wir den Wert des Feldes um MAP_STEP_FREE + * Wird ein Punkt als belegt erkannt, ziehen wir um ihn einen Streukreis mit dem Radius MAP_RADIUS. + * Das Zentrum des Kreises wird um MAP_STEP_OCCUPIED dekrementiert, nach aussen hin immer weniger + * Wird ein Feld als Loch erkannt, setzen wir den Wert fest auf -128 (Achtung, derzeit noch nicht fertig impelementiert) + */ + +#define MAP_SECTIONS ((( MAP_SIZE*MAP_RESOLUTION)/MAP_SECTION_POINTS)) + +#define MAP_STEP_FREE 2 /*!< Um diesen Wert wird ein Feld inkrementiert, wenn es als frei erkannt wird */ +#define MAP_STEP_OCCUPIED 10 /*!< Um diesen Wert wird ein Feld dekrementiert, wenn es als belegt erkannt wird */ + +#define MAP_RADIUS 10 /*!< Umkreis einen Messpunkt, der als Besetzt aktualisiert wird (Streukreis) [mm]*/ +#define MAP_RADIUS_FIELDS (MAP_RESOLUTION*MAP_RADIUS/1000) /*!< Umkreis einen Messpunkt, der als Besetzt aktualisiert wird (Streukreis) [Felder]*/ + +#define MAP_PRINT_SCALE /*!< Soll das PGM eine Skala erhalten */ +#define MAP_SCALE (MAP_RESOLUTION/2) /*!< Alle wieviel Punkte kommt wein Skalen-Strich */ + +#ifdef SHRINK_MAP_ONLINE + uint16 map_min_x=MAP_SIZE*MAP_RESOLUTION/2; /*!< belegter Bereich der Karte [Kartenindex]: kleinste X-Koordinate */ + uint16 map_max_x=MAP_SIZE*MAP_RESOLUTION/2; /*!< belegter Bereich der Karte [Kartenindex]: groesste X-Koordinate */ + uint16 map_min_y=MAP_SIZE*MAP_RESOLUTION/2; /*!< belegter Bereich der Karte [Kartenindex]: kleinste Y-Koordinate */ + uint16 map_max_y=MAP_SIZE*MAP_RESOLUTION/2; /*!< belegter Bereich der Karte [Kartenindex]: groesste Y-Koordinate */ +#endif + +typedef struct { + int8 section[MAP_SECTION_POINTS][MAP_SECTION_POINTS]; /*!< Einzelne Punkte */ +} map_section_t; /*!< Datentyp fuer die elementarfelder einer Gruppe */ + + +#ifdef MMC_VM_AVAILABLE + map_section_t * map[2][1]; /*! Array mit den Zeigern auf die Elemente */ + uint32 map_start_block; /*! Block, bei dem die Karte auf der MMC-Karte beginnt. Derzeit nur bis 32MByte adressierbar*/ + uint32 map_current_block; /*! Block, der aktuell im Puffer steht. Derzeit nur bis 32MByte adressierbar*/ + uint8* map_buffer; /*! dynamischer Puffer */ +#else + #ifdef MCU + #ifdef MMC_AVAILABLE + // Wenn wir die MMC-Karte haben, passen immer 2 Sektionen in den SRAM + map_section_t * map[2][1]; /*! Array mit den Zeigern auf die Elemente */ + uint32 map_start_block; /*! Block, bei dem die Karte auf der MMC-Karte beginnt. Derzeit nur bis 32MByte adressierbar*/ + uint32 map_current_block; /*! Block, der aktuell im Puffer steht. Derzeit nur bis 32MByte adressierbar*/ + uint8 map_buffer[sizeof(map_section_t)*2]; /*! statischer Puffer */ + uint8 map_current_block_updated; /*! markiert, ob der aktuelle Block gegenueber der MMC-Karte veraendert wurde */ + #else + // Ohne MMC-Karte nehmen wir nur 1 Sektionen in den SRAM und das wars dann + map_section_t * map[1][1]; /*! Array mit den Zeigern auf die Elemente */ + #endif + #else + map_section_t * map[MAP_SECTIONS][MAP_SECTIONS]; /*! Array mit den Zeigern auf die Elemente */ + #endif +#endif // MMC_VM_AVAILABLE + +/*! + * initialisiere die Karte + * @return 0 wenn alles ok ist + */ +int8 map_init(void){ + #ifndef MMC_VM_AVAILABLE + #ifdef MMC_AVAILABLE + map_start_block=0xFFFFFFFF; + map_current_block_updated = 0xFF; // Die MMC-Karte ist erstmal nicht verfuegbar + + if (mmc_get_init_state() != 0) return 1; + map_start_block= mini_fat_find_block("MAP",map_buffer); + + // Die Karte auf den Puffer biegen + map[0][0]=(map_section_t*)map_buffer; + map[1][0]=(map_section_t*)(map_buffer+sizeof(map_section_t)); + + if (map_start_block != 0xFFFFFFFF){ + map_current_block_updated = False; // kein Block geladen und daher auch nicht veraendert + return 1; + } + #endif + #else + map_start_block = mmc_fopen("MAP")>>9; // TODO: Mit Bytes arbeiten und Shiften sparen + printf("Startaddress of map: 0x%lx \n\r", map_start_block<<9); + if (map_start_block == 0) return 1; + map_buffer = mmc_get_data(map_start_block<<9); + if (map_buffer == NULL) return 1; + + // Die Karte auf den Puffer biegen + map[0][0]=(map_section_t*)map_buffer; + map[1][0]=(map_section_t*)(map_buffer+sizeof(map_section_t)); + #endif // MMC_VM_AVAILABLE + + return 0; +} + +/*! + * liefert einen Zeiger auf die Section zurueck, in der der Punkt liegt. + * Auf dem MCU kuemmert sie sich darum, die entsprechende Karte aus der MMC-Karte zu laden + * @param x x-Ordinate der Karte (nicht der Welt!!!) + * @param y y-Ordinate der Karte (nicht der Welt!!!) + * @param create Soll das Segment erzeugt werden, falls es noch nicht existiert? + * @return einen zeiger auf die Karte + */ +map_section_t * map_get_section(uint16 x, uint16 y, uint8 create){ + uint16 section_x, section_y; + + // Berechne in welcher Sektion sich der Punkt befindet + section_x=x/ MAP_SECTION_POINTS; + section_y=y/ MAP_SECTION_POINTS; + + if ((section_x>= MAP_SECTIONS) || (section_y >= MAP_SECTIONS) || + (section_x < 0) || (section_y <0)){ + #ifdef PC + printf("Versuch auf in Feld ausserhalb der Karte zu zugreifen!! x=%d y=%d\n",x,y); + #endif + return NULL; + } + +// cut + + #ifndef MMC_VM_AVAILABLE + #ifdef MMC_AVAILABLE // Mit MMC-Karte geht hier einiges anders + // wenn die Karte nicht sauber initialisiert ist, mache nix! + if (map_current_block_updated == 0xFF) + return map[0][0]; + + // Berechne den gesuchten Block + uint32 block = section_x + section_y*MAP_SECTIONS; + block = block >>1; // es passen immer 2 Sections in einen Block + block += map_start_block; // Offset drauf + + // Ist der Block noch nicht geladen + if (map_current_block != block){ + // Wurde der Block im RAM veraendert? + if (map_current_block_updated == True) + mmc_write_sector(map_current_block,map_buffer,0); + + //Lade den neuen Block + mmc_read_sector(block,map_buffer); + map_current_block=block; + map_current_block_updated = False; + } + + // richtige der beiden sections raussuchen + uint8 index= section_x & 0x01; + return map[index][0]; + #else // ohne MMC-Karte einfach direkt mit dem SRAM arbeiten + if ((map[section_x][section_y] == NULL) && (create==True)){ + uint16 index_x, index_y; + map[section_x][section_y]= malloc(sizeof(map_section_t)); + for (index_x=0; index_xsection[index_x][index_y]=0; + + } + return map[section_x][section_y]; + #endif // MMC_AVAILABLE + #else + // Berechne den gesuchten Block + uint32 block = section_x + section_y*MAP_SECTIONS; + block = block >>1; // es passen immer 2 Sections in einen Block + block += map_start_block; // Offset drauf + map_buffer = mmc_get_data(block<<9); + if (map_buffer != NULL){ + map[0][0]=(map_section_t*)map_buffer; + map[1][0]=(map_section_t*)(map_buffer+sizeof(map_section_t)); + // richtige der beiden sections raussuchen + uint8 index= section_x & 0x01; + return map[index][0]; + } else return map[0][0]; + #endif // MMC_VM_AVAILABLE +} + +/*! + * liefert den Wert eines Feldes + * @param x x-Ordinate der Karte (nicht der Welt!!!) + * @param y y-Ordinate der Karte (nicht der Welt!!!) + * @return Wert des Feldes (>0 heisst frei, <0 heisst belegt + */ +int8 map_get_field (uint16 x, uint16 y) { + uint16 index_x, index_y; + + // Suche die Section heraus + map_section_t * section = map_get_section(x,y,False); + + // Eventuell existiert die Section noch nicht + if (section == NULL) + return 0; + + // Berechne den Index innerhalb der Section + index_x = x % MAP_SECTION_POINTS; + index_y = y % MAP_SECTION_POINTS; + + return section->section[index_x][index_y]; +} + +/*! + * Setzt den Wert eines Feldes auf den angegebenen Wert + * @param x x-Ordinate der Karte (nicht der Welt!!!) + * @param y y-Ordinate der Karte (nicht der Welt!!!) + * @param value neuer wert des Feldes (> 0 heisst frei, <0 heisst belegt + */ +void map_set_field(uint16 x, uint16 y, int8 value) { + uint16 index_x, index_y; + + // Suche die Section heraus + map_section_t * section = map_get_section(x,y,True); + + // Wenn wir keine section zurueck kriegen, stimmt was nicht + if (section == NULL) + return; + + // Berechne den Index innerhalb der Section + index_x = x % MAP_SECTION_POINTS; + index_y = y % MAP_SECTION_POINTS; + + section->section[index_x][index_y]=value; + + #ifdef SHRINK_MAP_ONLINE + // Belegte Kartengroesse anpassen + if (x < map_min_x) + map_min_x=x; + if (x > map_max_x) + map_max_x= x; + if (y < map_min_y) + map_min_y=y; + if (y > map_max_y) + map_max_y= y; + #endif + + #ifdef MMC_AVAILABLE + #ifndef MMC_VM_AVAILABLE + if (map_current_block_updated != 0xFF) + map_current_block_updated = True; + #endif + #endif + +} + +/*! + * liefert den Wert eines Feldes + * @param x x-Ordinate der Welt + * @param y y-Ordinate der Welt + * @return Wert des Feldes (>0 heisst frei, <0 heisst belegt + */ +int8 map_get_point (float x, float y){ + //Ort in Kartenkoordinaten + uint16 X = world_to_map(x); + uint16 Y = world_to_map(y); + + return map_get_field(X,Y); +} + +/*! + * aendert den Wert eines Feldes um den angegebenen Betrag + * @param x x-Ordinate der Karte (nicht der Welt!!!) + * @param y y-Ordinate der Karte (nicht der Welt!!!) + * @param value Betrag um den das Feld veraendert wird (>= heisst freier, <0 heisst belegter + */ +void map_update_field (uint16 x, uint16 y, int8 value) { + + int16 tmp= map_get_field(x,y); + if (tmp == -128) // Nicht aktualiseren, wenn es sich um ein Loch handelt + return; + + tmp+=value; + + // pruefen, ob kein ueberlauf stattgefunden hat und das Feld nicht schon als Loch (-128) markiert ist + if ((tmp < 128) && (tmp > -128)) + map_set_field(x,y,(int8)tmp); +} + +/*! + * markiert ein Feld als frei -- drueckt den Feldwert etwas mehr in Richtung "frei" + * @param x x-Ordinate der Karte (nicht der Welt!!!) + * @param y y-Ordinate der Karte (nicht der Welt!!!) + */ +void map_update_free (uint16 x, uint16 y) { + map_update_field(x,y,MAP_STEP_FREE); +} + +/*! + * aendert den Wert eines Feldes um den angegebenen Betrag + * @param x x-Ordinate der Karte (nicht der Welt!!!) + * @param y y-Ordinate der Karte (nicht der Welt!!!) + * @param value Betrag um den das Feld veraendert wird (>= heisst freier, <0 heisst belegter + */ +#if MAP_RADIUS_FIELDS > 0 + void map_update_field_circle(uint16 x, uint16 y, int8 radius, int8 value) { + int16 dX,dY; + uint16 h=radius*radius; + for(dX = -radius; dX <= radius; dX++){ + for(dY = -radius; dY <= radius; dY++) { + if(dX*dX + dY*dY <= h) + map_update_field (x + dX, y + dY,value); + } + } + } +#endif + +/*! + * markiert ein Feld als belegt -- drueckt den Feldwert etwas mehr in Richtung "belegt" + * @param x x-Ordinate der Karte (nicht der Welt!!!) + * @param y y-Ordinate der Karte (nicht der Welt!!!) + */ +void map_update_occupied (uint16 x, uint16 y) { + // Nur wenn ein Umkreis gewuenscht ist, auch einen malen + #if MAP_RADIUS_FIELDS > 0 + uint8 r; + for (r=1; r<=MAP_RADIUS_FIELDS; r++){ + map_update_field_circle(x,y,r,-MAP_STEP_OCCUPIED/MAP_RADIUS_FIELDS); + } + #else + map_update_field(x,y,-MAP_STEP_OCCUPIED); + #endif +} + +/*! + * Konvertiert eine Weltkoordinate in eine Kartenkoordinate + * @param koord Weltkordiante + * @return kartenkoordinate + */ +uint16 world_to_map(float koord){ + uint16 tmp = koord * MAP_RESOLUTION / 1000 + (MAP_SIZE*MAP_RESOLUTION/2); + + return tmp; +} + +/*! + * Konvertiert eine Kartenkoordinate in eine Weltkoordinate + * @param map_koord kartenkoordinate + * @return Weltkordiante + */ +float map_to_world(uint16 map_koord){ + float tmp = (map_koord - (MAP_SIZE*MAP_RESOLUTION/2)) / MAP_RESOLUTION * 1000; + + return tmp; +} + +/*! + * Aktualisiert die Karte mit den Daten eines Sensors + * @param x X-Achse der Position des Sensors + * @param y Y-Achse der Position des Sensors + * @param head Blickrichtung im Bogenmaß + * @param dist Sensorwert + */ +void update_map_sensor(float x, float y, float h, int16 dist){ + //Ort des Sensors in Kartenkoordinaten + uint16 X = world_to_map(x); + uint16 Y = world_to_map(y); + + uint16 d; + if (dist==SENS_IR_INFINITE) + d=SENS_IR_MAX_DIST; + else + d=dist; + + + + // Hinderniss, dass der Sensor sieht in Weltkoordinaten + float PH_x = x + (DISTSENSOR_POS_FW + d) * cos(h); + float PH_y = y + (DISTSENSOR_POS_FW + d) * sin(h); + // Hinderniss, dass der linke Sensor sieht in Kartenkoordinaten + uint16 PH_X = world_to_map(PH_x); + uint16 PH_Y = world_to_map(PH_y); + + if ((dist > 80 ) && (dist = dY) { // Hangle Dich an der laengeren Achse entlang + dY--; // stoppe ein Feld vor dem Hinderniss + uint16 lh = dX / 2; + for (i=0; i= dX) { + lh -= dX; + lY += sY; + } + } + } else { + dX--; // stoppe ein Feld vor dem Hinderniss + uint16 lh = dY / 2; + for (i=0; i= dY) { + lh -= dY; + lX += sX; + } + } + } +} + +/*! + * Aktualisiert den Standkreis der internen Karte + * @param x X-Achse der Position + * @param y Y-Achse der Position + */ +void update_map_location(float x, float y){ + int16 x_map = world_to_map(x); + int16 y_map = world_to_map(y); + + // Aktualisiere zuerst die vom Bot selbst belegte Flaeche + #define dim BOT_DIAMETER/2*MAP_RESOLUTION/100 + int16 dX,dY; + for(dX = -dim; dX <= dim; dX++) + for(dY = -dim; dY <= dim; dY++) { + if(dX*dX + dY*dY <= dim*dim) + map_update_free (x_map + dX, y_map + dY); + } +} + +/*! + * Aktualisiert die interne Karte anhand der Sensordaten + * @param x X-Achse der Position + * @param y Y-Achse der Position + * @param head Blickrichtung in Grad + * @param distL Sensorwert links + * @param distR Sensorwert rechts + */ +void update_map(float x, float y, float head, int16 distL, int16 distR){ +// printf("update_map: x=%f, y=%f, head=%f, distL=%d, distR=%d\n",x,y,head,distL,distR); + + float h= head * M_PI /180; + + //Ort des rechten Sensors in Weltkoordinaten + float Pr_x = x + (DISTSENSOR_POS_SW * sin(h)); + float Pr_y = y - (DISTSENSOR_POS_SW * cos(h)); + + //Ort des linken Sensors in Weltkoordinaten + float Pl_x = x - (DISTSENSOR_POS_SW * sin(h)); + float Pl_y = y + (DISTSENSOR_POS_SW * cos(h)); + + update_map_sensor(Pl_x,Pl_y,h,distL); + update_map_sensor(Pr_x,Pr_y,h,distR); +} + + + +#ifdef PC + /*! + * Schreibt einbe Karte in eine PGM-Datei + * @param filename Zieldatei + */ + void map_to_pgm(char * filename){ + printf("Speichere Karte nach %s\n",filename); + FILE *fp = fopen(filename, "w"); + + uint16 x,y; + + // lokale Variablen mit den defaults befuellen + uint16 min_x=map_min_x; + uint16 max_x=map_max_x; + uint16 min_y=map_min_y; + uint16 max_y=map_max_y; + + #ifdef SHRINK_MAP_OFFLINE // nun muessen wir die Grenezn ermitteln + // Kartengroesse reduzieren + int8 free=1; + while ((min_y < max_y) && (free ==1)){ + for (x=min_x; x min_y; y--){ + for (x=min_x; x