rc2007-soccer/source/ct-Bot/bot-logic/bot-logik.c

/*
 * c't-Sim - 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 	bot-logik.c
 * @brief 	High-Level Routinen fuer die Steuerung des c't-Bots.
 * Diese Datei sollte der Einstiegspunkt fuer eigene Experimente sein, 
 * den Roboter zu steuern.
 * 
 * bot_behave() arbeitet eine Liste von Verhalten ab. 
 * Jedes Verhalten kann entweder absolute Werte setzen, dann kommen niedrigerpriorisierte nicht mehr dran.
 * Alternativ dazu kann es Modifikatoren aufstellen, die bei niedriger priorisierten angewendet werden.
 * bot_behave_init() baut diese Liste auf.
 * Jede Verhaltensfunktion bekommt einen Verhaltensdatensatz uebergeben, in den Sie ihre Daten eintraegt
 * 
 * @author 	Benjamin Benz (bbe@heise.de)
 * @author 	Christoph Grimmer (c.grimmer@futurio.de)
 * @date 	01.12.05
*/


#include "bot-logic/bot-logik.h"

#ifdef BEHAVIOUR_AVAILABLE

#include "display.h"
#include "rc5.h"



#include <stdlib.h>

int16 speedWishLeft;				/*!< Puffervariablen fuer die Verhaltensfunktionen absolut Geschwindigkeit links*/
int16 speedWishRight;				/*!< Puffervariablen fuer die Verhaltensfunktionen absolut Geschwindigkeit rechts*/

float faktorWishLeft;				/*!< Puffervariablen fuer die Verhaltensfunktionen Modifikationsfaktor links*/
float faktorWishRight;				/*!< Puffervariablen fuer die Verhaltensfunktionen Modifikationsfaktor rechts */

int16 target_speed_l=BOT_SPEED_STOP;	/*!< Sollgeschwindigkeit linker Motor - darum kuemmert sich bot_base()*/
int16 target_speed_r=BOT_SPEED_STOP;	/*!< Sollgeschwindigkeit rechter Motor - darum kuemmert sich bot_base() */


/*! Liste mit allen Verhalten */
Behaviour_t *behaviour = NULL;

/*! 
 * Das einfachste Grundverhalten 
 * @param *data der Verhaltensdatensatz
 */
void bot_base_behaviour(Behaviour_t *data){
	speedWishLeft=target_speed_l;
	speedWishRight=target_speed_r;
}

/*!
 * Initialisert das ganze Verhalten
 */
void bot_behave_init(void){
	#ifdef BEHAVIOUR_REMOTECALL_AVAILABLE
		// Dieses Verhalten kann andere Starten
		insert_behaviour_to_list(&behaviour, new_behaviour(254, bot_remotecall_behaviour,INACTIVE));
	#endif

	#ifdef BEHAVIOUR_SERVO_AVAILABLE
		insert_behaviour_to_list(&behaviour, new_behaviour(253, bot_servo_behaviour,INACTIVE));
	#endif

	// Demo-Verhalten, ganz einfach, inaktiv
	// Achtung, im Moment hat es eine hoehere Prioritaet als die Gefahrenerkenner!!!
	#ifdef BEHAVIOUR_SIMPLE_AVAILABLE
		insert_behaviour_to_list(&behaviour, new_behaviour(252, bot_simple_behaviour,INACTIVE));
		insert_behaviour_to_list(&behaviour, new_behaviour(251, bot_simple2_behaviour,INACTIVE));
	#endif


	// Hoechste Prioritate haben die Notfall Verhalten

	// Verhalten zum Schutz des Bots, hohe Prioritaet, Aktiv
	#ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE	
		insert_behaviour_to_list(&behaviour, new_behaviour(250, bot_avoid_border_behaviour,ACTIVE));
	#endif
	#ifdef BEHAVIOUR_AVOID_COL_AVAILABLE	
		insert_behaviour_to_list(&behaviour, new_behaviour(249, bot_avoid_col_behaviour,ACTIVE));
	#endif

	#ifdef BEHAVIOUR_SCAN_AVAILABLE
		// Verhalten, das die Umgebung des Bots on-the fly beim fahren scannt
		insert_behaviour_to_list(&behaviour, new_behaviour(155, bot_scan_onthefly_behaviour,ACTIVE));
	
		// Verhalten, das einmal die Umgebung des Bots scannt
		insert_behaviour_to_list(&behaviour, new_behaviour(152, bot_scan_behaviour,INACTIVE));
	#endif
	
	// Alle Hilfsroutinen sind relativ wichtig, da sie auch von den Notverhalten her genutzt werden
	// Hilfsverhalten, die Befehle von Boten-Funktionen ausfuehren, erst inaktiv, werden von Boten aktiviert	
	#ifdef BEHAVIOUR_TURN_AVAILABLE
		insert_behaviour_to_list(&behaviour, new_behaviour(150, bot_turn_behaviour,INACTIVE));
	#endif
	#ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE
		insert_behaviour_to_list(&behaviour, new_behaviour(149, bot_drive_distance_behaviour,INACTIVE));
	#endif
	#ifdef BEHAVIOUR_GOTO_AVAILABLE
		insert_behaviour_to_list(&behaviour, new_behaviour(148, bot_goto_behaviour,INACTIVE));
	#endif

	// Hilfsverhalten zum Anfahren von Positionen
	#ifdef BEHAVIOUR_GOTOXY_AVAILABLE
		insert_behaviour_to_list(&behaviour, new_behaviour(147, bot_gotoxy_behaviour,INACTIVE));
	#endif


	#ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE
		insert_behaviour_to_list(&behaviour, new_behaviour(44, bot_catch_pillar_behaviour,INACTIVE));
	#endif

	
	#ifdef BEHAVIOUR_OLYMPIC_AVAILABLE
		bot_olympic_init(52,100,INACTIVE);
	#endif

	#ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
		// Verhalten um einer Linie zu folgen
		insert_behaviour_to_list(&behaviour, new_behaviour(70, bot_follow_line_behaviour, ACTIVE));
	#endif

	#ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE
		bot_solve_maze_init(100,43,INACTIVE);
	#endif

	#ifdef BEHAVIOUR_DRIVE_SQUARE_AVAILABLE
		// Demo-Verhalten, etwas komplexer, inaktiv
		insert_behaviour_to_list(&behaviour, new_behaviour(51, bot_drive_square_behaviour,INACTIVE));
	#endif



	// Grundverhalten, setzt aeltere FB-Befehle um, aktiv
	insert_behaviour_to_list(&behaviour, new_behaviour(2, bot_base_behaviour, ACTIVE));

	// Um das Simple-Behaviour zu nutzen, die Kommentarzeichen vor der folgenden Zeile entfernen!!!
	// activateBehaviour(bot_simple_behaviour);
	// activateBehaviour(bot_simple2_behaviour);

	#ifdef PC
		#ifdef DISPLAY_AVAILABLE
			/* Anzeigen der geladenen Verhalten  */
				Behaviour_t	*ptr	= behaviour;
	
				display_cursor(5,1);
				display_printf("Verhaltensstack:\n");
				while(ptr != NULL)	{
					display_printf("Prioritaet: %d.\n", ptr->priority);
					ptr = ptr->next;
				}
		#endif
	#endif
}


/*!
 * Aktiviert eine Regel mit gegebener Funktion
 * @param function Die Funktion, die das Verhalten realisiert.
 */
void activateBehaviour(BehaviourFunc function){
	Behaviour_t *job;						// Zeiger auf ein Verhalten

	// Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben 
	for (job = behaviour; job; job = job->next) {
		if (job->work == function) {
			job->active = ACTIVE;
			break;
		}
	}
}


/*!
 * Deaktiviert eine Regel mit gegebener Funktion
 * @param function Die Funktion, die das Verhalten realisiert.
 */
void deactivateBehaviour(BehaviourFunc function){
	Behaviour_t *job;						// Zeiger auf ein Verhalten
		
	// Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben 
	for (job = behaviour; job; job = job->next) {
		if (job->work == function) {
			job->active = INACTIVE;
			break;
		}
	}
}

/*! 
 * Ruft ein anderes Verhalten auf und merkt sich den Ruecksprung 
 * return_from_behaviour() kehrt dann spaeter wieder zum aufrufenden Verhalten zurueck
 * @param from aufrufendes Verhalten
 * @param to aufgerufenes Verhalten
 * @param override Hier sind zwei Werte Moeglich:
 * 		1. OVERRIDE : Das Zielverhalten to wird aktiviert, auch wenn es noch aktiv ist. 
 * 					  Das Verhalten, das es zuletzt aufgerufen hat wird dadurch automatisch 
 * 					  wieder aktiv und muss selbst sein eigenes Feld subResult auswerten, um zu pruefen, ob das
 * 					  gewuenschte Ziel erreicht wurde, oder vorher ein Abbruch stattgefunden hat. 
 * 		2. NOOVERRIDE : Das Zielverhalten wird nur aktiviert, wenn es gerade nichts zu tun hat.
 * 						In diesem Fall kann der Aufrufer aus seinem eigenen subResult auslesen,
 * 						ob seibem Wunsch Folge geleistet wurde.
 */ 
void switch_to_behaviour(Behaviour_t * from, void *to, uint8 override ){
	Behaviour_t *job;						// Zeiger auf ein Verhalten
	
	// Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben 
	for (job = behaviour; job; job = job->next) {
		if (job->work == to) {
			break;		// Abbruch der Schleife, job zeigt nun auf die Datenstruktur des Zielverhaltens
		}
	}	

	if (job->caller){		// Ist das auzurufende Verhalten noch beschaeftigt?
		if (override==NOOVERRIDE){	// nicht ueberschreiben, sofortige Rueckkehr
			if (from)
				from->subResult=SUBFAIL;
			return;
		}
		// Wir wollen also ueberschreiben, aber nett zum alten Aufrufer sein und ihn darueber benachrichtigen
		job->caller->active=ACTIVE;	// alten Aufrufer reaktivieren
		job->caller->subResult=SUBFAIL;	// er bekam aber nicht das gewuenschte Resultat
	}

	if (from) {
		// laufendes Verhalten abschalten
		from->active=INACTIVE;
		from->subResult=SUBRUNNING;
	}
		
	// neues Verhalten aktivieren
	job->active=ACTIVE;
	// Aufrufer sichern
	job->caller =  from;
}

/*! 
 * Kehrt zum aufrufenden Verhalten zurueck
 * @param running laufendes Verhalten
 */ 
void return_from_behaviour(Behaviour_t * data){
	data->active=INACTIVE; 				// Unterverhalten deaktivieren
	if (data->caller){			
		data->caller->active=ACTIVE; 	// aufrufendes Verhalten aktivieren
		data->caller->subResult=SUBSUCCESS;	// Unterverhalten war erfolgreich
	}
	data->caller=NULL;				// Job erledigt, Verweis loeschen
}

/*!
 * Deaktiviert alle Verhalten bis auf Grundverhalten. Bei Verhaltensauswahl werden die Aktivitaeten vorher
 * in die Verhaltens-Auswahlvariable gesichert.
 */
void deactivateAllBehaviours(void){
	Behaviour_t *job;						// Zeiger auf ein Verhalten
	
	#ifdef DISPLAY_BEHAVIOUR_AVAILABLE
	  // bei Verhaltensanzeige in Aktivitaets-Auswahl-Variable sichern
	  #ifndef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE
	     // bei Verhaltensanzeige in Aktivitaets-Auswahl-Variable sichern
	     // nicht bei dynamischer Anzeige und Selektion
        set_behaviours_equal();
      #endif
	#endif
		
	// Einmal durch die Liste gehen und (fast) alle deaktivieren, Grundverhalten nicht 
	for (job = behaviour; job; job = job->next) {
		if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) {
            // Verhalten deaktivieren 
			job->active = INACTIVE;	
		}
	}	
}

/*! 
 * Zentrale Verhaltens-Routine, wird regelmaessig aufgerufen. 
 * Dies ist der richtige Platz fuer eigene Routinen, um den Bot zu steuern.
 */
void bot_behave(void){	
	Behaviour_t *job;						// Zeiger auf ein Verhalten
	
	float faktorLeft = 1.0;					// Puffer fuer Modifkatoren
	float faktorRight = 1.0;				// Puffer fuer Modifkatoren
	
	#ifdef RC5_AVAILABLE
		rc5_control();						// Abfrage der IR-Fernbedienung
	#endif

	/* Solange noch Verhalten in der Liste sind...
	   (Achtung: Wir werten die Jobs sortiert nach Prioritaet aus. Wichtige zuerst einsortieren!!!) */
	for (job = behaviour; job; job = job->next) {
		if (job->active) {
			/* WunschVariablen initialisieren */
			speedWishLeft = BOT_SPEED_IGNORE;
			speedWishRight = BOT_SPEED_IGNORE;
			
			faktorWishLeft = 1.0;
			faktorWishRight = 1.0;
			
			job->work(job);	/* Verhalten ausfuehren */
			/* Modifikatoren sammeln  */
			faktorLeft  *= faktorWishLeft;
			faktorRight *= faktorWishRight;
           /* Geschwindigkeit aendern? */
			if ((speedWishLeft != BOT_SPEED_IGNORE) || (speedWishRight != BOT_SPEED_IGNORE)){
				if (speedWishLeft != BOT_SPEED_IGNORE)
					speedWishLeft *= faktorLeft;
				if (speedWishRight != BOT_SPEED_IGNORE)
					speedWishRight *= faktorRight;
					
				motor_set(speedWishLeft, speedWishRight);
				break;						/* Wenn ein Verhalten Werte direkt setzen will, nicht weitermachen */
			}
			
		}
		/* Dieser Punkt wird nur erreicht, wenn keine Regel im System die Motoren beeinflusen will */
		if (job->next == NULL) {
				motor_set(BOT_SPEED_IGNORE, BOT_SPEED_IGNORE);
		}
	}
}

/*! 
 * Erzeugt ein neues Verhalten 
 * @param priority Die Prioritaet
 * @param *work Den Namen der Funktion, die sich drum kuemmert
 */
Behaviour_t *new_behaviour(uint8 priority, void (*work) (struct _Behaviour_t *data), int8 active){
	Behaviour_t *newbehaviour = (Behaviour_t *) malloc(sizeof(Behaviour_t)); 
	
	if (newbehaviour == NULL) 
		return NULL;
	
	newbehaviour->priority = priority;
	newbehaviour->active=active;
	newbehaviour->next= NULL;
	newbehaviour->work=work;
	newbehaviour->caller=NULL;
	newbehaviour->subResult=SUBSUCCESS;
	return newbehaviour;
}

/*!
 * Fuegt ein Verhalten der Verhaltenliste anhand der Prioritaet ein.
 * @param list Die Speicherstelle an der die globale Verhaltensliste anfaengt
 * @param behave Einzufuegendes Verhalten
 */
void insert_behaviour_to_list(Behaviour_t **list, Behaviour_t *behave){
	Behaviour_t	*ptr	= *list;
	Behaviour_t *temp	= NULL;
	
	/* Kein Eintrag dabei? */
	if (behave == NULL)
		return;
	
	/* Erster Eintrag in der Liste? */
	if (ptr == NULL){
		ptr = behave;
		*list = ptr;
	} else {
		/* Gleich mit erstem Eintrag tauschen? */
		if (ptr->priority < behave->priority) {
			behave->next = ptr;
			ptr = behave;
			*list = ptr;
		} else {
			/* Mit dem naechsten Eintrag vergleichen */
			while(NULL != ptr->next) {
				if (ptr->next->priority < behave->priority)	
					break;
				
				/* Naechster Eintrag */
				ptr = ptr->next;
			}
			
			temp = ptr->next;
			ptr->next = behave;
			behave->next = temp;
		}
	}
}



#ifdef DISPLAY_BEHAVIOUR_AVAILABLE

/*!
 * ermittelt ob noch eine weitere Verhaltensseite existiert 	
 */
 int8  another_behaviour_page(void) {
  int16 max_behaviours ;
  Behaviour_t	*ptr	;
  
  /* dazu muss ich auch gueltige Screenseite sein */
  #ifdef DISPLAY_SCREENS_AVAILABLE
   if (display_screen != 2)
     return 0;
  #endif 
  
  ptr = behaviour;
  max_behaviours = 0;
  
// zuerst alle Verhalten ermitteln ausser Grundverhalten
  while(ptr != NULL)	{			 
	if  ((ptr->priority >= PRIO_VISIBLE_MIN) &&(ptr->priority <= PRIO_VISIBLE_MAX)) 
    			max_behaviours++;		  
						  
	ptr = ptr->next;
   }  

   return (behaviour_page  * 6) < max_behaviours;
}


/*! 
 * toggled ein Verhalten der Verhaltensliste an Position pos,
 * die Aenderung erfolgt nur auf die Puffervariable  
 * @param pos Listenposition, entspricht der Taste 1-6 der gewaehlten Verhaltensseite
 */
void toggleNewBehaviourPos(int8 pos){
	Behaviour_t *job;						// Zeiger auf ein Verhalten
    int8 i;
    
    // nur aendern, wenn ich richtige Screenseite bin 
     if (display_screen != 2)
       return ;
     
    // richtigen Index je nach Seite ermitteln 
    pos = (behaviour_page - 1) * 6 + pos;
    i   = 0;

	// durch die Liste gehen, bis wir den gewuenschten Index erreicht haben 
	for (job = behaviour; job; job = job->next) {
	    if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) {		
		  i++;
		  if (i == pos) {
		  	  // bei dynamischer Wahl wird direkt die Zustandsvariable geaendert
		  	  #ifdef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE
		  	    job->active = !job->active;
		  	  #else
		  	     job->active_new = !job->active_new;
		  	  #endif
			  			      
			  break;
		  }
	    }
	}
}

#ifndef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE
/*! 
 * Startschuss, die gewaehlten neuen Verhaltensaktivitaeten werden in die
 * Verhaltensliste geschrieben und die Verhalten damit scharf geschaltet 
 */
void set_behaviours_active_to_new(void) {

 Behaviour_t *job;	
   for (job = behaviour; job; job = job->next) 	{
					 
	if  ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) 
            job->active = job->active_new;            				 
	 
   }

}

/*!
 * Die Aktivitaeten der Verhalten werden in die Puffervariable geschrieben, 
 * welche zur Anzeige und Auswahl verwendet wird
 */
void set_behaviours_equal(void) {
 Behaviour_t *job;	
   for (job = behaviour; job; job = job->next) 	{
					 
	if  ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) 
            job->active_new = job->active;            				 
	 
   }
}
#endif
#endif
#endif