## c't-Projekte - Mailinglisten

[Voriger (Datum)] [Nächster (Datum)] [Voriger (Thread)] [Nächster (Thread)]
[Nach Datum][Nach Thread]

### [ct-bot] neues Verhalten bot_drive_line_shortest_way...

Absender: Frank Menzel
Datum: Mi, 24.12.2008 10:58:30

    Hallo,
    in letzter Zeit ist es etwas scheinbar etwas ruhiger in der Mailingliste
    geworden. So möchte ich pünktlich zu Weihnachten diese Ruhe etwas stören
    und mein neues Verhalten bot_drive_line_shortest_way vorstellen.
    Hier die Beschreibung laut Programmtext:
    /*!
     * @file    behaviour_drive_line_shortest_way.c
     * @brief   Linienverfolger, der an Kreuzungen eine bestimmte
    Vorzugsrichtung einschlaegt (links) und diesen Weg weiterverfolgt, bis
    das Ziel
     *          (gruenes Feld an Wand) gefunden ist; Linien muessen immer an
    einem gruenen Feld ohne Hindernis enden, damit der botein Ende erkennt
    und
     *          umdrehen kann
     *          Die Kreuzungen und der eingeschlagene Weg werden auf dem
    Stack vermerkt, Wege die nicht zum Ziel fuehren vergessen; Am Ziel
    angekommen
     *          steht im Stack der kuerzeste Weg; Es kann nun via Display
    auf dem kuezesten Weg zum Ausgangspunkt zurueckgefahren werden oder der
    Bot wieder
     *          manuell an den Start gestellt werden und das Ziel auf
    kuerzestem Weg angefahren werden
     * @author Frank Menzel (Menzelfr@xxxxxx)
     * @date    21.12.2008
     */

    Anbei auch der Parcour, mit dem das Verhalten funktioniert. Auf dem zum
    Verhalten gehörenden Screen kann mit Taste 5 (GoLine) das Verhalten
    gestartet werden, der Bot fährt vorwärts zur Linie und weiter via
    Linienverfolger. An einer Kreuzung angekommen schlägt er den linken Weg
    ein und erforscht so alle Wege und Möglichkeiten zum Ziel zu kommen. Das
    Ziel ist das rechte grüne Feld an der Wand (Zieldefinition Grün vor
    Wand). Eine Linie endet an einem grünen Feld, wo der bot dann umkehrt
    und zur Kreuzung zurückfährt und dann wieder den linken Weg nimmt. Die
    Kreuzungen und die relativen Pfadabbiegungen werden auf den Stack
    gespeichert. Eine Kreuzung, wo alle Wege ins Nichts führen, wird
    vergessen, da diese irrelevant ist zur Zielfindung. So am Ziel
    angekommen stehen im Stack nur noch alle Kreuzungen und Richtungen
    direkt zum Ziel, der kürzeste Weg ist somit gefunden. Falls auf dem Weg
    dorthin die Linie verloren wird, kann der Bot manuell wieder richtig auf
    die Linie gesetzt werden und mit Taste 6 (Continue) das Verhalten an
    dieser Stelle weitergeführt werden.
    Am Ziel kann der Bot den nun feststehenden kürzesten Weg wieder zum
    Ausgangspunkt zurückfahren via Taste 9 oder man stellt den Bot manuell
    wieder an den Ausgangspunkt und mit Taste 8 geht?s direkt wieder zum
    Ziel, d.h. an den Kreuzungen wird in die richtige Richtung direkt zum
    Ziel abgebogen.
    Ich habe Grün im Programm festgelegt, weil diese Farbe gut im Simu
    darstellbar ist, kann aber auf die eigene gewünschte Farbe im Prog
    festgelegt werden. Als Ziel gilt diese Farbe und dahinter Wand.
    Als Erweiterungen könnte man später auch die die T-Kreuzungen erkennen,
    würde aber hier erst einmal höheren Aufwand bedeuten. Auch könnte man
    den kürzesten Weg, der sich ja bei Zielankunft im Stack befindet, einem
    2. Bot senden via bot2bot-Kommunikation (wenn dies mal so möglich ist),
    der sich am Startpunkt des 1. Bots befindet und nun den Weg des 1. Bots
    abfährt. Nach Abfahren des Stacks (vorwärts oder rückwärts) ist hier der
    Stack wieder leer, so könnte man auch verlustlos den abfahren um diesen
    zu erhalten bzw. die Kreuzungen einfach erneut pushen.
    Erst einmal so weit. Ich hoffe, in Kürze eine Doku nachreichen zu
    können.

    Gruß, Frank

    Index: C:/botneu/ct-Bot/bot-logic/behaviour_drive_line_shortest_way.c
    ===================================================================
    --- C:/botneu/ct-Bot/bot-logic/behaviour_drive_line_shortest_way.c	(revision 0)
    +++ C:/botneu/ct-Bot/bot-logic/behaviour_drive_line_shortest_way.c	(revision 0)
    @@ -0,0 +1,560 @@
    +/*
    + * 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_drive_line_shortest_way.c
    + * @brief 	Linienverfolger, der an Kreuzungen eine bestimmte Vorzugsrichtung einschlaegt (links) und diesen Weg weiterverfolgt, bis das Ziel
    + *          (gruenes Feld an Wand) gefunden ist; Linien muessen immer an einem gruenen Feld ohne Hindernis enden, damit der botein Ende erkennt und
    + *          umdrehen kann
    + *          Die Kreuzungen und der eingeschlagene Weg werden auf dem Stack vermerkt, Wege die nicht zum Ziel fuehren vergessen; Am Ziel angekommen
    + *          steht im Stack der kuerzeste Weg; Es kann nun via Display auf dem kuezesten Weg zum Ausgangspunkt zurueckgefahren werden oder der Bot wieder
    + *          manuell an den Start gestellt werden und das Ziel auf kuerzestem Weg angefahren werden
    + * @author 	Frank Menzel (Menzelfr@xxxxxx)
    + * @date 	21.12.2008
    + */
    +
    +#include "bot-logic/bot-logik.h"
    +
    +#ifdef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +
    +#include "pos_store.h"
    +#include "rc5-codes.h"
    +
    +/*! Statusvariable des Verhaltens */
    +static int8 lineState=0;
    +
    +/*! Farbe des Umkehr- und Zielfeldes, via Default auf Gruen festgelegt */
    +#define GROUND_GOAL_DEF      GROUND_GOAL
    +
    +/*! Positionsspeicher, den das Verhalten benutzt zum Speichern der Weginfos */
    +static pos_store_t * pos_store = NULL;
    +/*! Statischer Speicher fuer pos_store */
    +static position_t pos_store_data[POS_STORE_SIZE];
    +
    +/*!
    + * Zeit zwischen zwei Korrekturen [ms]. Groessere Werte bewirken "ruhigeres" Fahren,
    + * erhoehen damit aber auch die Reaktionszeit (z.B. bei scharfen Kurven problematisch)
    + */
    +#define	CORRECTION_DELAY	150
    +
    +/*! Kennungen, welcher der Bordersensoren zugeschlagen hat zur Erkennung der Kreuzungen, notwendig
    +*   weil sicht nicht immer beide gleichzeitig ueber Kreuzungslinie befinden
    +*/
    +static uint8_t border_side_l_fired=0;
    +static uint8_t border_side_r_fired=0;
    +
    +/*! bevorzugte Richtung an Kreuzungen; 1 zuerst immer nach links, -1 rechts */
    +#define START_SIDEWISH 1
    +
    +/*! Variable, die im Verhalten die Richtung an Kreuzung bestimmt; zum Beginn auf bevorzugte Startrichtung gesetzt */
    +static int8_t sidewish=START_SIDEWISH;
    +
    +/*! Kennung ob bot sich von Kreuzung weg bewegt und einen Weg entdeckt (vorwaerts) oder sich auf dem Rueckweg befindet nach
    + *  Erkennen der gruenen Umkehrfarbe */
    +static uint8_t way_back=0;
    +
    +
    +/*! eingeschlagene Richtung an Kreuzung ausgehend von urspruenglichem Ankunftsweg */
    +static int16_t direction_counter=0;
    +
    +/*! Kreuzungstyp, hier nur die X-Kreuzungen erlaubt (4 Seiten); mitgefuehrt fuer moegliche Erweiterungen */
    +static int16_t crosstype=0;
    +
    +/*! Zusatnd des Verhaltens, ob Linienfolger Weg erst suchen soll oder den bereits im Stack befindlichen kuerzesten Weg abfahren soll;
    + *  zum Abfahren des kuerzesten Weges aus Stack ist Wert True sonst False
    + */
    +static uint8_t go_stack_way=0;
    +
    +
    +/*! Zustaende des Verhaltens */
    +#define GO_TO_LINE	              0
    +#define CHECK_LINE	              1
    +#define GO_FORWARD                2
    +#define TURN_SIDEWISH_ON_CROSSING 3
    +#define GO_FORWARD_AFTER_TURN     4
    +#define CHECK_BORDER              5
    +#define GO_FORWARD_AFTER_GREEN    6
    +
    +
    +/*!
    + * Push der Kreuzungsinformationen, etwas tricky dazu der eigentliche Positionsspeicher benutzt
    + * @param crosstype	Typ der Kreuzung (jetzt nur 1 Typ, gedacht fuer spaetere Erweiterungen und weil Postyp sowieso 2 Params hat)
    + * @param direction	eingeschlagener Weg an der Kreuzung
    + */
    +static void push_stack_crossing(int16_t crosstype, int16_t direction) {
    +   position_t p_temp;  // Stack erlaubt nur Speicherung von Positionstypen
    +   p_temp.x=crosstype;
    +   p_temp.y=direction;
    +
    +	// der eigentliche Push der Kreuzungsinformation
    +	pos_store_push(pos_store, p_temp);
    +
    +}
    +
    +/*!
    + * Holt eine Kreuzungsinformation vom Stack (Bot auf Rueckweg an Kreuzung oder Stackabfahren vom Ziel zum Start)
    + * @param *crosstype Kreuzungstyp
    + * @param *direction zuletzt eingeschlagene Richtung
    + * @return True wenn erfolgreich, False falls Stack leer ist
    + */
    +static uint8_t pop_stack_crossing(int16_t *crosstype, int16_t *direction) {
    +    position_t p_temp;  // Stack erlaubt nur Speicherung von Positionstypen
    +	if (!pos_store_pop(pos_store, &p_temp))
    +		return False;
    +    *crosstype = p_temp.x;
    +    *direction = p_temp.y;
    +
    +	return True;
    +}
    +
    +/*!
    + * Holt eine Kreuzung von vorn aus dem Stack (in dem Fall Queue) zum Abfahren von Start- zum Zielpunkt
    + * @param *crosstype Kreuzungstyp
    + * @param *direction zuletzt eingeschlagene Richtung
    + * @return True wenn erfolgreich, False falls Stack leer ist
    + */
    +static uint8_t dequeue_stack_crossing(int16_t *crosstype, int16_t *direction) {
    +    position_t p_temp;  // Stack erlaubt nur Speicherung von Positionstypen
    +	if (!pos_store_dequeue(pos_store, &p_temp))
    +		return False;
    +    *crosstype = p_temp.x;
    +    *direction = p_temp.y;
    +
    +	return True;
    +}
    +
    +/*! Prueft ob sich der Bot auf dem definierten Umkehr- Zielfeld befindet (via Default gruen; gut im Simulator verwendbar)
    + *  @return True falls Bot sich auf dem Farbfeld befindet sonst False
    + */
    +uint8_t green_field(void) {
    +  if ((sensLineL>GROUND_GOAL_DEF-10 && sensLineL<GROUND_GOAL_DEF+10) ||
    +			    (sensLineR>GROUND_GOAL_DEF-10 && sensLineR<GROUND_GOAL_DEF+10))
    +     return True;
    +
    +  return False;
    +}
    +
    +/*!  Prueft ob der Bot sich auf dem Ziel befindet, also wenn er auf dem definierten Farbfeld steht und Hindernis dahinter
    + *   @return True falls bot sich auf dem Zielfarbfeld befindet sonst False
    + */
    +uint8_t goal_reached(void) {
    +	if (green_field() && sensDistL<300 && sensDistR<300)
    +		return True;
    +
    +	return False;
    +
    +}
    +
    +/* Check-Routine zum Erkennen ob sich bot schon auf der Linie befindet
    + * @return True wenn mindestens ein Liniensensor die Linie erkennt
    + */
    +static uint8_t check_line_sensors(void) {
    +  if (sensLineL >= LINE_SENSE || sensLineR >= LINE_SENSE)
    +  	return True;
    +
    +  return False;
    +}
    +
    +
    +/*!
    + * Das eigentliche Verhalten, welches den bot einer Linie folgen laesst, X-Kreuzungen erkennt und
    + * dann in bestimmter Reihenfolge die Abzweigungen entlangfaehrt bis zu seinem Ziel (gruenes Feld an Hindernis); die
    + * Kreuzungen werden enweder neu im Stack angelegt oder von dort geholt und die Wegeinfos dort wieder vermerkt; eine Kreuzung
    + * wird vergessen, wenn kein Weg von dort zum Ziel gefuehrt hatte; Verhalten laesst den bot ebenefalls den bereits gemerkten Weg
    + * zum Ziel oder von dort rueckwaerts direkt auf kuerzestem Weg zum Ausgangspunkt fahren
    + * @param *data	Verhaltensdatensatz
    + */
    +void bot_drive_line_shortest_way_behaviour(Behaviour_t * data) {
    +	static int16_t lastLeft = 0;
    +	static int16_t lastRight= 0;
    +	static uint8_t lastCorrection = 0;
    +	static uint32_t lastCorrectionTime = 0;
    +
    +	uint8_t correction = 0;
    +
    +
    +	switch (lineState) {
    +		case GO_TO_LINE:  // bot faehrt gewisse Strecke vorwaerts bis zur Linie
    +
    +        // naechster Verhaltenszustand
    +		lineState=CHECK_LINE;
    +
    +		// falls vom Ziel rueckwaerts gefahren werden soll und bot ist noch auf Zielendposition, dann erst einmal drehen
    +		if (goal_reached()&& way_back) {
    +			(bot_turn(data,180));
    +			break; // im selben Verhaltensstatus weiter mit Liniensuche Strecke voraus fahren
    +		}
    +		else {
    +		  bot_goto_dist(data, 600, 1);                                            // maximal 60cm vorwaerts bis Linie voraus
    +		  bot_cancel_behaviour(data, bot_goto_pos_behaviour,check_line_sensors);  // Ende der Vorwaertsfahrt wenn Linie erkannt wurde
    +		}
    +		break;
    +
    +	case CHECK_LINE:  // laesst hier den bot eine Linie folgen
    +
    +     if (goal_reached()) {
    +		LOG_DEBUG("Ziel erreicht und Ende");
    +		lineState=99; // Verhalten Ende
    +		break;
    +	 }
    +  	 if (green_field()) {
    +  	 	way_back=True;
    +		LOG_DEBUG("auf Gruen Umkehr");
    +		bot_turn(data,180);               // 180 Grad Drehung vollfuehren
    +		lineState=GO_FORWARD_AFTER_GREEN; // weiter mit Eintritt nach Gruenerkennung
    +		break;
    +	 }
    +
    +	if (sensLineL >= LINE_SENSE) {  // Linker Sensor auf Linie, rechter somit auch
    +		/* falls linker Sensor oder der rechte die Linie erkennen wird vorwaerts gefahren */
    +		speedWishLeft  = BOT_SPEED_SLOW;
    +		speedWishRight = BOT_SPEED_SLOW;
    +		correction=0;
    +
    +		if (sensBorderL > BORDER_DANGEROUS || sensBorderR > BORDER_DANGEROUS) {
    +
    +			// Kennungen setzen auf Querlinie erkannt links oder rechts voraus, also wenn Abgrundsensor Linie (vorausgesetzt Abgrund gibt es nicht) erkennt
    +			if (sensBorderR > BORDER_DANGEROUS)
    +			  border_side_r_fired=True;
    +			else
    +			  border_side_l_fired=True;
    +
    +			// Beide erkennen Querlinie
    +			if (sensBorderL > BORDER_DANGEROUS && sensBorderR > BORDER_DANGEROUS) {
    +			 	 border_side_l_fired=True;
    +			 	 border_side_r_fired=True;
    +			 }
    +
    +			if (border_side_l_fired && border_side_r_fired ) {
    +		      LOG_DEBUG("beide zugeschlagen und vor!!!");
    +		      lineState=CHECK_BORDER;
    +		       BLOCK_BEHAVIOUR(data, 500); // etwas warten
    +			  break;
    +	        }
    +
    +		}
    +		else {
    +
    +		if (lastLeft<0 || lastRight<0) {
    +           // bei Wechsel zu geradeaus Kreuzungskennunggen ruecksetzen
    +		   border_side_l_fired=False;
    +		   border_side_r_fired=False;
    +		}
    +	}
    +
    +	} else if (sensLineL < LINE_SENSE && sensLineR < LINE_SENSE && !border_side_r_fired) {
    +		/* kein Sensor mehr auf Linie und auch nicht rechts Abzweigung gesehen dann links drehen*/
    +		speedWishLeft  = -BOT_SPEED_FOLLOW;
    +		speedWishRight = BOT_SPEED_FOLLOW;
    +		correction = 1;
    +	} else {
    +		/* sonst rechts drehen */
    +		speedWishLeft  = BOT_SPEED_FOLLOW;
    +		speedWishRight = -BOT_SPEED_FOLLOW;
    +		correction = 2;
    +	}
    +
    +	if (lastCorrection != correction && !timer_ms_passed(&lastCorrectionTime, CORRECTION_DELAY)) {
    +		/* Falls die letzte Korrektur gerade erst war, reagieren wir (noch) nicht */
    +		speedWishLeft  = lastLeft;
    +		speedWishRight = lastRight;
    +		break;
    +	}
    +
    +	/* neue Werte merken */
    +	lastCorrection = correction;
    +	lastLeft  = speedWishLeft;
    +	lastRight = speedWishRight;
    +
    +	break;
    +
    +	case CHECK_BORDER:  // kleines Stueck vorfahren fuer Abgrundcheck
    +	    lineState=GO_FORWARD;       // naechster Verhaltenszustand
    +	    bot_goto_dist(data, 20, 1); // 2cm vorfahren, Liniensensoren sind dann runter von Linie, falls nicht Abgrund
    +	    break;
    +
    +	case GO_FORWARD:  // Erkennung ob Abgrund und Ende, falls nicht weiter bis Liniensensoren auf Krezung
    +	    if (sensBorderL > BORDER_DANGEROUS || sensBorderR > BORDER_DANGEROUS) {
    +				LOG_DEBUG("Abgrund und Ende %1d %1d",sensBorderL ,sensBorderR);
    +				lineState=99;                 // Verhalten Ende
    +				break;
    +			}
    +	    lineState=TURN_SIDEWISH_ON_CROSSING;  // naechster Verhaltenszustand
    +	    bot_goto_dist(data, 20, 1);           //vorfahren bis Liniensensoren ideal auf Kreuzung stehen zur Drehung
    +	    break;
    +
    +	case TURN_SIDEWISH_ON_CROSSING:  // Festlegen und Ausfuehren der Drehung je nach Richtung und Fahrlogik (Stackfahren vor oder zurueck)
    +	    // beim Rueckweg way_back==True vom Stack hier die Kreuzung holen; bei Hinweg neu auf den
    +	    // Stack legen
    +
    +	    LOG_DEBUG("vor turn 90 l/r %1d %1d,way_back  %1d",sensLineL,sensLineR,way_back);
    +
    +	    lineState=GO_FORWARD_AFTER_TURN;  // naechster Verhaltenszustand
    +
    +        if (!go_stack_way) { // Logik Zielsuchen, kein Stackfahren
    +
    +	     if (way_back) {  // bot auf Rueckweg nach Wegende mit Gruenfeld
    +
    +	      if (!pop_stack_crossing(&crosstype,&direction_counter)) {
    +	      	LOG_DEBUG("kein Pop moeglich");
    +	      	lineState=99;                 // Verhalten Ende
    +			break;
    +	      }
    +	      direction_counter++;
    +
    +	      LOG_DEBUG("---X vom Stack nehmen wg. back %1d counter %1d",way_back,direction_counter);
    +	      if (direction_counter<=3){
    +	      	push_stack_crossing(1,direction_counter);
    +	      	way_back=False;  // wird wieder zu vorwaerts
    +	      	LOG_DEBUG("wieder push, back %1d counter %1d",way_back,direction_counter);
    +	      }
    +	      else
    +	      	LOG_DEBUG("X wird vergessen-irrelevant");
    +
    +	     }
    +	     else {  // Bot kommt auf Vorwaertsfahrt an Kreuzung an
    +
    +	       direction_counter=1;                        // ersten Weg nehmen
    +	       push_stack_crossing(1,direction_counter);   // Kreuzung und Weg auf Stack legen
    +	       LOG_DEBUG(">> X ist neu vorw. und Push, counter %1d",direction_counter);
    +	       way_back=False;                             // jedenfalls auf vorwaerts
    +	     }
    +        }
    +        else {  // hier soll der im Stack liegende kuerzeste Weg abgefahren werden
    +        	LOG_DEBUG("Stack abfahren und Richtung aus Stack");
    +
    +	           // Unterscheidung fuer Vorwaerts und Rueckwaerts,
    +	           // bei vorwaerts wurde Bot wieder an Ausgangspunkt gesetzt und faehrt zum Ziel
    +	           // bei rueckwaerts faehrt bot vom Ziel zurueck zum Anfang der Linie bis Stack leer ist an Ankunft 1.Kreuzung	ab Start
    +	           // gespeicherter Richtungswert muss je nach vorwaerts oder rueckwaerts anders interpretiert werden
    +	        	if (way_back) {
    +	        	 // Weg rueckwaerts heisst Weg vom Ziel zurueckzufahren, also via Pop von Kreuzung zu Kreuzung hangeln
    +	        	 if (!pop_stack_crossing(&crosstype,&direction_counter)) {
    +	      	       LOG_DEBUG("kein Pop moeglich");
    +	      	       lineState=99;
    +			       break;
    +	             }
    +
    +	        	// je nach Start-Ausgangs-Richtungswahl Richtungswert interpretieren
    +	        	if (START_SIDEWISH==1) {  //links bevorzugt
    +
    +	        	 if (direction_counter==1) {
    +	        		LOG_DEBUG("nach rechts");
    +	        		sidewish=-1;
    +	        	   }
    +	        	 if (direction_counter==3) {
    +	        		LOG_DEBUG("nach links");
    +	        		sidewish=1;
    +	        	  }
    +	        	}
    +	        	else {                    // rechts bevorzugt
    +	        		LOG_DEBUG("neg wg. rechts zuerst");
    +	        		if (direction_counter==1) {
    +	        		LOG_DEBUG("nach links");
    +	        		sidewish=1;
    +	        	   }
    +	        	 if (direction_counter==3) {
    +	        		LOG_DEBUG("nach rechts");
    +	        		sidewish=-1;
    +	        	  }
    +	        	}
    +
    +	        	}  // Ende zurueck vom Ziel
    +	        	else {
    +	        	  // bot soll hier den kuerzesten Weg vom Start- zum Zielpunkt abfahren, d.h. bot befindet sich wieder
    +	        	  // am Startpunkt (manuell hingesetzt/ gefahren) und muss sich den jeweils 1. Stackwert holen und abfahren
    +	        	  if (!dequeue_stack_crossing(&crosstype,&direction_counter)) {
    +	      	        LOG_DEBUG("kein dequeue moeglich");
    +	      	        lineState=99;
    +			        break;
    +	               }
    +
    +	            // auch hier je nach Wunsch-Start-Richtungswahl die Richtung interpretieren
    +	        	if (START_SIDEWISH==1) {
    +	        	  if (direction_counter==1) {  // links bevorzugt
    +	        		LOG_DEBUG("nach links");
    +	        		sidewish=1;
    +	        	  }
    +	        	 if (direction_counter==3) {
    +	        		LOG_DEBUG("nach rechts");
    +	        		sidewish=-1;
    +	        	  }
    +	        	}
    +	        	else {                         // rechts bevorzugt
    +	        		LOG_DEBUG("neg wegen rechts zuerst");
    +	        		 if (direction_counter==1) {
    +	        		LOG_DEBUG("nach rechts");
    +	        		sidewish=-1;
    +	        	  }
    +	        	 if (direction_counter==3) {
    +	        		LOG_DEBUG("nach links");
    +	        		sidewish=1;
    +	        	  }
    +	        	}
    +
    +	        	}
    +
    +	        	LOG_DEBUG("X Richtg. %1d",direction_counter);
    +
    +	        	if (direction_counter==2) {
    +	        		LOG_DEBUG("geradeaus");
    +	        		break;// ohne Drehung weiter
    +	        	}
    +        }
    +        // Drehung 90 Grad ausfuehren
    +        bot_turn(data,sidewish * 90);
    +	    break;
    +
    +	case GO_FORWARD_AFTER_TURN:    // hierher nach 90 Grad Drehung in gewuenschte Richtung
    +	    BLOCK_BEHAVIOUR(data, 500); // etwas warten
    +	    LOG_DEBUG("etwas vor nach Dreh l/r %1d %1d",sensLineL,sensLineR);
    +
    +	    // geht dann wieder mit Linienfolger weiter
    +	    lineState=CHECK_LINE;       // naechster Verhaltenszustand
    +
    +	    bot_goto_dist(data, 50, 1); //vorfahren, Linesensoren sind dann auf Kreuzung
    +	    border_side_l_fired=False;  // Kennungen ruecksetzen
    +        border_side_r_fired=False;
    +	    break;
    +
    +	case GO_FORWARD_AFTER_GREEN:    // hierher nach Erkennung des Gruenfeldes mit Richtungsumkehr
    +	    LOG_DEBUG("etwas vor after green l/r %1d %1d",sensLineL,sensLineR);
    +	    lineState=CHECK_LINE;       // Linienfolger geht wieder von vorn los
    +	    BLOCK_BEHAVIOUR(data, 500); // evtl. etwas warten
    +	    break;
    +
    +	case 99:  // Verhaltensende
    +	    LOG_DEBUG("Ende Behav. l/r %1d %1d",sensLineL,sensLineR);
    +	    return_from_behaviour(data);
    +		break;
    +	}
    +
    +}
    +
    +/*!
    + * Startet das Verhalten
    + * @param	*caller Verhaltensdatensatz des Aufrufers
    + */
    +void bot_drive_line_shortest_way(Behaviour_t * caller) {
    +	switch_to_behaviour(caller, bot_drive_line_shortest_way_behaviour, NOOVERRIDE);
    +	//border_side_r=0;
    +	lineState=0;
    +	border_side_l_fired=0;
    +	border_side_r_fired=0;
    +	sidewish=START_SIDEWISH;
    +	way_back=False;
    +	go_stack_way=False;
    +	pos_store = pos_store_create(get_behaviour(bot_follow_line_behaviour), pos_store_data);
    +	direction_counter=0;
    +
    +	/* stoerende Notfallverhalten aus */
    +#ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
    +	deactivateBehaviour(bot_avoid_col_behaviour);
    +#endif
    +#ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE
    +	deactivateBehaviour(bot_avoid_border_behaviour);
    +#endif
    +}
    +/*  Falls Linienfolger Linie nicht findet kann hier weitergefuehrt werden nach manuellem richtigen wiederausrichten
    + */
    +void bot_drive_line_shortest_way_continue(Behaviour_t * caller) {
    +	switch_to_behaviour(caller, bot_drive_line_shortest_way_behaviour, NOOVERRIDE);
    +	lineState=0;
    +	border_side_l_fired=0;
    +	border_side_r_fired=0;
    +
    +}
    +
    +/*  Falls Linienfolger Linie nicht findet kann hier weitergefuehrt werden nach manuellem richtigen wiederausrichten
    + */
    +void bot_drive_line_shortest_way_forward(Behaviour_t * caller) {
    +	switch_to_behaviour(caller, bot_drive_line_shortest_way_behaviour, NOOVERRIDE);
    +	lineState=0;
    +	border_side_l_fired=0;
    +	border_side_r_fired=0;
    +	go_stack_way=True;
    +	way_back=False;
    +
    +}
    +void bot_drive_line_shortest_way_backward(Behaviour_t * caller) {
    +	switch_to_behaviour(caller, bot_drive_line_shortest_way_behaviour, NOOVERRIDE);
    +	lineState=0;
    +	border_side_l_fired=0;
    +	border_side_r_fired=0;
    +	go_stack_way=True;
    +	way_back=True;
    +
    +}
    +
    +
    +/*!
    + * Keyhandler zur Verwendung via Fernbedienung auf dem Display zum Stackanfahren
    + */
    +#ifdef DISPLAY_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +static void driveline_disp_key_handler(void) {
    +	switch (RC5_Code) {
    +
    +
    +		case RC5_CODE_5:
    +		/* Verhalten starten zum Anfahren der Stackpunkte */
    +		RC5_Code = 0;
    +		bot_drive_line_shortest_way(NULL);
    +		break;
    +
    +		case RC5_CODE_6:
    +		/* Verhalten zum Speichern relevanter Wegepopsitionen zum Spaeteren Zurueckfahren */
    +		RC5_Code = 0;
    +		bot_drive_line_shortest_way_continue(NULL);
    +		break;
    +
    +        case RC5_CODE_8:
    +		/* Zueckfahren vom Ziel zum Ausgangspunkt auf kuerzestem gespeicherten Weg */
    +		RC5_Code = 0;
    +		bot_drive_line_shortest_way_forward(NULL);
    +		break;
    +		case RC5_CODE_9:
    +		/* Zueckfahren vom Ziel zum Ausgangspunkt auf kuerzestem gespeicherten Weg */
    +		RC5_Code = 0;
    +		bot_drive_line_shortest_way_backward(NULL);
    +		break;
    +
    +
    +	} // switch
    +} // Ende Keyhandler
    +
    +
    +/*!
    + * Display zum Verhalten
    + */
    +void drive_line_shortest_way_display(void) {
    +	display_cursor(1, 1);
    +	display_printf("DRIVE_LINE_S_WAY");
    +	//display_cursor(2, 1);
    +	//display_printf("Save/Del      : 3/8");
    +	display_cursor(2, 1);
    +	display_printf("GoLine/Continue:5/6");
    +	display_cursor(4, 1);
    +	display_printf("GoWayForw/Back:8/9");
    +
    +	driveline_disp_key_handler(); // aufrufen des Key-Handlers
    +}
    +#endif	// DISPLAY_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +
    +
    +#endif	// BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    Index: C:/botneu/ct-Bot/bot-logic/bot-logik.c
    ===================================================================
    --- C:/botneu/ct-Bot/bot-logic/bot-logik.c	(revision 1528)
    +++ C:/botneu/ct-Bot/bot-logic/bot-logik.c	(working copy)
    @@ -284,6 +284,11 @@
     		// Verhalten um einer Linie zu folgen
     		insert_behaviour_to_list(&behaviour, new_behaviour(70, bot_follow_line_behaviour, INACTIVE));
     	#endif
    +
    +	#ifdef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +		// Linie folgen ueber Kreuzungen hinweg zum Ziel, kuerzester Weg befindet sich danach im Stack
    +		insert_behaviour_to_list(&behaviour, new_behaviour(69, bot_drive_line_shortest_way_behaviour, INACTIVE));
    +	#endif

     	#ifdef BEHAVIOUR_OLYMPIC_AVAILABLE
     		// unwichtigere Hilfsverhalten
    Index: C:/botneu/ct-Bot/ct-Bot.h
    ===================================================================
    --- C:/botneu/ct-Bot/ct-Bot.h	(revision 1528)
    +++ 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_STORE_AVAILABLE  /*!< Positionsspeicher vorhanden */
    +#define POS_STORE_AVAILABLE  /*!< Positionsspeicher vorhanden */

     //#define WELCOME_AVAILABLE	/*!< kleiner Willkommensgruss */

    Index: C:/botneu/ct-Bot/include/bot-logic/behaviour_drive_line_shortest_way.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-logic/behaviour_drive_line_shortest_way.h	(revision 0)
    +++ C:/botneu/ct-Bot/include/bot-logic/behaviour_drive_line_shortest_way.h	(revision 0)
    @@ -0,0 +1,65 @@
    +/*
    + * 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_drive_line_shortest_way.c
    + * @brief 	Linienverfolger, der an Kreuzungen eine bestimmte Vorzugsrichtung einschlaegt (links) und diesen Weg weiterverfolgt, bis das Ziel
    + *          (gruenes Feld an Wand) gefunden ist; Linien muessen immer an einem gruenen Feld ohne Hindernis enden, damit der botein Ende erkennt und
    + *          umdrehen kann
    + *          Die Kreuzungen und der eingeschlagene Weg werden auf dem Stack vermerkt, Wege die nicht zum Ziel fuehren vergessen; Am Ziel angekommen
    + *          steht im Stack der kuerzeste Weg; Es kann nun via Display auf dem kuezesten Weg zum Ausgangspunkt zurueckgefahren werden oder der Bot wieder
    + *          manuell an den Start gestellt werden und das Ziel auf kuerzestem Weg angefahren werden
    + * @author 	Frank Menzel (Menzelfr@xxxxxx)
    + * @date 	21.12.2008
    + */
    +
    +#include "bot-logic/bot-logik.h"
    +
    +#ifndef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_H_
    +#define BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_H_
    +
    +#ifdef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +
    +/*!
    + * Das eigentliche Verhalten, welches den bot einer Linie folgen laesst, X-Kreuzungen erkennt und
    + * dann in bestimmter Reihenfolge die Abzweigungen entlangfaehrt bis zu seinem Ziel (gruenes Feld an Hindernis); die
    + * Kreuzungen werden enweder neu im Stack angelegt oder von dort geholt und die Wegeinfos dort wieder vermerkt; eine Kreuzung
    + * wird vergessen, wenn kein Weg von dort zum Ziel gefuehrt hatte; Verhalten laesst den bot ebenefalls den bereits gemerkten Weg
    + * zum Ziel oder von dort rueckwaerts direkt auf kuerzestem Weg zum Ausgangspunkt fahren
    + * @param *data	Verhaltensdatensatz
    + */
    +void bot_drive_line_shortest_way_behaviour(Behaviour_t *data);
    +
    +/*!
    + * Startet das Verhalten
    + * @param	*caller Verhaltensdatensatz des Aufrufers
    + */
    +void bot_drive_line_shortest_way(Behaviour_t *caller);
    +
    +/*  Falls Linienfolger Linie nicht findet kann hier weitergefuehrt werden nach manuellem richtigen wiederausrichten
    + */
    +void bot_drive_line_shortest_way_continue(Behaviour_t * caller) ;
    +
    +/*!
    + * Display zum Verhalten
    + */
    +void drive_line_shortest_way_display(void);
    +
    +#endif	// BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +#endif	/*BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_H_*/
    Index: C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h
    ===================================================================
    --- C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h	(revision 1528)
    +++ C:/botneu/ct-Bot/include/bot-logic/available_behaviours.h	(working copy)
    @@ -57,6 +57,8 @@

     //#define BEHAVIOUR_DRIVE_AREA_AVAILABLE /*!< flaechendeckendes Fahren mit Map */

    +#define BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE /*!< Linienfolger ueber Kreuzungen zum Ziel */
    +
     /* Aufgrund einer ganzen reihe von Abhaengigkeiten sollte man beim Versuch Speicher
      * zu sparen, zuerst mal bei den Hauptverhalten ausmisten, sonst kommen die
      * Unterverhalten durch die Hintertuer wieder rein */
    @@ -75,7 +77,11 @@
     	#warning "Pfadplanung geht nur, wenn POS_STORE_AVAILABLE"
     	#undef BEHAVIOUR_PATHPLANING_AVAILABLE
     #endif
    +#ifdef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +	#warning "Drive_line_shortest_way geht nur, wenn POS_STORE_AVAILABLE"
    +	#undef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
     #endif
    +#endif

     #ifdef BEHAVIOUR_DRIVE_AREA_AVAILABLE
     	#define BEHAVIOUR_GOTO_POS_AVAILABLE
    @@ -95,6 +101,11 @@
     	#define BEHAVIOUR_GOTO_POS_AVAILABLE
     #endif

    +#ifdef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +	#define BEHAVIOUR_GOTO_POS_AVAILABLE
    +	#define BEHAVIOUR_CANCEL_BEHAVIOUR_AVAILABLE
    +#endif
    +
     #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
     	#define BEHAVIOUR_TURN_AVAILABLE
     	#define BEHAVIOUR_FACTOR_WISH_AVAILABLE
    @@ -242,5 +253,7 @@

     #include "bot-logic/behaviour_pathplaning.h"

    +#include "bot-logic/behaviour_drive_line_shortest_way.h"
    +
     #endif	// BEHAVIOUR_AVAILABLE
     #endif	/*AVAILABLE_BEHAVIOURS_H_*/
    Index: C:/botneu/ct-Bot/ui/gui.c
    ===================================================================
    --- C:/botneu/ct-Bot/ui/gui.c	(revision 1528)
    +++ C:/botneu/ct-Bot/ui/gui.c	(working copy)
    @@ -292,7 +292,10 @@
     	#ifdef BEHAVIOUR_PATHPLANING_AVAILABLE
     		register_screen(&pathplaning_display);
     	#endif
    +	#ifdef BEHAVIOUR_DRIVE_LINE_SHORTEST_WAY_AVAILABLE
    +		register_screen(&drive_line_shortest_way_display);
     	#endif
    +	#endif
     }

     #endif	// DISPLAY_AVAILABLE
    Index: C:/botneu/ct-Bot/ui/available_screens.h
    ===================================================================
    --- C:/botneu/ct-Bot/ui/available_screens.h	(revision 1528)
    +++ C:/botneu/ct-Bot/ui/available_screens.h	(working copy)
    @@ -46,6 +46,7 @@
     #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				/*!< Display zur Pfadplanung */
    +#define DISPLAY_DRIVE_LINE_SHORTEST_WAY_AVAILABLE	/*!< Steuerung des Verhaltens auf diesem Screen */

     #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 @@
    +#Mon Dec 22 13:40:19 CET 2008
    +eclipse.preferences.version=1
    +indexerId=org.eclipse.cdt.core.fastIndexer

- Antworten:
  - Re: [ct-bot] neues Verhalten bot_drive_line_shortest_way...
    - From: Timo Sandmann
  - Re: [ct-bot] neues Verhalten bot_drive_line_shortest_way...
    - From: Timo Sandmann

- Voriger (Datum): RE: [ct-bot] mmc_test mit Fehler..., Menzel, Frank IT-OO4
- Nächster (Datum): Re: [ct-bot] neues Verhalten bot_drive_line_shortest_way..., Timo Sandmann
- Voriger (Thread): Re: [ct-bot] mmc_test mit Fehler..., Timo Sandmann
- Nächster (Thread): Re: [ct-bot] neues Verhalten bot_drive_line_shortest_way..., Timo Sandmann
- Indizes:
  - Datum
  - Thread