Absender: Frank Menzel
Datum: Di, 30.09.2008 20:33:28
Hallo, anbei mal mein Vorschlag für ein Pfadplanungsverhalten, welches hier ja schon grundsätzlich beschrieben wurde: http://www.heise.de/ct/projekte/machmit/ctbot/wiki/NeueVerhalten Das Handling ist folgendes: Man fährt den Bot rum und lässt ihn die Welt entdecken. Auf dem eigenen Screen kann man an beliebiger Stelle die Taste 5 drücken und er sollte dann den Weg zum Startpunkt finden und anfahren. Um einen anderen Zielpunkt auf der Karte zu definieren, kann das Ziel neu festgelegt werden durch die Taste 8 und somit gilt die aktuelle Botposition als anzufahrendes Ziel. Im Sim funktioniert es bei mir recht gut und findet die meisten Pfade. Ach so, hier ist die Stackerweiterung mit bei, die im Mailverkehr besprochen wurde, nur leider eben noch als Array-Version (was der Sache erst mal keinen Abbruch tut). Auch gilt hier erst einmal ?nur? die 4er-Nachbarschaft, d.h. er fährt nur gerade Wege und keine Schrägen. Als technischen Hintergrund habe ich in der der behaviour_pathplaning.c folgendes eingetragen: * @brief Wave-Pfadplanungsverhalten; eine niedrigaufloesende Map wird ueber die hochaufloesende gelegt und * auf dieser folgende Schritte ausgefuehrt: * -Innerhalb des wirklich benutzten Mappenbereiches wird jede Zelle durchlaufen und falls der Durchschnittswert * der Hochaufloesenden Map < 0 ist (Hinderniswert) hier eingetragen mit Wert 1 * -anzufahrende Zielposition erhaelt Mapwert 2 * -ausgehend von Zielposition laeuft eine Welle los bis zum Bot-Ausgangspunkt, d.h. von 2 beginnend erhaelt jede Nachbarzelle * den naechst hoeheren Zellenwert * -wird als ein Nachbar die Botposition erreicht, wird der Pfad zurueckverfolgt und immer die Zelle mit kleinerem Wert gewaehlt; * die Zellenkoordinaten werden als Weltkoordinaten auf den Stack gelegt und kann nun abgefahren werden Happy Pfadplanen :-) Gruß, Frank
Index: C:/botneu/ct-Bot/bot-logic/behaviour_pathplaning.c =================================================================== --- C:/botneu/ct-Bot/bot-logic/behaviour_pathplaning.c (revision 0) +++ C:/botneu/ct-Bot/bot-logic/behaviour_pathplaning.c (revision 0) @@ -0,0 +1,673 @@ +/* + * 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 behaviour_pathplaning.c + * @brief Wave-Pfadplanungsverhalten; eine niedrigaufloesende Map wird ueber die hochaufloesende gelegt und + * auf dieser folgende Schritte ausgefuehrt: + * -Innerhalb des wirklich benutzten Mappenbereiches wird jede Zelle durchlaufen und falls der Durchschnittswert + * der Hochaufloesenden Map < 0 ist (Hinderniswert) hier eingetragen mit Wert 1 + * -anzufahrende Zielposition erhaelt Mapwert 2 + * -ausgehend von Zielposition laeuft eine Welle los bis zum Bot-Ausgangspunkt, d.h. von 2 beginnend erhaelt jede Nachbarzelle + * den naechst hoeheren Zellenwert + * -wird als ein Nachbar die Botposition erreicht, wird der Pfad zurueckverfolgt und immer die Zelle mit kleinerem Wert gewaehlt; + * die Zellenkoordinaten werden als Weltkoordinaten auf den Stack gelegt und kann nun abgefahren werden + + * + * @author Frank Menzel (Menzelfr@xxxxxxx) + * @date 23.09.2008 +*/ + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_PATHPLANING_AVAILABLE +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> +#include "ct-Bot.h" +#include "bot-local.h" +#include "map.h" +#include "ui/available_screens.h" +#include "rc5-codes.h" +#include "display.h" +#include "log.h" +#include "pos_stack.h" + + +//#define DEBUG_PATHPLANING // Schalter fuer Debugausgaben +#ifndef LOG_AVAILABLE + #undef DEBUG_PATHPLANING +#endif +#ifndef DEBUG_PATHPLANING + #undef LOG_DEBUG + #define LOG_DEBUG(a, ...) {} +#endif + + +// einzeichenn des geplanten Weges in die Highres-Map als helle Punkte +//#define SHOW_PATH_IN_MAP + +/****** fuer geringaufloesende Map zur Pfadplanung --ACHTUNG ONLINE_SHRINK einschalten -- *****************/ + +#ifndef SHRINK_MAP_ONLINE + #warning "ONLINE Shrink einschalten !" +#endif + +#ifdef MCU +#ifndef __AVR_ATmega644__ + #warning "nur der Mega644 hat genug RAM !" +#endif +#endif + + +#ifdef PC + #define MAP_SIZE_LOWRES 12 /*! PC kann 12 Meter locker speichern */ +#else + #define MAP_SIZE_LOWRES 4 // echter Bot kann leider nicht sehr viel +#endif + +#define MAP_SECTION_POINTS_LOWRES 32 /*!< Kantenlaenge einer Section in Punkten ==> eine Section braucht MAP_SECTION_POINTS*MAP_SECTION_POINTS Bytes */ +#define MAP_RESOLUTION_LOWRES 8 /*!< Aufloesung der Karte in Punkte pro Meter */ + +#define MAP_SECTIONS_LOWRES (((uint16_t)(MAP_SIZE_LOWRES*MAP_RESOLUTION_LOWRES)/MAP_SECTION_POINTS_LOWRES)) /*!< Anzahl der Sections in der Lowres-Map */ + + +typedef struct { + int8 section[MAP_SECTION_POINTS_LOWRES][MAP_SECTION_POINTS_LOWRES]; /*!< Einzelne Punkte */ +} map_section_lowres_t; /*!< Datentyp fuer die Elementarfelder einer Gruppe */ + +map_section_lowres_t * map_lowres[MAP_SECTIONS_LOWRES][MAP_SECTIONS_LOWRES]; /*! Array mit den Zeigern auf die Elemente */ + + + + +/*! + * Konvertiert eine Lowres-Kartenkoordinate in eine Weltkoordinate + * @param map_koord Kartenkoordinate + * @return Weltkoordiante + */ +int16_t map_to_world_lowres(uint16_t map_koord) { +#if (1000 / MAP_RESOLUTION_LOWRES) * MAP_RESOLUTION_LOWRES != 1000 + #warning "MAP_RESOLUTION_LOWRES ist kein Teiler von 1000!" +#endif + int16_t tmp = ((map_koord - (MAP_SIZE_LOWRES*MAP_RESOLUTION_LOWRES/2.0)) * 1000) / MAP_RESOLUTION_LOWRES ;//in dieser Reihenfolge wegen Integerdivision + return tmp; +} + +/*! + * Konvertiert eine Weltkoordinate in eine Lowres-Kartenkoordinate + * @param koord Weltkoordiante + * @return Kartenkoordinate + */ +uint16_t world_to_map_lowres(int16_t koord) { +#if (1000 / MAP_RESOLUTION_LOWRES) * MAP_RESOLUTION_LOWRES != 1000 + #warning "MAP_RESOLUTION_LOWRES ist kein Teiler von 1000" +#endif + return ((koord*MAP_RESOLUTION_LOWRES) / 1000) + (MAP_SIZE_LOWRES*MAP_RESOLUTION_LOWRES/2); +} + + + +/*! + * Zugriff auf ein Feld der Lowres-Karte. Kann lesend oder schreibend sein. + * @param x X-Ordinate der Karte + * @param y Y-Ordinate der Karte + * @param value Neuer Wert des Feldes (> 0 heisst frei, <0 heisst belegt) + * @param set 0 zum Lesen, 1 zum Schreiben + * @return Mapwert + */ +int8_t access_field_lowres(uint16_t x, uint16_t y, int8_t value, uint8_t set) { + uint16 section_x, section_y, index_x, index_y; + + // Berechne in welcher Sektion sich der Punkt befindet + section_x=x/ MAP_SECTION_POINTS_LOWRES; + section_y=y/ MAP_SECTION_POINTS_LOWRES; + + if ((section_x>= MAP_SECTIONS_LOWRES) || (section_y >= MAP_SECTIONS_LOWRES)){ + #ifdef PC + printf("Versuch ein Feld ausserhalb der Karte zu lesen!! x=%d y=%d\n",x,y); + #endif + return 0; + } + + if (set) { // Schreibzugriff + + // Eventuell existiert die Section noch nicht + if (map_lowres[section_x][section_y] == NULL){ + // Dann anlegen + map_lowres[section_x][section_y]= malloc(sizeof(map_section_lowres_t)); + for (index_x=0; index_x<MAP_SECTION_POINTS_LOWRES; index_x++) + for (index_y=0; index_y<MAP_SECTION_POINTS_LOWRES; index_y++) + map_lowres[section_x][section_y]->section[index_x][index_y]=0; + } + // Berechne den Index innerhalb der Section + index_x = x % MAP_SECTION_POINTS_LOWRES; + index_y = y % MAP_SECTION_POINTS_LOWRES; + map_lowres[section_x][section_y]->section[index_x][index_y]=value; + return value; // Schluss mit Werterueckgabe nach Schreibzugriff + + } + + // Eventuell existiert die Section noch nicht + if (map_lowres[section_x][section_y] == NULL) + return 0; + + // Berechne den Index innerhalb der Section + index_x = x % MAP_SECTION_POINTS_LOWRES; + index_y = y % MAP_SECTION_POINTS_LOWRES; + return map_lowres[section_x][section_y]->section[index_x][index_y]; +} + + +/*! + * Loescht die komplette Lowres-Karte + */ +static inline void delete_lowres(void) { + uint8_t i,j; + LOG_DEBUG("Delete von 0- %1d",MAP_SECTION_POINTS_LOWRES); + for (j=0; j< MAP_SECTIONS_LOWRES*MAP_SECTION_POINTS_LOWRES; j++) { + LOG_DEBUG("zeile %1d",j); + for (i=0; i< MAP_SECTIONS_LOWRES*MAP_SECTION_POINTS_LOWRES; i++) { + access_field_lowres(i,j,0,1); + //LOG_DEBUG("Spalte %1d",i); + } + } +} + + + +//Ausgangspunkt der Welle (eigentlicher Zielpunkt) +static int16_t startwave_x=(MAP_SIZE_LOWRES*MAP_RESOLUTION_LOWRES/2); // Map-Mittelpunkt ist Initialwert +static int16_t startwave_y=(MAP_SIZE_LOWRES*MAP_RESOLUTION_LOWRES/2); + +/* Koordinate des Startpunktes (Botpos) zum Terminieren der fortlaufenden Welle, da diese vom Zielpunkt ausgeht */ +static int16_t endkoord_x=0; +static int16_t endkoord_y=0; + +/* das Rechteck der real benutzten Koordinaten der Welt */ +static int16_t min_x; +static int16_t min_y; +static int16_t max_x; +static int16_t max_y; + +/* gibt den Umkreisradius in mm an fuer die Durchschnittsermittlung */ +static int16_t average_val=0; + + +/*! + * Eintragen der Hindernisse in Lowres-Karte aus der Map-Highres-Karte + */ +void set_hazards(void) { + + int16_t x, y; + + // Umrechnen des die Welt umschliessenden Rechtecks in Lowres-Koordinaten + min_x=world_to_map_lowres(map_get_min_x()); + max_x=world_to_map_lowres(map_get_max_x()); + min_y=world_to_map_lowres(map_get_min_y()); + max_y=world_to_map_lowres(map_get_max_y()); + min_y=(min_y>0)?min_y:0; // sicherstellen dass Grenzen positiv sind + min_x=(min_x>0)?min_x:0; + + + int16_t ym; + int8_t mapavg; + + // Zellen durchlaufen und aus der hochaufloesenden Weltkarte Hindernisse hier eintragen + for (y=max_y; y>=min_y; y--) { + ym=(map_to_world_lowres(y)); //Zeilenwert als Weltkoord umrechnen + for (x=min_x; x<=max_x; x++) { + + // Durchschnittswert der echten Weltkarte fuer diese Zelle holen; falls mal Weg nicht gefunden werden konnte, wird + // schrittweise Radius fuer den Durchschnitt verringert-vielleicht wird dann Weg gefunden + mapavg=map_get_average(map_to_world_lowres(x), ym, average_val); // probiert: 80 zu viele Hind 60 40 gut, 30 gut, 20 gut weniger auch weniger Hindernisse + if (mapavg<0) // hier koennte man bei Weg nicht findbar auch Schwellenwert schrittweise veraendern + access_field_lowres(x,y,1,1); + } + } + + //Wellen-Startpunkt eintragen mit Mapwert 2 + access_field_lowres(startwave_x,startwave_y,2,1); + +} + + +// Wellenzaehler; uint8 ausreichend da in dieser Aufloesung 8 Punkte je Meter -> bei 12 Meter noch unterhalb Wertebereich +static uint8_t wavecounter=0; + +/*! + * Wellenwert wird auf die uebergebene Koordinate eingetragen und in die FIFO-Queu uebernommen auf nicht-Hinderniswert + * @param map_x X-Lowres-Map Koordinate + * @param map_y Y-Lowres-Map Koordinate + * @param actual_wave einzutragender Wellenwert + * @param end_reached Kennung ob schon Zielpunkt erreicht wurde + * @param *neighbour_found Kennung ob Nachbarzelle gueltig war (kein Hindernis und initial), d.h.Kennung ob Welle gueltigen Nachbarn gefunden hatte + * @return True wenn Zielpunkt erreicht sonst Wert end_reached selbst + */ +uint8_t get_neighbour(int16_t map_x,int16_t map_y, int8_t actual_wave, uint8_t end_reached, uint8_t *neighbour_found) { + + // nur im gueltigen Bereich + if (map_x<0 || map_y<0 || map_x<min_x || map_x>max_x || map_y<min_y ||map_y>max_y) + return end_reached; + + // Mapwert auslesen aus uebergebener Koordinate des Nachbarn + int8_t mapval=access_field_lowres(map_x,map_y,0,0); + + //Wellenwert wird nur auf noch initiale Felder eingetragen + if (mapval==0) { //nicht fuer Hindernis + + //in Lowres-Map Wellenwert auf die Koordinaten vermerken + access_field_lowres(map_x,map_y,actual_wave,1); + + //Nachbarpunkt als gueltig kennzeichnen + *neighbour_found=True; + + //in FIFO-Queue aufnehmen zur weiteren Nachbarsuche + if (!pos_queue_queue(map_x,map_y)) + LOG_DEBUG(">> -Queue voll- %1d %1d bei Punkt: %1d %1d zu Welle: %1d",map_x,map_y,actual_wave); + } // nur initiale Felder + + // Ziel gefunden + if (map_x==endkoord_x && map_y==endkoord_y) { + LOG_DEBUG("Startkoord erreicht auf %1d %1d",map_x,map_y); + return True; + } + + //vorherigen Wert zurueckgeben + return end_reached; + +} // Ende get_neighbour + + +// Zustaende des Wellenverhaltens +#define SET_HAZARDS_TO_LOWRES 1 +#define NEIGHBOURS_FROM_FIFO 2 +#define SEARCH_STACKPATH_AND_QUEUE 3 +#define START_BOT_GO_STACK_BEHAVIOUR 4 +#define WAVE_NEXT_TRY 5 + +// Begrenzung des Wellenzaehlers, d.h. obere Grenze als Abbruchbedingung +#define MAX_WAVECOUNTER 120 + +uint8_t wave_state=0; // Statusvariable selbst + +/*! + * Wave-Verhalten; berechnet die Welle ausgehend vom Zielpunkt bis zur Botposition; dann wird diese zurueckverfolgt und sich der Pfad + * auf dem Stack gemerkt und anschliessend das Stack-Fahrverhalten aufgerufen + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + */ +void bot_calc_wave_behaviour(Behaviour_t *data) { + + // Mapvariable zum Durchlaufen der Lowres-Planungs-Map + static int16_t x; + static int16_t y; + + // Zaehlervariable zum bestimmen der Nachbarn + static int8_t i=0; + static int8_t j=0; + + // Kennung ob eine gueltige Nachbarzelle gefunden werden konnte + static uint8_t neighbour_found=0; + + // Endekennung + static uint8_t endreached=False; //Kennung gesetzt fuer Ziel gefunden; Terminierung der Schleife + + + switch (wave_state) { + // zuerst loeschen der Planungs-LowRes-Karte + case 0: + LOG_DEBUG("Loeschen der Lowres-Karte"); + delete_lowres(); + + pos_stack_clear(); // Stack/ Queue loeschen + + wavecounter=2; // geht ab Wert 2 los; d.h. Wert Wellenzentrum - Zielpunkt bekommt diesen Wert + + wave_state=SET_HAZARDS_TO_LOWRES; + break; + + // Uebertragen der in der Highres-Karte vermerkten Hindernisse in die Lowres-Planungs-Karte + case SET_HAZARDS_TO_LOWRES: + LOG_DEBUG("Hindernisse eintragen"); + set_hazards(); // Hindernisse aus der Highres-Karte in die Planungs-Lowres-Karte eintragen + wave_state=NEIGHBOURS_FROM_FIFO; + break; + + + // hier abarbeiten aller in der FIFO-Queue eingetragenen Koordinaten, d.h. fuer jede wird wieder der gueltige Nachbar in die Queue hinten eingefuegt bis Ende erreicht + case NEIGHBOURS_FROM_FIFO: + + wavecounter++; // die ersten Nachbarn haben den Startwellenwert (2) + 1 + endreached=False; // Abbruchbedingung der Schleife init. + + + pos_queue_queue(startwave_x,startwave_y); // Startpunkt der Welle in die Queue einfuegen, damit gehts los + + LOG_DEBUG("Wellenstart bei %1d %1d Welle %1d, avg %1d",startwave_x,startwave_y,wavecounter,average_val); + + pos_queue_queue(999,999); // nach Abarbeiten des Wellenstartpunktes muss Wellenzaehler erhoeht werden + + // Solange FIFO-Queue durchgehen und Nachbarn wieder anfuegen bis Queue leer ist oder Ziel (Botstartpunkt) gefunden (1 Nachbar ist Zielfeld) + while (!endreached && pos_queue_dequeue(&x,&y)) { + + // bei Kennung 999 ist Welle abgearbeitet, d.h. zu allen Punkten der Welle die Nachbarn wieder hinten angefuegt und Wellenzaehler wird erhoeht + // wenn die Nachbarn der neuen Welle beginnen wird Wellenzaehler erhoeht + if (x==999 && y==999) { + // die letzte in Queue eingefuegte Koord wird sich gemerkt; wenn zu dieser bei naechster Welle alle Nachbarn + // bearbeitet sind, ist naechste Welle vorbei + wavecounter++; + + // Erhoehung fuer Wellenzaehler vermerken wenn Endepunkt noch nicht gefunden ist und gueltiger Nachbarpunkt vorhanden war + if (!endreached && neighbour_found) + pos_queue_queue(999,999); + + // Kennung fuer gueltigen Nachbarn wieder ruecksetzen + neighbour_found= False; + + } + else { + // abarbeiten aller 4 Nachbarn zu einem Punkt der Queue, auch wenn schon gefunden wurde zwecks guter Pfadfindung; ein True wird nicht ueberschrieben + for (j=-1; j<=1; j++) + for (i=-1; i<=1; i++) { + if ((j==0 && i!=0)||(i==0 && j!=0)) { + endreached=get_neighbour(x+i,y+j,wavecounter,endreached, &neighbour_found); + }//if + }//for i for j + + } // else if + } // while + + // Loeschen der FIFO-Queue; Stack- bzw. Queue-Array wird ja im folgenden + // verwendet zur Speicherung der Abfahrpositionen bei der Wellenrueckverfolgung vom Bot ausgehend zum Wellenstartpunkt + pos_stack_clear(); + + // Wellenpfad zurueckverfolgen oder gleich Ende wenn nichts gefunden + wave_state=(endreached)?SEARCH_STACKPATH_AND_QUEUE:WAVE_NEXT_TRY; + + if (endreached) + LOG_DEBUG("Welle hat Botpos erreicht") + else + LOG_DEBUG("Welle konnte Botpos nicht erreichen->next try"); + + break; + + + // in der Lowres-Karte sind nun die Hindernisse und die Welle eingetragen; vom Botpunkt zum Wellenstartpunkt + // wird die Welle nun zurueckverfolgt und die anzufahrenden Koordinaten in den Stack gespeichert + case SEARCH_STACKPATH_AND_QUEUE: + + LOG_DEBUG("--Welle hat Botposition erreicht bei Wert %1d avg %1d",wavecounter,average_val); + + // ein paar notwendige Initialisierungen und Variablendeklarationen + int8_t minval=1; + + uint16_t x_nextdest=0; + uint16_t y_nextdest=0; + + // Ausgehend vom Wellen-Ausgangspunkt, dem Zielpunkt, wird jeweils der Nachbar genommen mit dem kleinsten Wert + x=endkoord_x; + y=endkoord_y; + + int8_t mapval_min=wavecounter+1; //auf erhoehten Wellenwert setzen, weil in Schleife der Wellenwert immer kleiner dem letzten Wellewert ist + endreached=False; // Schleifenabbruchvar init. + wavecounter=0; // Wellenzaehler init. + + //Map durchlaufen und den Nachbarn mit immer niedrigerem Mapwert verfolgen bis Wellenstartpunkt erreicht wurde + while (!endreached) { + + neighbour_found=False; + + // fuer jeden Nachbarn von Botposition bis Wellenstartpunkt den immer kleineren Wellenwert verfolgen + for (j=-1; (j<=1)&&!endreached; j++) + for (i=-1; (i<=1)&&!endreached; i++) + + //Vergleich nur fuer die richtigen 4 Nachbarn und wenn Wellenstartpunkt noch nicht erreicht + if (((j==0 && i!=0)||(i==0 && j!=0))&&!(x+i==endkoord_x && y+j==endkoord_y)) {//nur gueltige Koords gecheckt + minval=access_field_lowres(x+i,y+j,0,0); // Mapwert auslesen + + // die Koordinate mit niedrigstem Wellenwert merken + if (minval>1 && minval<mapval_min) {//naechster genommener Wellenwert muss kleiner aus letztem Lauf sein + x_nextdest=x+i; + y_nextdest=y+j; + + LOG_DEBUG("kleinster Wellenwert %1d Koord %1d %1d",minval,x_nextdest,y_nextdest); + + // Wellenwert und Kennung fuer Nachbar gefunden setzen + mapval_min=minval; + neighbour_found=True; + + // wenn ein Nachbar Zielpunkt ist Endekennung setzen + if (x_nextdest==startwave_x && y_nextdest==startwave_y) { + endreached=True; + LOG_DEBUG("Ende gefunden %1d %1d",x_nextdest,y_nextdest); + } + } //if + } //if + + // hier die Punkte jeweils in die Queue (Stack) einfuegen; das hier zuerst eingefuegte naechste Ziel ist ja bei der Wellenrueckverfolgung die naechste Position vom Bot ausgehend; + // die Positionen werden aber hinten angefuegt, d.h. die naechste Botpos zuerst bis zum Ziel (Start der Welle); das Botfahrverhalten muss dann aber auch die hier zuerst eingefuegten + // Positionen auch zuerst abfahren + if (neighbour_found || endreached) { + LOG_DEBUG("Pfadpunkt in Queue %1d %1d Ende %1d",x_nextdest,y_nextdest,endreached); + // zum spaeteren Stack-Abfahren die Zwischenziel-Koordinaten als Weltkoordinaten auf den Stack legen + if (!pos_queue_queue(map_to_world_lowres(x_nextdest),map_to_world_lowres(y_nextdest))) { + LOG_DEBUG("Queue ging schief -voll ?"); + endreached=True; + } + + // Der zu fahrende Pfad kann in der Highres-Map als helle Punkte eingezeichnet werden; dazu nach Pfadplanung und zu Beginn des Fahrverhaltens + // abbrechen und der zu fahrende Weg ist nun in der Highres-map eingezeichnet; die Routine access_field() muss dazu aber auch in der map.h deklariert werden +#ifdef SHOW_PATH_IN_MAP + access_field(world_to_map(map_to_world_lowres(x_nextdest)),world_to_map(map_to_world_lowres(y_nextdest)),127,1); +#endif + } + wavecounter++; // Wellencounter erhoehen; dient hier nur fuer Abbruchbedingung nach erreichen eines bestimmten Zaehlerstandes + + if (wavecounter>=MAX_WAVECOUNTER) { // spaetestens jetzt Abbruchbedingung zur Sicherheit + endreached=True; + LOG_DEBUG("Endecounter erreicht %1d",wavecounter); + } + + // Ausgangspunkt fuer naechsten Durchlauf setzen, also ab Nachbarpunkt mit geringstem Wellenwert weiter zurueckverfolgen + x=x_nextdest; + y=y_nextdest; + } //while + + // Wenn erfolgreich Pfad gefunden wurde, dann abfahren sonst Ende + wave_state=(endreached)?START_BOT_GO_STACK_BEHAVIOUR:99; + if (endreached) + LOG_DEBUG("Pfad rueckwaerts gefunden->Stackgo") + else + LOG_DEBUG("Pfad not found->Ende"); + + break; + + // Welle konnte nicht Botposition erreichen, evtl. versperren zu viel eingetargene Hindernisse den Weg; + // hier mehrfacher Versuch durch Verringerung des Durchmesserumkreises weniger Hindernisse einzutragen + case WAVE_NEXT_TRY: + average_val=average_val-10; + wave_state=(average_val>=10)?0:99; + if (average_val>=10) + LOG_DEBUG(">>>>>>>>>>> nix gefunden und naechster Versuch im Umkreis %1d",average_val); + break; + + + case START_BOT_GO_STACK_BEHAVIOUR: + LOG_DEBUG("--Pfad gefunden und Abfahren -STACKGO-- avg %1d",average_val); + bot_drive_stack_fifo(data); + wave_state=99; + break; + + + default: + LOG_DEBUG("Waveverhalten beendet Wavecounter %1d Durchmesserwert %1d",wavecounter,average_val); + return_from_behaviour(data); + break; + } +} + +/*! + * Routine zum Setzen der Zielkoordinaten + * @param x X-Map-Zielkoordinate + * @param y Y-Map-Zielkoordinate + */ +void bot_set_destination(int16 x, int16 y) { + startwave_x = x; + startwave_y = y; + + //Wellen Startpunkt eintragen + LOG_DEBUG("Wellen-Start bei %1d %1d",startwave_x,startwave_y); +} + + +/*! + * Rufe das Wave-Verhalten auf + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + */ +void bot_do_calc_wave(Behaviour_t * caller) { + + switch_to_behaviour(caller,bot_calc_wave_behaviour,OVERRIDE); + wave_state = 0; + + average_val=40; // beginnend mit Radius 40mm zum Eintragen der Hindernisse aus der Highres- in die Planungs-Lowres-Karte + + LOG_DEBUG("Start Welle vom Zielpunkt %1d %1d",startwave_x,startwave_y); + + //Botkoordinate setzen zum terminieren des Wellenverhaltens + endkoord_x=world_to_map_lowres(x_pos); + endkoord_y=world_to_map_lowres(y_pos); + LOG_DEBUG("Botpos auf %1d %1d",endkoord_x,endkoord_y); + + /* Kollisions-Verhalten ausschalten */ +#ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + deactivateBehaviour(bot_avoid_col_behaviour); +#endif + + +} + +/*! + * Rufe das Wave-Verhalten auf mit Uebergabe des zu erreichenden Zielpunkten + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + * @param dest_x X-World-Zielkoordinate + * @param dest_y Y-World-Zielkoordinate + */ +void bot_calc_wave(Behaviour_t * caller, int16_t dest_x, int16_t dest_y) { + + //Zielpunkt setzen, ab dem die Welle losgeht + bot_set_destination(world_to_map_lowres(dest_x),world_to_map_lowres(dest_y));//Weltkoords in Lowres-Mapkoords umrechnen + + bot_do_calc_wave(caller); +} + +#ifdef DEBUG_PATHPLANING +/*! + * Zeigt einen Ausschnitt der Planungs-Map auf Konsole an; gut zum pruefen wo Hindernisse gesehen werden und die Welle verlaeuft + */ +void show_labmap(void){ + + int16 x,y; + int16_t xw; + int16_t yw; + + LOG_DEBUG("Wellenstart vom Zielpunkt %1d %1d",startwave_x,startwave_y); + access_field_lowres(startwave_x,startwave_y,2,1); //vermerken des Wellen-Startpunktes + LOG_DEBUG("Botpunkt %1d %1d",endkoord_x,endkoord_y); + access_field_lowres(endkoord_x,endkoord_y,88,1); // Botpos vermerken + + int8_t tmp=0; + for (y=40; y< 60; y++) { + // for (y=0; y< 20; y++) { // wenn nur 4m + printf("y: %2d:", y); + + // for (x=10; x<= 30; x++) { //wenn nur 4m + for (x=40; x<= 60; x++) { + xw=map_to_world_lowres(x); + yw=map_to_world_lowres(y); + tmp=access_field_lowres(x,y,tmp,0); + + printf("%2d|",tmp); + } + printf("\n"); + + } + } + +#endif + + + +#ifdef PATHPLANING_DISPLAY + + static void pathplaning_disp_key_handler(void){ + /* Keyhandling fuer Pathplaning-Verhalten */ + switch (RC5_Code){ + + case RC5_CODE_4: + RC5_Code = 0; + delete_lowres(); + break; + + case RC5_CODE_5: + RC5_Code = 0; + bot_do_calc_wave(0); + break; + +#ifdef DEBUG_PATHPLANING + case RC5_CODE_6: + RC5_Code = 0; + show_labmap(); + break; +#endif + case RC5_CODE_8: + RC5_Code = 0; + bot_set_destination(world_to_map_lowres(x_pos),world_to_map_lowres(y_pos)); + break; + + }//case + + } // Ende Keyhandler + + + /*! + * @brief Display der Pfadplanung-Routinen + */ + void pathplaning_display(void){ + + display_cursor(1,1); + display_printf("-Pathplaning-"); + display_cursor(2,1); + display_printf("F4:Delete"); + display_cursor(3,1); + display_printf("F5:GoPlaning"); + display_cursor(4,1); +#ifdef DEBUG_PATHPLANING + display_printf("F6/8:ShowMap/SetDest"); +#else + display_printf("F8:SetDest"); +#endif + + pathplaning_disp_key_handler(); + } +#endif // PATHPLANING_DISPLAY + + +#endif // BEHAVIOUR_PATHPLANING_AVAILABLE Index: C:/botneu/ct-Bot/bot-logic/bot-logik.c =================================================================== --- C:/botneu/ct-Bot/bot-logic/bot-logik.c (revision 1486) +++ C:/botneu/ct-Bot/bot-logic/bot-logik.c (working copy) @@ -275,6 +275,10 @@ insert_behaviour_to_list(&behaviour, new_behaviour(170, bot_observe_right_behaviour, INACTIVE)); insert_behaviour_to_list(&behaviour, new_behaviour(72, bot_drive_area_behaviour, INACTIVE)); #endif + + #ifdef BEHAVIOUR_PATHPLANING_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(71, bot_calc_wave_behaviour, INACTIVE)); + #endif #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE // Verhalten um einer Linie zu folgen Index: C:/botneu/ct-Bot/bot-logic/behaviour_drive_stack.c =================================================================== --- C:/botneu/ct-Bot/bot-logic/behaviour_drive_stack.c (revision 1486) +++ C:/botneu/ct-Bot/bot-logic/behaviour_drive_stack.c (working copy) @@ -34,6 +34,7 @@ #include "rc5-codes.h" #include "pos_stack.h" #include "math_utils.h" +#include "rc5-codes.h" #include <stdlib.h> static uint8_t drivestack_state = 0; /*!< Status des drive_stack-Verhaltens */ @@ -43,17 +44,25 @@ static int16_t posx = 0; static int16_t posy = 0; +static uint8_t go_fifo = 0; /*!< falls True wird nich mit Pop nach LIFO sondern via Queue FIFO gefahren */ + /*! * Verhalten zum Anfahren aller auf dem Stack befindlichen Punkte, wobei das Fahr-Unterverhalten bot_goto_pos benutzt wird * @param *data Der Verhaltensdatensatz */ void bot_drive_stack_behaviour(Behaviour_t * data) { + uint8_t get_pos; + switch (drivestack_state) { case 0: // Koordinaten werden vom Stack geholt und angefahren; Ende nach nicht mehr erfolgreichem Pop - if (!pos_stack_pop(&posx, &posy)) - drivestack_state = 1; - else + + // wenn Fifo-Queue definiert, kann sowohl nach LIFO (Stack) oder FIFO (Queue) gefahren werden + get_pos=(go_fifo)?pos_queue_dequeue(&posx,&posy):pos_stack_pop(&posx, &posy); + + if (!get_pos) + drivestack_state = 1; + else bot_goto_pos(data, posx, posy, 999); break; @@ -76,7 +85,8 @@ void bot_drive_stack(Behaviour_t * caller) { switch_to_behaviour(caller, bot_drive_stack_behaviour, OVERRIDE); drivestack_state = 0; - if (behaviour_is_activated(bot_put_stack_waypositions_behaviour)) { + go_fifo = 0; + if (behaviour_is_activated(bot_put_stack_waypositions_behaviour)) { /* falls put_stack_waypos an ist, temporaer deaktivieren */ deactivateBehaviour(bot_put_stack_waypositions_behaviour); put_stack_active = 1; @@ -86,11 +96,21 @@ } /*! + * Botenfunktion: Verhalten zum Anfahren aller in der FIFO-Queue befindlichen Punkte + * @param *caller Der Verhaltensdatensatz des Aufrufers + */ +void bot_drive_stack_fifo(Behaviour_t * caller) { + go_fifo = True; + switch_to_behaviour(caller, bot_drive_stack_behaviour, OVERRIDE); + drivestack_state = 0; +} + +/*! * Speichern der uebergebenen Koordinaten auf dem Stack * @param pos_x X-Koordinate * @param pos_y Y-Koordinate */ -static void bot_push_pos(int16_t pos_x, int16_t pos_y) { +void bot_push_pos(int16_t pos_x, int16_t pos_y) { // sichern der Koordinaten in den Stack posx = pos_x; posy = pos_y; @@ -131,7 +151,7 @@ /*! * Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken; Verhalten ist nach Aktivierung via Botenroutine * ein Endlosverhalten und sammelt bis zur Deaktivierung die Wegepunkte; deaktiviert wird es via Notaus oder direkt mit Befehl zum Zurueckfahren und - * damit Start des Stack-Fahrverhaltens + * damit Start des Stack-Fahrverhaltens * @param *data Der Verhaltensdatensatz */ void bot_put_stack_waypositions_behaviour(Behaviour_t * data) { @@ -143,7 +163,7 @@ // Abstand zur letzten Position ueberschritten if (get_dist(last_xpos, last_ypos, x_pos, y_pos) > DIST_FOR_PUSH) { - // kein Push notwendig bei gerader Fahrt voraus zum Sparen des Stack-Speicherplatzes + // kein Push notwendig bei gerader Fahrt voraus zum Sparen des Stack-Speicherplatzes if ((int16) heading != last_heading) { bot_push_pos(x_pos, y_pos); } @@ -207,6 +227,12 @@ RC5_Code = 0; bot_put_stack_waypositions(NULL); break; + + case RC5_CODE_7: + /* Verhalten starten zum Anfahren der Stackpunkte vom Startpunkt an vorwaerts */ + RC5_Code = 0; + bot_drive_stack_fifo(NULL); + break; case RC5_CODE_8: /* Loeschen des Positionsstacks */ @@ -219,15 +245,15 @@ /*! - * Display zum Setzen und Anfahren der Stackpunkte + * Display zum Setzen und Anfahren der Stackpunkte */ void drive_stack_display(void) { display_cursor(1, 1); - display_printf("Bot-Pos %5d %5d", (int16_t)x_pos, (int16_t)y_pos); + display_printf("Stack %5d %5d", posx, posy); display_cursor(2, 1); - display_printf("Stack %5d %5d", posx, posy); + display_printf("Save/Del : 3/8"); display_cursor(3, 1); - display_printf("Save/Goto/Del: 3/4/8"); + display_printf("GoBack/Forward: 4/7"); display_cursor(4, 1); display_printf("Start WayPushPos: 5"); Index: C:/botneu/ct-Bot/Changelog.txt =================================================================== --- C:/botneu/ct-Bot/Changelog.txt (revision 1486) +++ C:/botneu/ct-Bot/Changelog.txt (working copy) @@ -1,5 +1,7 @@ CHANGELOG fuer c't-Bot ====================== +2008-09-30 Frank Menzel [Menzelfr@xxxxxxx]: behaviour_pathplaning zugefuegt mit eigenem Screen + 2008-09-16 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Verbesserungen im MMC-Code (groesserer Timeout bei HW-SPI, Debug-Ausgaben bei MiniFAT) 2008-09-06 Frank Menzel [Menzelfr@xxxxxxx]: Verhalten drive_area stark ueberarbeitet Index: C:/botneu/ct-Bot/ct-Bot.h =================================================================== --- C:/botneu/ct-Bot/ct-Bot.h (revision 1486) +++ C:/botneu/ct-Bot/ct-Bot.h (working copy) @@ -53,7 +53,7 @@ #define MEASURE_MOUSE_AVAILABLE /*!< Geschwindigkeiten werden aus den Maussensordaten berechnet */ //#define MEASURE_COUPLED_AVAILABLE /*!< Geschwindigkeiten werden aus Maus- und Encoderwerten ermittelt und gekoppelt */ -//#define POS_STACK_AVAILABLE /*!< Positionsstack vorhanden */ +#define POS_STACK_AVAILABLE /*!< Positionsstack vorhanden */ //#define WELCOME_AVAILABLE /*!< kleiner Willkommensgruss */ @@ -70,7 +70,7 @@ #define BEHAVIOUR_AVAILABLE /*!< Nur wenn dieser Parameter gesetzt ist, exisitiert das Verhaltenssystem */ -//#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */ +#define MAP_AVAILABLE /*!< Aktiviere die Kartographie */ //#define SPEED_CONTROL_AVAILABLE /*!< Aktiviert die Motorregelung */ //#define ADJUST_PID_PARAMS /*!< macht PID-Paramter zur Laufzeit per FB einstellbar */ Index: C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h =================================================================== --- C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h (revision 1486) +++ C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h (working copy) @@ -33,6 +33,7 @@ #define BEHAVIOUR_SERVO_AVAILABLE /*!< Kontrollverhalten fuer die Servos */ +#define BEHAVIOUR_PATHPLANING_AVAILABLE /*!< Pfadplanungsverhalten */ //#define BEHAVIOUR_DRIVE_STACK_AVAILABLE /*!< Abfahren der auf dem Stack gesicherten Koordinaten */ //#define BEHAVIOUR_OLYMPIC_AVAILABLE /*!< Olympiadenverhalten vorhanden? */ @@ -62,6 +63,7 @@ #ifndef MAP_AVAILABLE #undef BEHAVIOUR_SCAN_AVAILABLE #undef BEHAVIOUR_DRIVE_AREA_AVAILABLE + #undef BEHAVIOUR_PATHPLANING_AVAILABLE #endif #ifndef POS_STACK_AVAILABLE @@ -69,7 +71,11 @@ #warning "drive_area() geht nur, wenn POS_STACK_AVAILABLE" #undef BEHAVIOUR_DRIVE_AREA_AVAILABLE #endif +#ifdef BEHAVIOUR_PATHPLANING_AVAILABLE + #warning "Pfadplanung geht nur, wenn POS_STACK_AVAILABLE" + #undef BEHAVIOUR_PATHPLANING_AVAILABLE #endif +#endif #ifdef BEHAVIOUR_DRIVE_AREA_AVAILABLE #define BEHAVIOUR_GOTO_POS_AVAILABLE @@ -77,6 +83,10 @@ #define BEHAVIOUR_CANCEL_BEHAVIOUR_AVAILABLE #endif +#ifdef BEHAVIOUR_PATHPLANING_AVAILABLE + #define BEHAVIOUR_DRIVE_STACK_AVAILABLE +#endif + #ifdef BEHAVIOUR_GOTOXY_AVAILABLE #define BEHAVIOUR_TURN_AVAILABLE #endif @@ -230,5 +240,7 @@ #include "bot-logic/behaviour_drive_area.h" +#include "bot-logic/behaviour_pathplaning.h" + #endif // BEHAVIOUR_AVAILABLE #endif /*AVAILABLE_BEHAVIOURS_H_*/ Index: C:/botneu/ct-Bot/include/bot-logic/behaviour_pathplaning.h =================================================================== --- C:/botneu/ct-Bot/include/bot-logic/behaviour_pathplaning.h (revision 0) +++ C:/botneu/ct-Bot/include/bot-logic/behaviour_pathplaning.h (revision 0) @@ -0,0 +1,76 @@ +/* + * 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 behaviour_pathplaning.h + * @brief Wave-Pfadplanungsverhalten; eine niedrigaufloesende Map wird ueber die hochaufloesende gelegt und + * auf dieser folgende Schritte ausgefuehrt: + * -Innerhalb des wirklich benutzten Mappenbereiches wird jede Zelle durchlaufen und falls der Durchschnittswert + * der Hochaufloesenden Map < 0 ist (Hinderniswert) hier eingetragen mit Wert 1 + * -anzufahrende Zielposition erhaelt Mapwert 2 + * -ausgehend von Zielposition laeuft eine Welle los bis zum Bot-Ausgangspunkt, d.h. von 2 beginnend erhaelt jede Nachbarzelle + * den naechst hoeheren Zellenwert + * -wird als ein Nachbar die Botposition erreicht, wird der Pfad zurueckverfolgt und immer die Zelle mit kleinerem Wert gewaehlt; + * die Zellenkoordinaten werden als Weltkoordinaten auf den Stack gelegt und kann nun abgefahren werden + + * + * @author Frank Menzel (Menzelfr@xxxxxxx) + * @date 23.09.2008 +*/ + + +#ifndef BEHAVIOUR_PATHPLANING_H_ +#define BEHAVIOUR_PATHPLANING_H_ + +#include "ct-Bot.h" +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_PATHPLANING_AVAILABLE + +/*! + * @brief Display der Pfadplanung-Routinen + */ +void pathplaning_display(void); + +/*! + * Rufe das Wave-Verhalten auf mit Uebergabe des zu erreichenden Zielpunkten + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + * @param dest_x X-World-Zielkoordinate + * @param dest_y Y-World-Zielkoordinate + */ +void bot_calc_wave(Behaviour_t * caller, int16_t dest_x, int16_t dest_y); + +/*! + * Rufe das Wave-Verhalten auf + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + */ +void bot_do_calc_wave(Behaviour_t * caller); + +/*! + * Wave-Verhalten; berechnet die Welle ausgehend vom Zielpunkt bis zur Botposition; dann wird diese zurueckverfolgt und sich der Pfad + * auf dem Stack gemerkt und anschliessend das Stack-Fahrverhalten aufgerufen + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + */ +void bot_calc_wave_behaviour(Behaviour_t *data); + + +#endif // BEHAVIOUR_PATHPLANING_AVAILABLE +#endif /* BEHAVIOUR_PATHPLANING_H_ */ Index: C:/botneu/ct-Bot/include/bot-logic/behaviour_drive_stack.h =================================================================== --- C:/botneu/ct-Bot/include/bot-logic/behaviour_drive_stack.h (revision 1486) +++ C:/botneu/ct-Bot/include/bot-logic/behaviour_drive_stack.h (working copy) @@ -53,9 +53,16 @@ void bot_push_actpos(Behaviour_t * caller); /*! + * Speichern der uebergebenen Koordinaten auf dem Stack + * @param pos_x X-Koordinate + * @param pos_y Y-Koordinate + */ +void bot_push_pos(int16_t pos_x, int16_t pos_y); + +/*! * Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken; Verhalten ist nach Aktivierung via Botenroutine * ein Endlosverhalten und sammelt bis zur Deaktivierung die Wegepunkte; deaktiviert wird es via Notaus oder direkt mit Befehl zum Zurueckfahren und - * damit Start des Stack-Fahrverhaltens + * damit Start des Stack-Fahrverhaltens * @param *data Der Verhaltensdatensatz */ void bot_put_stack_waypositions_behaviour(Behaviour_t * data); @@ -67,9 +74,15 @@ void bot_put_stack_waypositions(Behaviour_t * caller); /*! - * Display zum Setzen und Anfahren der Stackpunkte + * Display zum Setzen und Anfahren der Stackpunkte */ void drive_stack_display(void); +/*! + * Botenfunktion: Verhalten zum Anfahren aller in der FIFO-Queue befindlichen Punkte + * @param *caller Der Verhaltensdatensatz des Aufrufers + */ +void bot_drive_stack_fifo(Behaviour_t * caller); + #endif // BEHAVIOUR_DRIVE_STACK_AVAILABLE #endif /*BEHAVIOUR_DRIVESTACK_H_*/ Index: C:/botneu/ct-Bot/include/map.h =================================================================== --- C:/botneu/ct-Bot/include/map.h (revision 1486) +++ C:/botneu/ct-Bot/include/map.h (working copy) @@ -35,8 +35,8 @@ #define CLEAR_MAP_ON_INIT /*!< Loescht die Karte, wenn der Bot gebootet wird */ /* Es lohnt nicht gigantische Karten auszugeben, wenn sie nichts enthalten, daher hier zwei Varianten, um die Karte auf die realen Groesse zu reduzieren */ -//#define SHRINK_MAP_ONLINE /*!< Wenn gesetzt, wird bei jedem Update der belegte Bereich der Karte protokolliert. Pro: schnelle ausgabe Contra permanenter aufwand */ -#define SHRINK_MAP_OFFLINE /*!< Wenn gesetzt, wird erst bei der Ausgabe der belegte Bereich der Karte berechnet. Pro: kein permanenter aufwand Contra: ausgabe dauert lange */ +#define SHRINK_MAP_ONLINE /*!< Wenn gesetzt, wird bei jedem Update der belegte Bereich der Karte protokolliert. Pro: schnelle ausgabe Contra permanenter aufwand */ +//#define SHRINK_MAP_OFFLINE /*!< Wenn gesetzt, wird erst bei der Ausgabe der belegte Bereich der Karte berechnet. Pro: kein permanenter aufwand Contra: ausgabe dauert lange */ /* Geomtrie der Karte - Achtung, nur aendern, wenn man die Konsequenzen genau kennt! */ #define MAP_SIZE 12.288 /*!< Kantenlaenge der Karte in Metern. Zentrum ist der Startplatz des Bots. Achtung, MAP_SIZE*MAP_RESOLUTION muss ganzzahliges Vielfaches von MACRO_BLOCK_LENGTH sein */ Index: C:/botneu/ct-Bot/include/pos_stack.h =================================================================== --- C:/botneu/ct-Bot/include/pos_stack.h (revision 1486) +++ C:/botneu/ct-Bot/include/pos_stack.h (working copy) @@ -39,7 +39,7 @@ /*! ab hier der statische Stack mittels Array */ #ifdef ARRAY_POINT_STACK -#define POS_STACK_SIZE 20 // Stackgroesse ist beim Array begrenzt +#define POS_STACK_SIZE 50 // Stackgroesse ist beim Array begrenzt /*! Datenstruktur eines Koordinatenpunktes */ typedef struct { @@ -47,6 +47,21 @@ int16_t posy; } pos_stack_t; + +/*! + * Erweiterung des Stacks zur Queue; Element wird hinten angefuegt, identisch dem Stack-Push + * @return True wenn erfolgreich sonst False wenn Array voll ist + */ +uint8_t pos_queue_queue(int16_t x, int16_t y); + +/*! + * Erweiterung des Stacks zur Queue; Element wird vorn entnommen und alle anderen Elemente nach vorn hin verschoben + * @return True wenn Element erfolgreich entnommen werden konnte sonst False falls kein Element mehr enthalten ist + */ +uint8_t pos_queue_dequeue(int16_t * x, int16_t * y); + + + #else // ab hier der dynamische Stack /*! Datenstruktur eines Koordinatenpunktes */ Index: C:/botneu/ct-Bot/pos_stack.c =================================================================== --- C:/botneu/ct-Bot/pos_stack.c (revision 1486) +++ C:/botneu/ct-Bot/pos_stack.c (working copy) @@ -74,7 +74,7 @@ pos_sp->posx = x; pos_sp->posy = y; pos_sp++; - stack_count++; + stack_count++; } /*! @@ -92,6 +92,55 @@ return True; } + +static pos_stack_t * firstpos_sp = pos_stack; // Zeiger auf 1. Element +static pos_stack_t * sp_counter = pos_stack; + +/*! + * Erweiterung des Stacks zur Queue; Element wird hinten angefuegt, identisch dem Stack-Push + * @return True wenn erfolgreich sonst False wenn Array voll ist + */ +uint8_t pos_queue_queue(int16_t x, int16_t y) { + if (is_full()) + return False; + pos_stack_push(x,y); + return True; +} + +/*! + * Erweiterung des Stacks zur Queue; Element wird vorn entnommen und alle anderen Elemente nach vorn hin verschoben + * @return True wenn Element erfolgreich entnommen werden konnte sonst False falls kein Element mehr enthalten ist + */ +uint8_t pos_queue_dequeue(int16_t * x, int16_t * y) { + if (is_empty()) + return False; + + + firstpos_sp = pos_stack; // auf 1. Position setzen + sp_counter = pos_stack; + + *x = firstpos_sp->posx; + *y = firstpos_sp->posy; + //LOG_DEBUG("in leave %1d %1d counter: %1d",*x,*y,stack_count); + + + uint8_t i=0; + sp_counter++; // bereits auf naechstes Element setzen + + // hier muessen alle Elemente nach vorn gezogen werden + for (i=1; i< stack_count; i++) { + firstpos_sp->posx = sp_counter->posx; + firstpos_sp->posy = sp_counter->posy; + + firstpos_sp++; + sp_counter++; + } + stack_count--; // Stackcounter verringern + pos_sp--; //Letzten Stackzeiger verringern + + return True; +} + #else // ab hier der dynamische Stack */ static pos_stack_t * Point_Stack; /*!< die eigentlich zu verwendende Stack-Variable */ Index: C:/botneu/ct-Bot/ui/gui.c =================================================================== --- C:/botneu/ct-Bot/ui/gui.c (revision 1486) +++ C:/botneu/ct-Bot/ui/gui.c (working copy) @@ -269,6 +269,11 @@ #ifdef DISPLAY_DRIVE_STACK_AVAILABLE register_screen(&drive_stack_display); #endif + #ifdef PATHPLANING_DISPLAY + #ifdef BEHAVIOUR_PATHPLANING_AVAILABLE + register_screen(&pathplaning_display); + #endif + #endif } #endif // DISPLAY_AVAILABLE Index: C:/botneu/ct-Bot/ui/available_screens.h =================================================================== --- C:/botneu/ct-Bot/ui/available_screens.h (revision 1486) +++ C:/botneu/ct-Bot/ui/available_screens.h (working copy) @@ -45,6 +45,7 @@ #define DISPLAY_MAP_AVAILABLE /*!< Zeigt Map-Display an */ #define DISPLAY_TRANSPORT_PILLAR /*!< Steuerung Transport-Pillar-Verhalten auf diesem Screen */ #define DISPLAY_DRIVE_STACK_AVAILABLE /*!< Steuerung Stack-Verhalten auf diesem Screen */ +#define PATHPLANING_DISPLAY #ifndef SPEED_CONTROL_AVAILABLE #undef DISPLAY_REGELUNG_AVAILABLE Index: C:/botneu/ct-Bot/.settings/org.eclipse.cdt.core.prefs =================================================================== --- C:/botneu/ct-Bot/.settings/org.eclipse.cdt.core.prefs (revision 0) +++ C:/botneu/ct-Bot/.settings/org.eclipse.cdt.core.prefs (revision 0) @@ -0,0 +1,3 @@ +#Tue Sep 30 19:45:26 CEST 2008 +eclipse.preferences.version=1 +indexerId=org.eclipse.cdt.core.fastIndexer